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