《实时控制软件设计》第二次编程作业

//代码：

#include <iostream>
#include <Eigen/Dense>
#include <cmath>
#include <vector>
using Eigen::MatrixXd;
using namespace std;
float pi=3.1415926;
class jointframe{
private:
    double deg1;
    double deg2;
public:
    jointframe();
    jointframe(double deg3,double deg4);
    double getdeg1();
    double getdeg2();
};
jointframe::jointframe(){
    deg1=deg2=0;
}
jointframe::jointframe(double deg3,double deg4){
    deg1=deg3;
    deg2=deg4;
}
double jointframe::getdeg1(){
    return deg1;
}
double jointframe::getdeg2(){
    return deg2;
}

class worldframe{
private:
    double wfx;
    double wfy;
    double wfdeg;
public:
    worldframe();
    worldframe(double wfx1,double wfy1,double wfdeg1);
    MatrixXd getWF();
    double getX();
    double getY();
};
worldframe::worldframe(){
    wfx=wfy=0;
    wfdeg=0*pi/180;
}
worldframe::worldframe(double wfx1,double wfy1,double wfdeg1){
    wfx=wfx1;
    wfy=wfy1;
    wfdeg=wfdeg1*pi/180;
}
MatrixXd worldframe::getWF(){
    MatrixXd WF(3,3);
    WF(0,0)=cos(-wfdeg);
    WF(0,1)=sin(-wfdeg);
    WF(0,2)=-wfx;
    WF(1,0)=-sin(-wfdeg);
    WF(1,1)=cos(-wfdeg);
    WF(1,2)=-wfy;
    WF(2.0)=WF(2,1)=0;
    WF(2,2)=1;
    return WF;
}
double worldframe::getX(){
    return wfx;
}
double worldframe::getY(){
    return wfy;
}

class taskframe{
private:
    double tfx;
    double tfy;
    double tfdeg;
public:
    taskframe();
    taskframe(double tfx1,double tfy1,double tfdeg1);
    MatrixXd getTF();
    double getX();
    double getY();
};
taskframe::taskframe(){
    tfx=tfy=0;
    tfdeg=0*pi/180;
}
taskframe::taskframe(double tfx1,double tfy1,double tfdeg1){
    tfx=tfx1;
    tfy=tfy1;
    tfdeg=tfdeg1*pi/180;
}
MatrixXd taskframe::getTF(){
    MatrixXd TF(3,3);
    TF(0,0)=cos(tfdeg);
    TF(0,1)=sin(tfdeg);
    TF(0,2)=tfx;
    TF(1,0)=-sin(tfdeg);
    TF(1,1)=cos(tfdeg);
    TF(1,2)=tfy;
    TF(2.0)=TF(2,1)=0;
    TF(2,2)=1;
    return TF;
}
double taskframe::getX(){
    return tfx;
}
double taskframe::getY(){
    return tfy;
}


class Solver{
private:
    double x,y;
    double theta3,theta4;
    worldframe WF1;
public:
    Solver();
    MatrixXd getXY(double a,double b,double l1,double l2);
    double getdeg1(double c,double d,double l1,double l2);
    double getdeg2(double c,double d,double l1,double l2);
};
Solver::Solver(){
    x=y=0;
    theta3=theta4=0;
}
MatrixXd Solver::getXY(double a,double b,double l1,double l2){
    MatrixXd T(3,3),P(3,1),A(3,1);
    double ang1,ang2,arm1,arm2;
    ang1=a;
    ang2=b;
    arm1=l1;
    arm2=l2;
    T(0,0)=cos(ang1+ang2);
    T(0,1)=-sin(ang1+ang2);
    T(0,2)=arm2*cos(ang1+ang2)+arm1*cos(ang1);
    T(1,0)=sin(ang1+ang2);
    T(1,1)=cos(ang1+ang2);
    T(1,2)=arm2*sin(ang1+ang2)+arm1*sin(ang1); 
    T(2,0)=T(2,1)=0;
    T(2,2)=1;
    P(0,0)=WF1.getX();
    P(1,0)=WF1.getY();
    P(2,0)=1;
    A=T*P;
    return A;
}
double Solver::getdeg1(double c,double d,double l1,double l2){
    double theta34;
    double arm1,arm2,l;
    x=c;
    y=d;
    l=abs(sqrt(x*x+y*y));
    arm1=l1;
    arm2=l2;
    theta34=atan2(y,x)/pi*180;
    theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
    theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
    return theta3;  
}
double Solver::getdeg2(double c,double d,double l1,double l2){
    double theta34;
    double arm1,arm2,l;
    x=c;
    y=d;
    l=abs(sqrt(x*x+y*y));
    arm1=l1;
    arm2=l2;
    theta34=atan2(y,x)/pi*180;
    theta3=theta34+acos((-arm2*arm2+arm1*arm1+l*l)/(2*arm1*l))/pi*180;
    theta4=acos((-l*l+arm1*arm1+arm2*arm2)/(2*arm1*arm2))/pi*180+180;
    return theta4;  
}

