第二次编程

#ifndef POINT
#define POINT
class Point{
    public:
        double x;
        double y;
        
        Point(double a=0,double b=0);
        void SetP(double a,double b);
        void PrintP();
        
};
#endif

#include "Point.h"
#include <iostream>
using namespace std;

Point::Point(double a,double b){
    x=a;
    y=b;
}
void Point::SetP(double a,double b){
    x=a;
    y=b;
}
void Point::PrintP(){
    cout<<'('<<x<<','<<y<<')'<<endl;
}

#ifndef FRAME
#define FRAME
class WorldFrame{
    
};
class TaskFrame{
    public:
        double x;
        double y;
        double deg;
    
        TaskFrame(double a=0,double b=0,double d=0);
};
class JointFrame{

};
#endif

#include "Frame.h"
TaskFrame::TaskFrame(double a,double b,double d){
    x=a;
    y=b;
    deg=d;
}

#ifndef SOLVER
#define SOLVER
#include "Frame.h"
#include "Point.h"
class Solver{
    public:
        void WtoT(TaskFrame t,Point &p);
        void TtoW(TaskFrame t,Point &p);
        void TtoJ(Point &p,double a1,double a2);
        void JtoT(Point &p,double a1,double a2);
};
#endif

#include "Solver.h"
#include<Eigen/Dense>
using namespace Eigen;
#define PI 3.1415926
void Solver::WtoT(TaskFrame t,Point &p){
    MatrixXd m(3,3);
    MatrixXd pt(1,3);
    m(0,0)=cos(t.deg*PI/180);
    m(0,1)=sin(t.deg*PI/180);
    m(0,2)=0;
    m(1,0)=-sin(t.deg*PI/180);
    m(1,1)=cos(t.deg*PI/180);
    m(1,1)=0;
    m(2,0)=t.x,
    m(2,1)=t.y;
    m(2,2)=0;
    pt(0,0)=p.x;
    pt(0,1)=p.y;
    pt(0,2)=1;
    pt*=m;
    p.x=pt(0,0);
    p.y=pt(0,1);    
}
void Solver::TtoW(TaskFrame t,Point &p){
    MatrixXd m(3,3);
    MatrixXd pt(1,3);
    m(0,0)=cos(-t.deg*PI/180);
    m(0,1)=sin(-t.deg*PI/180);
    m(0,2)=0;
    m(1,0)=-sin(-t.deg*PI/180);
    m(1,1)=cos(-t.deg*PI/180);
    m(1,1)=0;
    m(2,0)=t.x;
    m(2,1)=t.y;
    m(2,2)=0;
    pt(0,0)=-p.x;
    pt(0,1)=-p.y;
    pt(0,2)=1;
    pt*=m;
    p.x=pt(0,0);
    p.y=pt(0,1);
}
void Solver::TtoJ(Point &p,double a1,double a2){
    double l,deg1,deg2,deg3;
    l=sqrt(p.x*p.x+p.y*p.y);
    deg1=atan(p.y/p.x);
    deg2=acos((a1*a1+l*l-a2*a2)/(2*a1*l));
    deg3=acos((a1*a1+a2*a2-l*l)/(2*a1*a2));
    p.x=(deg1+deg2)*180/PI;
    p.y=deg3*180/PI+180;
}
void Solver::JtoT(Point &p,double a1,double a2){
    p.x=a1*cos(p.x)+a2*cos(p.y);
    p.y=a1*sin(p.x)+a2*sin(p.y);
}

#ifndef ROBOT
#define ROBOT
#include "Solver.h"
class Robot{
    private:
        double arm1,arm2,deg1min,deg2min,deg1max,deg2max;
    public:
        Robot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
        void SetRobot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
        void PTPMove(WorldFrame wf,TaskFrame tf,Point p);
        void PTPMove(TaskFrame tf,Point P);
        void PTPMove(JointFrame jf,Point P);
        void print(Point &p);
};
#endif

#include "Robot.h"
#include <iostream>
using namespace std;
Robot::Robot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
    arm1=a1;
    arm2=a2;
    deg1min=d1min;
    deg2min=d2min;
    deg1max=d1max;
    deg2max=d2max;
}
void Robot::SetRobot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
    arm1=a1;
    arm2=a2;
    deg1min=d1min;
    deg2min=d2min;
    deg1max=d1max;
    deg2max=d2max;
}
void Robot::PTPMove(WorldFrame wf,TaskFrame tf,Point p){
    Solver s;
    s.WtoT(tf,p);
    s.TtoJ(p,arm1,arm2);
    print(p);
}
void Robot::PTPMove(TaskFrame tf,Point p){
    Solver s;
    s.TtoJ(p,arm1,arm2);
    print(p);
}
void Robot::PTPMove(JointFrame jf,Point p){
    print(p);
}
void Robot::print(Point &p){
    if(p.x>=deg1min||p.y<=deg1max){
        cout<<"关节坐标为：("<<p.x<<','<<p.y<<')'<<endl;
    }
    else cout<<"无法旋转到该位置"<<endl; 
}

#include "Robot.h"
/* run this program using the console pauser or add your own getch, system("pause") or input loop */

int main(int argc, char** argv) {
    Robot myRobot(8,8);
    WorldFrame WF;
    TaskFrame TF1(0,0,0),TF2(1,2,30),TF3(3,5,90);
    JointFrame JF;
    Point P1(0,0),P2(1,2),P3(2,2),P4(3,5),P5(0,2);
    myRobot.PTPMove(JF,P1);
    myRobot.PTPMove(WF,TF1,P2);
    myRobot.PTPMove(TF1,P3);
    myRobot.PTPMove(TF2,P4);
    myRobot.PTPMove(TF3,P5);
    return 0;

}

本次程序参考了张杭峰同学的代码，对其进行了一些修改，把所有的类都分块化。但是缺点在于运动反解只限于第一象限坐标使用。