1. 实现了到指定点各个关节的转角计算（多解性），并且所求解满足各个关节的最大角和最小角的限制条件。
2. 对方向向量进行了单位化，保证任意大小的向量都行
#include<iostream>
#include <Eigen/Dense>
#include "Robot.h"
int main(){
,l2 =;
Vector2d JF_vx(,),JF_vy(,);
Vector2d WF_vx(,),WF_vy(,);
POINT jf_origin(,),wf_origin(,);
Joint jt1(,,,-,,),jt2(l1,,,-,,);
Frame JF("jf",JF_vx,JF_vy,jf_origin),WF("jf",WF_vx,WF_vy,wf_origin);
Robot myRobot(l1,l2,jt1,jt2,JF,WF);
POINT tf1_origin(,),tf2_origin(,),tf3_origin(,);
Vector2d TF1_vx(,),TF1_vy(-,),TF2_vx(-,),TF2_vy(,-),TF3_vx(,-),TF3_vy(,);
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);
myRobot.TaskFrameCreate(TF1);
myRobot.TaskFrameCreate(TF2);
myRobot.TaskFrameCreate(TF3);
POINT P1(,),P2(,),P3(,),P4(,),P5(,);
myRobot.PTPMove(JF,P1);
// myRobot.RobotShow();
myRobot.PTPMove(WF,P2);
// myRobot.RobotShow();
myRobot.PTPMove(TF1,P3);
// myRobot.RobotShow();
myRobot.PTPMove(TF2,P4);
// myRobot.RobotShow();
myRobot.PTPMove(TF3,P5);
// myRobot.RobotShow();
;
}
main.cpp
#include<iostream>
#include <Eigen/Dense>
#include<vector>
using namespace Eigen;
using namespace std;
#define PI 3.141592653
class POINT
{
public:
double x, y;
string name;
POINT(){
};
POINT(double xx,double yy){
x=xx;
y=yy;
};
POINT(string nam,double xx,double yy){
name=nam;
x=xx;
y=yy;
}
POINT(const POINT &p){
name=p.name;
x=p.x;
y=p.y;
}
POINT operator =(const POINT &pt)
{
POINT ptt(pt);
return ptt;
}
void copyto(POINT &p);
void get_cin_point(void);
void display();
void rotate(double &angle);
void move(Vector2d &vec);
};
class Frame
{
public:
string name;
Vector2d vector_X;
Vector2d vector_Y;
POINT origin;
Frame(){
}
Frame(string nam,Vector2d &vx,Vector2d &vy,POINT &oripoint)
{
name=nam;
],)+pow(vx[],)); //化为单位方向向量
vx[]=vx[]/a;
vx[]=vx[]/a;
],)+pow(vy[],)); //化为单位方向向量
vy[]=vy[]/a;
vy[]=vy[]/a;
vector_X=vx;
vector_Y=vy;
//origin=oripoint;
oripoint.copyto(origin);
}
Frame(const Frame &fr)
{
name=fr.name;
vector_X=fr.vector_X;
vector_Y=fr.vector_Y;
origin=fr.origin;
}
Frame operator =(const Frame &fr)
{
Frame fra(fr);
return fra;
}
};
class Joint
{
public:
double x,y,theta;
double thetamin,thetamax,thetazero;
Joint(){
}
Joint(double xx,double yy,double thetaa,double thetaminn,double thetamaxx,double thetazeroo)
{
x=xx;
y=yy;
theta=thetaa;
thetamin=thetaminn;
thetamax=thetamaxx;
thetazero=thetazeroo;
}
CopyTo(Joint &jt)
{
jt.x=x;
jt.y=y;
jt.theta=theta;
jt.thetamin=thetamin;
jt.thetamax=thetamax;
jt.thetazero=thetazero;
}
};
class Robot
{
public:
double length1,length2;
Joint joint1,joint2;
Frame JointFrame,WorldFrame;
vector<Frame> fv;
Robot(){
}
Robot(double l1,double l2,Joint jt1,Joint jt2,Frame JF,Frame WF)
{
length1=l1;
length2=l2;
jt1.CopyTo(joint1);
jt2.CopyTo(joint2);
JointFrame=JF;
WorldFrame=WF;
}
void TaskFrameCreate(const Frame &tf);
void PTPMove(const Frame &fr,const POINT &pt);
void RobotShow(void);
void ToJoint();
void JointTo();
};
// class Solver
// {
// public:
// //friend void PTPMove(Frame &fr,POINT &pt);
// void ToJoint(Robot &myrobot);
// void JointTo(Robot &myrobot);
// };
Robot.h
#include "Robot.h"
#include "math.h"
void Robot::TaskFrameCreate(const Frame &tf)
{
fv.push_back(tf);
}
void Robot::PTPMove(const Frame &fr,const POINT &pt)
{
],fr.vector_X[]);
joint2.x=fr.origin.x+pt.x*cos(theta)-pt.y*sin(theta);
joint2.y=fr.origin.y+pt.x*sin(theta)+pt.y*cos(theta);
cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl;
ToJoint();
}
void Robot::ToJoint()
{
double t1,t2;
]={false};
bool flag1=false;
][];
;
)+pow(joint2.y,))+(pow(length1,)-pow(length2,)))/(*length1*sqrt(pow(joint2.x,)+pow(joint2.y,)));
|a>) flag1=false;
else{ //多解性
t1=acos(a);//acos默认取值 0~PI
t2=atan2(joint2.y,joint2.x);
theta[][]=t1+t2;
][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){
theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][]));
][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){
][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01)
{
flag[]=true;
}
}
}
theta[][]=-t1+t2;
][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){
theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][]));
][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){
][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01)
{
flag[]=true;
}
}
}
theta[][]=t1-t2;
][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){
theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][]));
][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){
][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01)
{
flag[]=true;
}
}
}
theta[][]=-t1-t2;
][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){
theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][]));
][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){
][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01)
{
flag[]=true;
}
}
}
;i<;i++)
{
if(flag[i]==true) {
flag1=true;
cnt++;
joint1.theta=theta[i][];
joint2.theta=theta[i][];
cout<<"第"<<cnt<<"种解为：";
cout<<]*/PI;
cout<<]*/PI<<endl;
}
}
}
if(flag1==false) cout<<"无法达到指定位置"<<endl;
}
void Robot::JointTo()
{
joint2.x=length1*cos(joint1.theta)+length2*cos(joint2.theta);
joint2.y=length1*sin(joint1.theta)+length2*sin(joint2.theta);
}
void Robot::RobotShow(void){
// cout<<"关节1转角"<< joint1.theta*180/PI;
// cout<<"关节2转角"<< joint2.theta*180/PI;
// cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl;
}
void POINT::copyto(POINT &p){
p.name=name;
p.x=x;
p.y=y;
}
Robot.cpp