1 #include <iostream>
2 #include<Eigen/Dense>
3 #include<cmath>
4 #define PI 3.1415926
5 using namespace std;
6 using namespace Eigen;
7 class Point{
8 public:
9 double x;
10 double y;
11
12 Point(double a=0,double b=0);
13 void SetP(double a,double b);
14
15 };
16 Point::Point(double a,double b){
17 x=a;
18 y=b;
19 }
20 void Point::SetP(double a,double b){
21 x=a;
22 y=b;
23 }
24
25 class WorldFrame{
26
27 };
28 class TaskFrame{
29 public:
30 double x;
31 double y;
32 double ang;
33
34 TaskFrame(double a=0,double b=0,double d=0);
35 void WtoT(Point &p);
36 void TtoW(Point &p);
37 void TtoJ(Point &p,double a1,double a2);
38 };
39
40 TaskFrame::TaskFrame(double a,double b,double A){
41 x=a;
42 y=b;
43 ang=A;
44 }
45
46 void TaskFrame::WtoT(Point &p){
47 MatrixXd m(3,3);
48 MatrixXd pt(1,3);
49 double deg;
50 deg=ang*PI/180;
51 m(0,0)=cos(deg);
52 m(0,1)=sin(deg);
53 m(0,2)=0;
54 m(1,0)=-sin(deg);
55 m(1,1)=cos(deg);
56 m(1,1)=0;
57 m(2,0)=x,
58 m(2,1)=y;
59 m(2,2)=0;
60 pt(0,0)=p.x;
61 pt(0,0)=p.y;
62 pt(0,2)=1;
63 pt*=m;
64 p.x=pt(0,0);
65 p.y=pt(0,1);
66 }
67 void TaskFrame::TtoW(Point &p){
68 MatrixXd m(3,3);
69 MatrixXd pt(1,3);
70 double deg;
71 deg=ang*PI/180;
72 m(0,0)=cos(deg);
73 m(0,1)=sin(deg);
74 m(0,2)=0;
75 m(1,0)=-sin(deg);
76 m(1,1)=cos(deg);
77 m(1,1)=0;
78 m(2,0)=x;
79 m(2,1)=y;
80 m(2,2)=0;
81 pt(0,0)=-p.x;
82 pt(0,0)=-p.y;
83 pt(0,2)=1;
84 pt*=m;
85 p.x=pt(0,0);
86 p.y=pt(0,1);
87 }
88 void TaskFrame::TtoJ(Point &p,double a1,double a2){
89 double l,deg1,deg2,deg3;
90 l=sqrt(p.x*p.x+p.y*p.y);
91 deg1=atan(p.y/p.x);
92 deg2=acos((a1*a1+l*l-a2*a2)/(2*a1*l));
93 deg3=acos((a1*a1+a2*a2-l*l)/(2*a1*a2));
94 p.x=(deg1+deg2)*180/PI;
95 p.y=deg3*180/PI+180;
96 }
97 class JointFrame{
98
99 };
100
101 class Robot{
102 private:
103 double arm1,arm2,deg1min,deg2min,deg1max,deg2max;
104 public:
105 Robot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
106 void SetRobot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
107 void PTPMove(WorldFrame wf,TaskFrame tf,Point p);
108 void PTPMove(TaskFrame tf,Point P);
109 void PTPMove(JointFrame jf,Point P);
110 void print(Point &p);
111 };
112
113 Robot::Robot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
114 arm1=a1;
115 arm2=a2;
116 deg1min=d1min;
117 deg2min=d2min;
118 deg1max=d1max;
119 deg2max=d2max;
120 }
121 void Robot::SetRobot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
122 arm1=a1;
123 arm2=a2;
124 deg1min=d1min;
125 deg2min=d2min;
126 deg1max=d1max;
127 deg2max=d2max;
128 }
129 void Robot::PTPMove(WorldFrame wf,TaskFrame tf,Point p){
130 tf.WtoT(p);
131 tf.TtoJ(p,arm1,arm2);
132 print(p);
133 }
134 void Robot::PTPMove(TaskFrame tf,Point p){
135 tf.TtoJ(p,arm1,arm2);
136 print(p);
137 }
138 void Robot::PTPMove(JointFrame jf,Point p){
139 print(p);
140 }
141 void Robot::print(Point &p){
142 if(p.x>=deg1min||p.y<=deg1max){
143 cout<<"机器人的关节坐标为：("<<p.x<<','<<p.y<<')'<<endl;
144 }
145 else cout<<"无法旋转到该位置"<<endl;
146 }
147
148
149
150 int main(int argc, char** argv) {
151 Robot myRobot(10,10);
152 WorldFrame WF;
153 TaskFrame TF1(0,0,0),TF2(2,2,30),TF3(1,3,60);
154 JointFrame JF;
155 Point P1(0,0),P2(1,1),P3(2,2),P4(2,1),P5(3,7);
156 myRobot.PTPMove(JF,P1);
157 myRobot.PTPMove(WF,TF1,P2);
158 myRobot.PTPMove(TF1,P3);
159 myRobot.PTPMove(TF2,P4);
160 myRobot.PTPMove(TF3,P5);
161 return 0;
162 }
运算结果：