class Robot{
private:
    double theta1;
    double theta2;
    double seg1;
    double seg2;
    jointframe JF;
    worldframe WF;
    std::vector<taskframe> TF;
    Solver normal;
    Solver inverse;
public:
    Robot();
    Robot(double alpha,double beta,double lenth1,double lenth2);
    void PTPMove(jointframe JF1,double X,double Y);
    void PTPMove(worldframe WF1,double X,double Y);
    void PTPMove(taskframe TF1,worldframe WF2,double X,double Y);
}; 
Robot::Robot(){
    theta1=180/pi*180;
    theta2=270/pi*180;
    seg1=0;seg2=0;
}
Robot::Robot(double alpha,double beta,double lenth1,double lenth2){
    if(alpha>180||alpha<100||beta<220||beta>300)
    {
        cout<<"初始角度错误，请重新输入。"<<endl; 
    }
    theta1=alpha/pi*180;
    theta2=beta/pi*180;
    seg1=lenth1;
    seg2=lenth2;
}
void Robot::PTPMove(jointframe JF1,double X,double Y){
    double x,y,c,d;
    MatrixXd B(3,1);
    c=X+JF1.getdeg1();
    d=Y+JF1.getdeg2();
    x=c*pi/180;
    y=d*pi/180;
    if(c>180||c<100||d<220||d>300)
    {
        cout<<"无法旋转到该位置"<<endl; 
    }
    else
    {
    B=normal.getXY(x,y,seg1,seg2);
    cout<<B(0,0)<<','<<B(1,0)<<endl;    
    }
    
}
void Robot::PTPMove(worldframe WF1,double X,double Y){
    double deg1,deg2,x,y;
    MatrixXd WFs(3,3),coord(3,1),ans(3,1);
    coord(0,0)=X;
    coord(1,0)=Y;
    coord(2,0)=1;
    WFs=WF1.getWF();
    ans=WFs*coord;
    x=ans(0,0);
    y=ans(1,0);
    deg1=inverse.getdeg1(x,y,seg1,seg2);
    deg2=inverse.getdeg2(x,y,seg1,seg2);
    if(deg1>180||deg1<100||deg2<220||deg2>300)
    {
        cout<<"无法旋转到该位置"<<endl;
    }
    else
    {    
         cout<<deg1<<','<<deg2<<endl;
    }
}
void Robot::PTPMove(taskframe TF1,worldframe WF2,double X,double Y){
    double deg1,deg2,x,y;
    MatrixXd WFs(3,3),TFs(3,3),coordt(3,1),anst(3,1);
    coordt(0,0)=X;
    coordt(1,0)=Y;
    coordt(2,0)=1;
    TFs=TF1.getTF();
    WFs=WF2.getWF();
    anst=WFs*TFs*coordt;
    x=anst(0,0);
    y=anst(1,0);
    deg1=inverse.getdeg1(x,y,seg1,seg2);
    deg2=inverse.getdeg2(x,y,seg1,seg2);
    if(deg1>180||deg1<100||deg2<220||deg2>300)
    {
        cout<<"无法旋转到该位置"<<endl;
    }
    else
    {
        cout<<deg1<<','<<deg2<<endl;
    }
}

int main()
{
    Robot myRobot(120,240,8,4);
    jointframe JF;
    worldframe WF;
    taskframe TF1(2,2,30),TF2(1,2,0),TF3(1,1,60);
    myRobot.PTPMove(JF,130,260);
    myRobot.PTPMove(WF,-2,9);
    myRobot.PTPMove(TF1,WF,-2,3);
    myRobot.PTPMove(TF2,WF,-1,7);
    myRobot.PTPMove(TF3,WF,-2,3);
    return 0;
}

//结果：