1.建立点类
1 #ifndef _POINT_H_
2 #define _POINT_H_
3
4 class Point{
5 public:
6 double x;
7 double y;
8 public:
9 Point(double a=0,double b=0);
10 void updatePoint(double a,double b);
11 };
12
13 #endif
Point.h
1 #include"Point.h"
2 #include<iostream>
3
4 Point::Point(double a,double b){
5 this->x=a;
6 this->y=b;
7 }
8
9 void Point::updatePoint(double a,double b){
10 this->x=a;
11 this->y=b;
12 }
Point.c
2.建立坐标系类
1 #ifndef _FRAME_H_
2 #define _FRAME_H_
3
4 #include<Eigen/Dense>
5 #include<cmath>
6 #include"Point.h"
7 using namespace Eigen;
8
9 class Frame{
10 protected:
11 Point OrigialPoint,AimPoint;
12 double OrigialAngal;
13 public:
14 Frame(){
15 OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
16 }
17 Frame(Point OrigialPoint0,double OrigialAngal0=0){
18 this->OrigialPoint=OrigialPoint0;
19 this->OrigialAngal=OrigialAngal0;
20 }
21 void UpdateMatrix(Point Zero){
22 AimPoint=Zero;
23 }
24 Point To_WF(Point twpoint);
25 };
26
27
28 class WorldFrame:public Frame{
29 public:
30 WorldFrame(){
31 OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
32 }
33 WorldFrame(Point OrigialPoint0,double OrigialAngal0=0);
34 ~WorldFrame(){
35 }
36
37 };
38
39
40
41 class JointFrame:public Frame{
42 public:
43 JointFrame(){
44 OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
45 }
46 JointFrame(Point OrigialPoint0,double OrigialAngal0=0);
47 ~JointFrame(){
48 }
49
50
51 };
52
53
54
55 class TaskFrame:public Frame{
56 public:
57 TaskFrame(){
58 OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
59 }
60 TaskFrame(Point OrigialPoint0,double OrigialAngal0=0);
61 ~TaskFrame(){
62 }
63
64
65 };
66
67
68 #endif
Frame.h
1 #include"Frame.h"
2
3 Point Frame::To_WF(Point twpoint){
4 Matrix2d RotateMatrix;
5 double OrigialAngal0=OrigialAngal/180*M_PI;
6 Point wfpoint;
7 RotateMatrix<< cos(OrigialAngal0),sin(OrigialAngal0),
8 -sin(OrigialAngal0),cos(OrigialAngal0);
9 Vector2d v1(twpoint.x,twpoint.y),v2(OrigialPoint.x,OrigialPoint.y),v3;
10 v3=RotateMatrix*v1+v2;
11 wfpoint.updatePoint(v3(0),v3(1));
12 return wfpoint;
13 }
14
15 WorldFrame::WorldFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
16 this->OrigialPoint=OrigialPoint0;
17 this->OrigialAngal=OrigialAngal0;
18 }
19
20 JointFrame::JointFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
21 this->OrigialPoint=OrigialPoint0;
22 this->OrigialAngal=OrigialAngal0;
23 }
24
25 TaskFrame::TaskFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
26 this->OrigialPoint=OrigialPoint0;
27 this->OrigialAngal=OrigialAngal0;
28 }
Frame.c
3.Solver解释器
1 #ifndef _SOLVER_H_
2 #define _SOLVER_H_
3 #include<math.h>
4 #include"Point.h"
5 #include"Frame.h"
6
7 class Solver{
8 private:
9 Point SolverResult;
10 public:
11 Solver();
12 Point PositiveKinematics(double L1,double L2,Point Angles);
13 Point NegativeKinematics(double L1,double L2,Point point);
14 Point FrameToWF(WorldFrame frame,Point AimPoint);
15 Point FrameToWF(JointFrame frame,Point AimPoint);
16 Point FrameToWF(TaskFrame frame,Point AimPoint);
17
18 };
19 #endif
Solver.h
1 #include"Solver.h"
2 #include"Point.h"
3 #include<cmath>
4
5 Solver::Solver(){
6 SolverResult.x=0;
7 SolverResult.y=0;
8 }
9
10 Point Solver::NegativeKinematics(double L1,double L2,Point point){
11 SolverResult.x=0;
12 SolverResult.y=0;
13 SolverResult.updatePoint(SolverResult.x,SolverResult.y);
14 return SolverResult;
15 }
16
17 Point Solver::PositiveKinematics(double L1,double L2,Point Angles){
18 SolverResult.x=L1 *cos(Angles.x/180*M_PI)+L2 *cos(Angles.y/180*M_PI);
19 SolverResult.y=L1 *sin(Angles.x/180*M_PI)+L2 *sin(Angles.y/180*M_PI);
20 SolverResult.updatePoint(SolverResult.x,SolverResult.y);
21 return SolverResult;
22 }
23
24
25
26 Point Solver::FrameToWF(WorldFrame frame,Point AimPoint){
27 return frame.To_WF(AimPoint);}
28 Point Solver::FrameToWF(JointFrame frame,Point AimPoint){
29 return frame.To_WF(AimPoint);}
30 Point Solver::FrameToWF(TaskFrame frame,Point AimPoint){
31 return frame.To_WF(AimPoint);}
Solver.c
4，机器人类
1 #ifndef _ROBOT_H_
2 #define _ROBOT_H_
3 #include"Solver.h"
4 //#include"Frame.h"
5 #include <vector>
6
7 class Robot{
8 private:
9 double JointAngle1,JointAngle2,L1,L2;
10 Point InsightPoint;
11 WorldFrame WF0;
12 JointFrame JF0;
13 TaskFrame TF0;
14 Solver solver;
15 std::vector<TaskFrame> TaskFrameVec;
16 public:
17 Robot(double length1,double length2){
18 L1=length1;L2=length2;
19 }
20 void PTPmove(WorldFrame f,Point p);
21 void PTPmove(JointFrame f,Point p);
22 void PTPmove(TaskFrame f,Point p);
23 void addTFrame(TaskFrame frame);
24 void deleteFrame();
25 };
26 #endif
Robot.h
1 #include<iostream>
2 #include"Robot.h"
3 using namespace std;
4
5 void Robot::PTPmove( WorldFrame f,Point p){
6 Point WorldframePoint=solver.FrameToWF(f,p);
7 Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
8 Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
9 Point WorldframePoint1(0,30);
10 Point AngleMatrix1=solver.NegativeKinematics(L1,L2,WorldframePoint1);
11 InsightPoint.updatePoint(PKResult.x,PKResult.y);
12 cout<<"机械手两关节转角"<<"<"<<AngleMatrix1.x<<","<<AngleMatrix1.y<<">"<<endl;
13 cout<<"点的位置为"<<"<"<<InsightPoint.x<<"," <<InsightPoint.y<<">"<<endl;
14
15 }
16
17 void Robot::PTPmove(JointFrame f,Point p){
18 Point WorldframePoint=solver.FrameToWF(f,p);
19 Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
20 Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
21 InsightPoint.updatePoint(PKResult.x,PKResult.y);
22 cout<<"机械手两关节转角"<<"<"<<AngleMatrix.x<<","<<AngleMatrix.y<<">"<<endl;
23 cout<<"点的位置为"<<"<"<<InsightPoint.x<<"," <<InsightPoint.y<<">"<<endl;
24 }
25
26 void Robot::PTPmove(TaskFrame f,Point p){
27 Point WorldframePoint=solver.FrameToWF(f,p);
28 Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
29 Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
30 InsightPoint.updatePoint(PKResult.x,PKResult.y);
31 cout<<"机械手两关节转角"<<"<"<<AngleMatrix.x<<","<<AngleMatrix.y<<">"<<endl;
32 cout<<"点的位置为"<<"<"<<PKResult.x<<"," <<PKResult.y<<">"<<endl;
33
34 }
35
36 void Robot::addTFrame(TaskFrame frame){
37 TaskFrameVec.push_back(frame);
38 }
39
40 void Robot::deleteFrame(){
41 TaskFrameVec.pop_back();
42 }
43
44
45
Robot.c
5 主函数
1 #include <iostream>
2 #include "Robot.h"
3
4 int main(){
5 Point one(5,20),two(0,30),three(0,20),a(0,0),c(10,0),d(5,0),e(10,0);
6 Robot MyRobot(10,20);
7 WorldFrame wframe(a,0);
8 JointFrame jframe1(a,0);
9 JointFrame jframe2(c,0);
10 TaskFrame tframe1(d,0);
11 TaskFrame tframe2(e,90);
12 MyRobot.PTPmove(tframe1,one);
13 MyRobot.PTPmove(wframe,two);
14 MyRobot.PTPmove(jframe1,three);
15 }
main.cpp
6 结果显示
小结：
1 坐标系类还有可以简化的余地；
2 运动学反解没有算出公式，显示的角度值都是0度；
3 机器人的转动角度范围没有确定；
4.建立坐标系类时用了继承，但感觉用的不好，或许没必要用；
时间： 2024-10-28 18:53:20