1 #include <iostream>
2 #include"robot.h"
3 /* run this program using the console pauser or add your own getch, system("pause") or input loop */
4
5 int main(int argc, char** argv) {
6 Robot Robot(140,200,6,4);
7 jointframe JF;
8 worldframe WF;
9 taskframe TF1(4,3,35),TF2(2,2,0),TF3(0,1,40);
10 Robot.PTPMove(JF,150,240);
11 Robot.PTPMove(WF,-1,7);
12 Robot.PTPMove(TF1,-2,3);
13 Robot.PTPMove(TF2,-1,5);
14 Robot.PTPMove(TF3,-1,3);
15 return 0;
16 }
17 #ifndef FRAME_H
18 #define FRAME_H
19 #include <iostream>
20 #include <Eigen/Dense>
21 #include <cmath>
22 #include <vector>
23 #define pi 3.1415926
24 using Eigen::MatrixXd;
25 using namespace std;
26
27 class jointframe{
28 private:
29 double deg1;
30 double deg2;
31 public:
32 jointframe(double deg=0,double deg4=0);
33 double getdeg1();
34 double getdeg2();
35 };
36
37 class worldframe{
38 private:
39 double wfx;
40 double wfy;
41 double wfdeg;
42 public:
43 worldframe();
44 //worldframe(double wfx1,double wfy1,double wfdeg1);
45 //MatrixXd getWF();
46 //double getX();
47 //double getY();
48 };
49
50
51
52 class taskframe{
53 private:
54 double tfx;
55 double tfy;
56 double tfdeg;
57 public:
58 taskframe();
59 taskframe(double tfx1,double tfy1,double tfdeg1);
60 MatrixXd getTF();
61 double getX();
62 double getY();
63 };
64 #endif
65 #include "frame.h"
66 jointframe::jointframe(double deg3,double deg4){
67 deg1=deg3;
68 deg2=deg4;
69 }
70 double jointframe::getdeg1(){
71 return deg1;
72 }
73 double jointframe::getdeg2(){
74 return deg2;
75 }
76 worldframe::worldframe(){
77 wfx=wfy=0;
78 wfdeg=0*pi/180;
79 }
80 taskframe::taskframe(){
81 tfx=tfy=0;
82 tfdeg=0*pi/180;
83 }
84 taskframe::taskframe(double tfx1,double tfy1,double tfdeg1){
85 tfx=tfx1;
86 tfy=tfy1;
87 tfdeg=tfdeg1*pi/180;
88 }
89 MatrixXd taskframe::getTF(){
90 MatrixXd TF(3,3);
91 TF(0,0)=cos(tfdeg);
92 TF(0,1)=sin(tfdeg);
93 TF(0,2)=tfx;
94 TF(1,0)=-sin(tfdeg);
95 TF(1,1)=cos(tfdeg);
96 TF(1,2)=tfy;
97 TF(2.0)=TF(2,1)=0;
98 TF(2,2)=1;
99 return TF;
100 }
101 double taskframe::getX(){
102 return tfx;
103 }
104 double taskframe::getY(){
105 return tfy;
106 }
107 #ifndef SOLVER_H
108 #define SOLVER_H
109 #include"frame.h"
110
111 class Solver{
112 private:
113 double x,y;
114 double theta3,theta4;
115 worldframe WF1;
116 public:
117 Solver();
118 MatrixXd getXY(double a,double b,double l1,double l2);
119 double getdeg1(double c,double d,double l1,double l2);
120 double getdeg2(double c,double d,double l1,double l2);
121 };
122 #endif
123 #include"solver.h"
124 Solver::Solver(){
125 x=y=0;
126 theta3=theta4=0;
127 }
128 MatrixXd Solver::getXY(double a,double b,double l1,double l2){
129 MatrixXd T(3,3);
130 double ang1,ang2,arm1,arm2;
131 ang1=a;
132 ang2=b;
133 arm1=l1;
134 arm2=l2;
135 T(0,0)=cos(ang1+ang2);
136 T(0,1)=-sin(ang1+ang2);
137 T(0,2)=arm2*cos(ang1+ang2)+arm1*cos(ang1);
138 T(1,0)=sin(ang1+ang2);
139 T(1,1)=cos(ang1+ang2);
140 T(1,2)=arm2*sin(ang1+ang2)+arm1*sin(ang1);
141 T(2,0)=T(2,1)=0;
142 T(2,2)=1;
143 return T;
144 }
145 double Solver::getdeg1(double c,double d,double l1,double l2){
146 double theta34;
147 double arm1,arm2,l;
148 x=c;
149 y=d;
150 l=abs(sqrt(x*x+y*y));
151 arm1=l1;
152 arm2=l2;
153 theta34=atan2(y,x)/pi*180;
154 theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
155 theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
156 return theta3;
157 }
158 double Solver::getdeg2(double c,double d,double l1,double l2){
159 double theta34;
160 double arm1,arm2,l;
161 x=c;
162 y=d;
163 l=abs(sqrt(x*x+y*y));
164 arm1=l1;
165 arm2=l2;
166 theta34=atan2(y,x)/pi*180;
167 theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
168 theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
169 return theta4;
170 }
171 #ifndef ROBOT_H
172 #define ROBOT_H
173 #include"solver.h"
174 class Robot{
175 private:
176 double theta1;
177 double theta2;
178 double seg1;
179 double seg2;
180 jointframe JF;
181 worldframe WF;
182 std::vector<taskframe> TF;
183 Solver normal;
184 Solver inverse;
185 public:
186 Robot();
187 Robot(double alpha,double beta,double lenth1,double lenth2);
188 void PTPMove(jointframe JF1,double X,double Y);
189 void PTPMove(worldframe WF1,double X,double Y);
190 void PTPMove(taskframe TF1,double X,double Y);
191 };
192 #endif
193 #include"robot.h"
194 Robot::Robot(){
195 theta1=180/pi*180;
196 theta2=270/pi*180;
197 seg1=0;seg2=0;
198 }
199 Robot::Robot(double alpha,double beta,double lenth1,double lenth2){
200 theta1=alpha/pi*180;
201 theta2=beta/pi*180;
202 seg1=lenth1;
203 seg2=lenth2;
204 }
205 void Robot::PTPMove(jointframe JF1,double X,double Y){
206 double x,y,c,d;
207 MatrixXd B(3,1);
208 c=X+JF1.getdeg1();
209 d=Y+JF1.getdeg2();
210 x=c*pi/180;
211 y=d*pi/180;
212 if(c>180||c<100||d<220||d>300)
213 {
214 cout<<"无法旋转到该位置"<<endl;
215 }
216 else
217 {
218 B=normal.getXY(x,y,seg1,seg2);
219 cout<<"关节坐标为"<<'('<<B(0,0)<<','<<B(1,0)<<')'<<endl;
220 }
221
222 }
223 void Robot::PTPMove(worldframe WF1,double X,double Y){
224 double deg1,deg2,x,y;
225 x=X;
226 y=Y;
227 deg1=inverse.getdeg1(x,y,seg1,seg2);
228 deg2=inverse.getdeg2(x,y,seg1,seg2);
229 if(deg1>180||deg1<100||deg2<220||deg2>300)
230 {
231 cout<<"无法旋转到该位置"<<endl;
232 }
233 else
234 {
235 cout<<"关节坐标为"<<'('<<deg1<<','<<deg2<<')'<<endl;
236 }
237 }
238 void Robot::PTPMove(taskframe TF1,double X,double Y){
239 double deg1,deg2,x,y;
240 MatrixXd TFs(3,3),coordt(3,1),anst(3,1);
241 coordt(0,0)=X;
242 coordt(1,0)=Y;
243 coordt(2,0)=1;
244 TFs=TF1.getTF();
245 anst=TFs*coordt;
246 x=anst(0,0);
247 y=anst(1,0);
248 deg1=inverse.getdeg1(x,y,seg1,seg2);
249 deg2=inverse.getdeg2(x,y,seg1,seg2);
250 if(deg1>180||deg1<100||deg2<220||deg2>300)
251 {
252 cout<<"无法旋转到该位置"<<endl;
253 }
254 else
255 {
256 cout<<"关节坐标为"<<'('<<deg1<<','<<deg2<<')'<<endl;
257 }
258 }
