第二次编程作业

  1 //Point2D.h 点类
  2 #ifndef POINT2D
  3 #define POINT2D
  4 
  5 #include<Eigen\Dense>
  6 using namespace Eigen;
  7 
  8 class Point2D
  9 {
 10 private:
 11     double x;
 12     double y;
 13 
 14 public:
 15     //Point2D(){};
 16     Point2D(double X = 0.0,double Y = 0.0){x = X; y = Y;}
 17 
 18     double getX(){return x;}
 19     double getY(){return y;}
 20 
 21     Vector2d  PTV();
 22     Point2D & operator+(const Point2D &);
 23     Point2D & operator-(const Point2D &);
 24     Point2D & operator=(const Point2D &);
 25 };
 26 
 27 #endif POINT2D
 28 
 29 
 30 //Point2D.cpp
 31 #include "Point2D.h"
 32 
 33 Point2D & Point2D::operator+(const Point2D & p)
 34 {
 35     Point2D temp(x+p.x,y+p.y);
 36     return temp;
 37 }
 38 
 39 Point2D & Point2D::operator-(const Point2D & p)
 40 {
 41     Point2D temp(x-p.x,y-p.y);
 42     return temp;
 43 }
 44 
 45 Point2D & Point2D::operator=(const Point2D & p)
 46 {
 47     Point2D temp(p.x,p.y);
 48     return temp;
 49 }
 50 
 51 Vector2d  Point2D::PTV()
 52 {
 53     Vector2d temp;
 54     temp(0) = getX();
 55     temp(1) = getY();
 56     return temp;
 57 }
 58 
 59 
 60 //Frames.h 坐标系类
 61 #ifndef Frame
 62 #define Frame
 63 
 64 #include"Point2D.h"
 65 
 66 class Frames
 67 {
 68 public:
 69     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 70     Frames(){};
 71     Frames(double,double,double,double,double,double);
 72     ~Frames(){};
 73     Matrix2d  rotation(Frames &);    //两坐标系之间的旋转矩阵
 74     Vector2d  move(Frames &);        //两坐标系之间的移动向量
 75 
 76 private:
 77     Point2D originalPoint;
 78     Vector2d XAxis;
 79     Vector2d YAxis;
 80 };
 81 
 82 #endif Frame
 83 
 84 
 85 //Frames.cpp
 86 #include"Frames.h"
 87 #include"math.h"
 88 #include<cmath>
 89 #include<iostream>
 90 using namespace std;
 91 
 92 #define pi 3.141592654
 93 
 94 Frames::Frames(double px,double py,double vxx,double vxy,double vyx,double vyy)
 95     :originalPoint(px,py),XAxis(vxx,vxy),YAxis(vyx,vyy)
 96 {
 97     
 98     if(XAxis.dot(YAxis) != 0.0)
 99     {
100         cout << "输入的坐标轴不垂直！" << endl;
101     }
102 }
103 
104 //两坐标系之间的旋转矩阵
105 Matrix2d  Frames::rotation(Frames& fram)
106 {
107     double s1;
108     double c1;
109     double theta;
110     Matrix2d temp;
111     if(fram.XAxis(0) !=0 && XAxis(0)!= 0)
112         theta = atan(fram.XAxis(1)/fram.XAxis(0))-atan(XAxis(1)/XAxis(0));
113     if(fram.XAxis(0) !=0 && XAxis(0)== 0)
114     {
115         if(XAxis(1)== 1)
116             theta = atan(fram.XAxis(1)/fram.XAxis(0))- 90/180*pi;
117         if(XAxis(1)== -1)
118             theta = atan(fram.XAxis(1)/fram.XAxis(0))- 180/180*pi;
119     }
120     if(fram.XAxis(0) ==0 && XAxis(0)!= 0)
121     {
122         double k = atan(XAxis(1)/XAxis(0));
123         if(fram.XAxis(1)== 1)
124             theta = (double)90/180*pi - k;
125         if(fram.XAxis(1)== -1)
126             theta = (double)180/180*pi - atan(XAxis(1)/XAxis(0));
127     }
128     s1 = sin(theta);
129     c1 = cos(theta);
130     if(abs(s1) < 0.000000000000001)
131         s1 = 0;
132     if(abs(c1) < 0.000000000000001)
133         c1 = 0;
134     temp << c1,-s1,
135             s1,c1;
136     return temp;
137 }
138 
139 
140 //两坐标系之间的移动向量
141 Vector2d  Frames::move(Frames & fra)
142 {
143     Vector2d temp;
144     temp = fra.originalPoint.PTV()-originalPoint.PTV();
145     return temp;
146 }
147 
148 
149 //Solver.h  解算类
150 #ifndef SOLVER
151 #define SOLVER
152 
153 #include"Frames.h"
154 
155 class Solver 
156 {
157 public:
158     Solver(){};
159     ~Solver(){};
160     Vector2d  directSolution(double link1,double link2,double th1,double th2);
161     Matrix2d  inverSolution(double link1,double link2,Frames &, Point2D &);
162 private:
163 
164 };
165 
166 
167 
168 
169 #endif SOLVER
170 
171 //Solver.cpp
172 #include"Solver.h"
173 #include<iostream>
174 using namespace std;
175 #include<cmath>
176 #define pi 3.141592654
177 
178 Vector2d Solver::directSolution(double link1,double link2,double th1,double th2)
179 {
180     Vector2d temp;
181     temp(0) = link1*cos(th1) + link2*cos(th1+th2);
182     temp(1) = link1*sin(th1) + link2*sin(th1+th2);
183 
184     return temp;
185 }
186 
187 Matrix2d  Solver::inverSolution(double link1,double link2,Frames &f,Point2D & p)
188 {
189     Vector3d point;
190     Vector2d result;    //p在基坐标系上的坐标值
191     Vector2d moveF;        //坐标系f与极坐标系原点之间的向量
192     Matrix2d rota;        //坐标系f与基坐标系之间的旋转矩阵
193     Matrix3d T;            //坐标系f与基坐标系之间的转换矩阵
194     Matrix2d Theta;    //关节角
195     Vector2d temp;
196     Frames base(0.0,0.0,1.0,0.0,0.0,1.0);
197 
198     //把坐标系f的点p转换到基坐标
199     rota = base.rotation(f);
200     moveF = base.move(f);
201     moveF = moveF.transpose();
202     T << rota,moveF,
203         0,0,1;
204     temp = p.PTV();
205     point << temp,1;
206     point = point.transpose();
207     point = T*point;
208     result << point(0),point(1);
209 
210     //判断点是否在找工作空间
211     double dis = sqrt(result.dot(result));
212     if(dis>(link1+link2) || dis<(link1-link2))
213     {
214         Matrix2d zeros(2,2);
215         cout << "坐标点不在工作空间！"<< endl;
216         return zeros;
217     }
218 
219     //求反解关节值
220     else
221     {
222     double beita = atan2(result(1),result(0));
223     double fi;
224     double theta11,theta12,theta21,theta22;
225 
226     fi = acos((dis*dis+link1*link1-link2*link2)/(2*link1*dis));
227     theta12 = acos((dis*dis-link1*link1-link2*link2)/(2*link1*link2))*180/pi;
228 
229     if(theta12<0)
230         theta11 = (beita + fi)*180/pi;
231     else 
232         theta11 = (beita - fi)*180/pi;
233 
234     theta22 = -acos((dis*dis-link1*link1-link2*link2)/(2*link1*link2))*180/pi;
235 
236     if(theta22<0)
237         theta21 = (beita + fi)*180/pi;
238     else 
239         theta21 = (beita - fi)*180/pi;
240     Theta << theta11,theta12,
241         theta21,theta22;
242     return Theta;
243     }
244     
245 }
246 
247 
248 //Robot.h 机器人类
249 #ifndef ROBOT
250 #define ROBOT
251 
252 #include<vector>    
253 #include"Solver.h"
254 using namespace std;
255 
256 
257 class Robot:Solver
258 {
259 public:
260     Robot(){};
261     Robot(vector<Frames> &fr,double link1 = 100,double link2 = 100,
262         double the1=0,double the2 = 0);
263     ~Robot(){};
264 
265     void PTPMove(Frames &,Point2D &);
266     void thetaToTCP(double,double);
267 
268     inline double getLink1(){return linkage1;}
269     inline double getLink2(){return linkage2;}
270     inline double getTheta1(){return theta1;}
271     inline double getTheta2(){return theta2;}
272 
273 private:
274     vector <Frames> fra;
275     double linkage1;
276     double linkage2;
277     double theta1;
278     double theta2;
279 };
280 
281 
282 
283 
284 #endif ROBOT
285 
286 
287 //Robot.cpp
288 #include"Robot.h"
289 #include<iostream>
290 using namespace std;
291 
292 Robot::Robot(vector<Frames>& fr,double link1,double link2
293              ,double the1,double the2):
294              linkage1(link1),linkage2(link2)
295              ,theta1(the1),theta2(the2)
296 {
297     for(int i=0; i < fr.size(); i++)
298         fra.push_back(fr[i]);
299     /*if (fr.size() == 0)
300     {
301         Frames base(0.0,0.0,1.0,0.0,0.0,1.0);
302         fra.push_back(base);
303     }*/
304 }
305 
306 void Robot::PTPMove(Frames & f,Point2D & p)
307 {
308     Matrix2d temp;
309     Matrix2d zeros(2,2);
310     temp = inverSolution(linkage1,linkage2,f,p);
311     if (temp != zeros)
312         cout << "关节值： " <<endl << temp << endl;
313     cout << endl;
314 }
315 void Robot::thetaToTCP(double th1,double th2)
316 {
317     Vector2d temp;
318     temp = directSolution(linkage1,linkage2,th1,th2);
319     cout << "运动到的点坐标：" << endl << temp;
320     cout << endl;
321 }
322 
323 
324 
325 //测试程序
326 
327 #include"Robot.h"
328 #include <iostream>
329 using namespace std;
330 
331 int main()
332 {
333     Point2D p(90,92);    //要运动到的目标点坐标
334     Point2D p1(500,1305);
335     vector<Frames> robotFrames;
336     Frames WF(0.0,0.0,1.0,0.0,0.0,1.0);    //世界坐标系
337     robotFrames.push_back(WF);
338     Frames J1(0.0,0.0,1.0,0.0,0.0,1.0);    //关节1坐标系
339     robotFrames.push_back(J1);
340     Frames J2(200.0,0.0,1.0,0.0,0.0,1.0);    //关节2坐标系
341     robotFrames.push_back(J2);
342     Frames TCP(200.0,150.0,0.0,1.0,-1.0,0.0);    //工具坐标系
343     robotFrames.push_back(TCP);
344 
345     vector<int> b;
346     for(int i = 0; i< 5;i++)
347         b.push_back(i);
348     //工作坐标系
349     Frames TF1(1.0,1.0,0.8,0.6,-0.6,0.8);
350     Frames TF2(1.0,2.0,0.0,1.0,-1.0,0.0);
351 
352     Robot myrobot(robotFrames,200.0,150.0,0.0,90.0);
353 
354     myrobot.PTPMove(WF,p);
355     myrobot.PTPMove(J1,p);
356     myrobot.PTPMove(J2,p);
357     myrobot.PTPMove(TF1,p);
358     myrobot.PTPMove(TF2,p);
359     myrobot.PTPMove(TCP,p);
360 
361     myrobot.PTPMove(WF,p1);
362     myrobot.PTPMove(J1,p1);
363     myrobot.PTPMove(J2,p1);
364     myrobot.PTPMove(TF1,p1);
365     myrobot.PTPMove(TF2,p1);
366     myrobot.PTPMove(TCP,p1);
367 
368     system("pause");
369      return 1;
370 }