标签：
1. 实现了到指定点各个关节的转角计算（多解性），并且所求解满足各个关节的最大角和最小角的限制条件。
2. 对方向向量进行了单位化，保证任意大小的向量都行
1 #include<iostream> 2 #include <Eigen/Dense> 3 #include "Robot.h" 4 5 int main(){ 6 const double l1=300,l2 =500; 7 Vector2d JF_vx(1,0),JF_vy(0,1); 8 Vector2d WF_vx(1,0),WF_vy(0,1); 9 POINT jf_origin("jf_origin",0,0),wf_origin("wf_origin",0,0); 10 Joint jt1(0,0,0,-180,180,0),jt2(l1,0,0,-180,180,0); 11 Frame JF("jf",JF_vx,JF_vy,jf_origin),WF("jf",WF_vx,WF_vy,wf_origin); 12 Robot myRobot(l1,l2,jt1,jt2,JF,WF); 13 POINT tf1_origin("tf1_origin",400,200),tf2_origin("tf2_origin",100,300),tf3_origin("tf3_origin",200,400); 14 Vector2d TF1_vx(0,1),TF1_vy(-1,0),TF2_vx(-1,0),TF2_vy(0,-1),TF3_vx(0,-1),TF3_vy(1,0); 15 Frame TF1("tf1",TF1_vx,TF1_vy,tf1_origin),TF2("tf2",TF2_vx,TF2_vy,tf2_origin),TF3("tf3",TF3_vx,TF3_vy,tf3_origin); 16 myRobot.TaskFrameCreate(TF1); 17 myRobot.TaskFrameCreate(TF2); 18 myRobot.TaskFrameCreate(TF3); 19 POINT P1(1,2),P2(1,2),P3(1,2),P4(1,2),P5(1,2); 20 myRobot.PTPMove(JF,P1); 21 // myRobot.RobotShow(); 22 myRobot.PTPMove(WF,P2); 23 // myRobot.RobotShow(); 24 myRobot.PTPMove(TF1,P3); 25 // myRobot.RobotShow(); 26 myRobot.PTPMove(TF2,P4); 27 // myRobot.RobotShow(); 28 myRobot.PTPMove(TF3,P5); 29 // myRobot.RobotShow(); 30 return 0; 31 }
1 #include<iostream> 2 #include <Eigen/Dense> 3 #include<vector> 4 using namespace Eigen; 5 using namespace std; 6 #define PI 3.141592653 7 class POINT 8 { 9 public: 10 double x, y; 11 string name; 12 POINT(){ 13 }; 14 POINT(double xx,double yy){ 15 x=xx; 16 y=yy; 17 }; 18 POINT(string nam,double xx,double yy){ 19 name=nam; 20 x=xx; 21 y=yy; 22 } 23 POINT(const POINT &p){ 24 name=p.name; 25 x=p.x; 26 y=p.y; 27 } 28 POINT operator =(const POINT &pt) 29 { 30 POINT ptt(pt); 31 return ptt; 32 } 33 void copyto(POINT &p); 34 void get_cin_point(void); 35 void display(); 36 void rotate(double &angle); 37 void move(Vector2d &vec); 38 }; 39 class Frame 40 { 41 public: 42 string name; 43 Vector2d vector_X; 44 Vector2d vector_Y; 45 POINT origin; 46 Frame(){ 47 } 48 Frame(string nam,Vector2d &vx,Vector2d &vy,POINT &oripoint) 49 { 50 name=nam; 51 double a=sqrt(pow(vx[0],2)+pow(vx[1],2)); //化为单位方向向量 52 vx[0]=vx[0]/a; 53 vx[1]=vx[1]/a; 54 double b=sqrt(pow(vy[0],2)+pow(vy[1],2)); //化为单位方向向量 55 vy[0]=vy[0]/a; 56 vy[1]=vy[1]/a; 57 vector_X=vx; 58 vector_Y=vy; 59 //origin=oripoint; 60 oripoint.copyto(origin); 61 } 62 Frame(const Frame &fr) 63 { 64 name=fr.name; 65 vector_X=fr.vector_X; 66 vector_Y=fr.vector_Y; 67 origin=fr.origin; 68 } 69 Frame operator =(const Frame &fr) 70 { 71 Frame fra(fr); 72 return fra; 73 } 74 }; 75 class Joint 76 { 77 public: 78 double x,y,theta; 79 double thetamin,thetamax,thetazero; 80 Joint(){ 81 } 82 Joint(double xx,double yy,double thetaa,double thetaminn,double thetamaxx,double thetazeroo) 83 { 84 x=xx; 85 y=yy; 86 theta=thetaa; 87 thetamin=thetaminn; 88 thetamax=thetamaxx; 89 thetazero=thetazeroo; 90 } 91 CopyTo(Joint &jt) 92 { 93 jt.x=x; 94 jt.y=y; 95 jt.theta=theta; 96 jt.thetamin=thetamin; 97 jt.thetamax=thetamax; 98 jt.thetazero=thetazero; 99 } 100 }; 101 class Robot 102 { 103 public: 104 double length1,length2; 105 Joint joint1,joint2; 106 Frame JointFrame,WorldFrame; 107 vector<Frame> fv; 108 Robot(){ 109 } 110 Robot(double l1,double l2,Joint jt1,Joint jt2,Frame JF,Frame WF) 111 { 112 length1=l1; 113 length2=l2; 114 jt1.CopyTo(joint1); 115 jt2.CopyTo(joint2); 116 JointFrame=JF; 117 WorldFrame=WF; 118 } 119 void TaskFrameCreate(const Frame &tf); 120 void PTPMove(const Frame &fr,const POINT &pt); 121 void RobotShow(void); 122 void ToJoint(); 123 void JointTo(); 124 }; 125 // class Solver 126 // { 127 // public: 128 // //friend void PTPMove(Frame &fr,POINT &pt); 129 // void ToJoint(Robot &myrobot); 130 // void JointTo(Robot &myrobot); 131 // };
1 #include "Robot.h" 2 #include "math.h" 3 void Robot::TaskFrameCreate(const Frame &tf) 4 { 5 fv.push_back(tf); 6 } 7 void Robot::PTPMove(const Frame &fr,const POINT &pt) 8 { 9 double theta=atan2(fr.vector_X[1],fr.vector_X[0]); 10 joint2.x=fr.origin.x+pt.x*cos(theta)-pt.y*sin(theta); 11 joint2.y=fr.origin.y+pt.x*sin(theta)+pt.y*cos(theta); 12 cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl; 13 ToJoint(); 14 } 15 void Robot::ToJoint() 16 { 17 double t1,t2; 18 bool flag[4]={false}; 19 bool flag1=false; 20 double theta[4][2]; 21 int cnt=0; 22 double a=((pow(joint2.x,2)+pow(joint2.y,2))+(pow(length1,2)-pow(length2,2)))/(2*length1*sqrt(pow(joint2.x,2)+pow(joint2.y,2))); 23 if(a<-1|a>1) flag1=false; 24 else{ //多解性 25 t1=acos(a);//acos默认取值 0~PI 26 t2=atan2(joint2.y,joint2.x); 27 theta[0][0]=t1+t2; 28 if(theta[0][0]*180/PI<joint1.thetamax&theta[0][0]*180/PI>joint1.thetamin){ 29 theta[0][1]=atan2(joint2.y-length1*sin(theta[0][0]),joint2.x-length1*cos(theta[0][0])); 30 if(theta[0][1]*180/PI<joint2.thetamax&theta[0][1]*180/PI>joint2.thetamin){ 31 if(abs(joint2.x-length1*cos(theta[0][0])-length2*cos(theta[0][1]))<0.01&abs(joint2.y-length1*sin(theta[0][0])-length2*sin(theta[0][1]))<0.01) 32 { 33 flag[0]=true; 34 } 35 } 36 } 37 theta[1][0]=-t1+t2; 38 if(theta[1][0]*180/PI<joint1.thetamax&theta[1][0]*180/PI>joint1.thetamin){ 39 theta[1][1]=atan2(joint2.y-length1*sin(theta[1][0]),joint2.x-length1*cos(theta[1][0])); 40 if(theta[1][1]*180/PI<joint2.thetamax&theta[1][1]*180/PI>joint2.thetamin){ 41 if(abs(joint2.x-length1*cos(theta[1][0])-length2*cos(theta[1][1]))<0.01&abs(joint2.y-length1*sin(theta[1][0])-length2*sin(theta[1][1]))<0.01) 42 { 43 flag[1]=true; 44 } 45 } 46 } 47 theta[2][0]=t1-t2; 48 if(theta[2][0]*180/PI<joint1.thetamax&theta[2][0]*180/PI>joint1.thetamin){ 49 theta[2][1]=atan2(joint2.y-length1*sin(theta[2][0]),joint2.x-length1*cos(theta[2][0])); 50 if(theta[2][1]*180/PI<joint2.thetamax&theta[2][1]*180/PI>joint2.thetamin){ 51 if(abs(joint2.x-length1*cos(theta[2][0])-length2*cos(theta[2][1]))<0.01&abs(joint2.y-length1*sin(theta[2][0])-length2*sin(theta[2][1]))<0.01) 52 { 53 flag[2]=true; 54 } 55 } 56 } 57 theta[3][0]=-t1-t2; 58 if(theta[3][0]*180/PI<joint1.thetamax&theta[3][0]*180/PI>joint1.thetamin){ 59 theta[3][1]=atan2(joint2.y-length1*sin(theta[3][0]),joint2.x-length1*cos(theta[3][0])); 60 if(theta[3][1]*180/PI<joint2.thetamax&theta[3][1]*180/PI>joint2.thetamin){ 61 if(abs(joint2.x-length1*cos(theta[3][0])-length2*cos(theta[3][1]))<0.01&abs(joint2.y-length1*sin(theta[3][0])-length2*sin(theta[3][1]))<0.01) 62 { 63 flag[3]=true; 64 } 65 } 66 } 67 for(int i=0;i<4;i++) 68 { 69 if(flag[i]==true) { 70 flag1=true; 71 cnt++; 72 joint1.theta=theta[i][0]; 73 joint2.theta=theta[i][1]; 74 cout<<"第"<<cnt<<"种解为："; 75 cout<<"关节1转角"<< theta[i][0]*180/PI; 76 cout<<"关节2转角"<< theta[i][1]*180/PI<<endl; 77 } 78 } 79 } 80 81 if(flag1==false) cout<<"无法达到指定位置"<<endl; 82 } 83 84 void Robot::JointTo() 85 { 86 joint2.x=length1*cos(joint1.theta)+length2*cos(joint2.theta); 87 joint2.y=length1*sin(joint1.theta)+length2*sin(joint2.theta); 88 } 89 void Robot::RobotShow(void){ 90 // cout<<"关节1转角"<< joint1.theta*180/PI; 91 // cout<<"关节2转角"<< joint2.theta*180/PI; 92 // cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl; 93 } 94 void POINT::copyto(POINT &p){ 95 p.name=name; 96 p.x=x; 97 p.y=y; 98 }
标签：
原文地址：http://www.cnblogs.com/yixu/p/5062047.html
