1 //Point2D.h 点类 2 #ifndef POINT2D 3 #define POINT2D 4 5 #include<EigenDense> 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 }
测试结果
