第二次作业

 #include <iostream>
#include <math.h>
#define PI 3.1415926
using namespace std;
  
class Point {
      
     private:
        Vector2d pos;
         
     public:
         Point();
         
         Point(Vector2d v);
         
         Vector2d getPoint();
         
         void setPoint(Vector2d v);        
 };
  
 
class Frame{
    private:
        Point origin;
        double degree;
    public:
        Frame(){}
        Frame(Point orig,double deg){
            origin=orig;
            degree=deg;
         }
        Point getPoint(){
            return origin;
        }
        double getDegree(){
            return degree;
        }
          
};
class Joint{
    private:
        double deg1;
        double deg2;
    public:
        Joint(){}
        Joint(int a,int b){
            deg1=a;
            deg2=b;
        }
        void setDeg1(double rad){
            deg1=rad*180/PI;
        }
        void setDeg2(double rad){
            deg2=rad*180/PI;
        }
        double getDeg1(){
            return deg1;
        }
        double getDeg2(){
            return deg2;
        }
          
};
  
  
class Solver{
    private:
        Joint joint;
        Frame frame;
    public:
        Point move(Point p1,Point p2){
            double tempx=p1.getX()+p2.getX();
            double tempy=p1.getY()+p2.getY();
            Point point(tempx,tempy);
            return point;
        }
        Point rotate(Point p,double deg){
            double tempx;
            double tempy;
            tempx=p.getX()*cos(PI*deg/180)-p.getY()*sin(PI*deg/180);
            tempy=p.getX()*sin(PI*deg/180)+p.getY()*cos(PI*deg/180);
            Point point(tempx,tempy);
            return point;
        }
          
        Point FrameStandardize(Frame fr,Point po){
            Point point1=rotate(po,fr.getDegree());
            Point point2=move(point1,fr.getPoint());
            return point1;
        }
        FrameToJoint(Point po,double arm1,double arm2){
            double len= sqrt(po.getX()*po.getX()+po.getY()*po.getY());
              
            if(len>=(arm1+arm2)||len<=abs(arm1-arm2)){
                cout<<"坐标超范围，机器人无法达到"<<endl;
            }else{
                double rad1=acos((arm1*arm1+len*len-arm2*arm2)/(2*arm1*len));
                double rad2=acos((arm1*arm1+arm2*arm2-len*len)/(2*arm1*arm2));
                double rad11=atan(po.getY()/po.getX());
                double rad22=PI;
                joint.setDeg1(rad1+rad11);
                joint.setDeg2(rad2+rad22);
                cout<<"关节1应转动角度为："<<joint.getDeg1()<<" 关节2应转动角度为："<<joint.getDeg2()<<endl;
            }
                      
        }
        FrameReturn(Frame fr){
        }
        JointToFrame(Point po){
              
        }
          
};
class Robot{
    private:
        double arm1;
        double arm2;
        double arm1Range;
        double arm2Range;
    public:
        Robot(){}
        Robot(double a,double b){
            arm1=a;
            arm2=b;
        }
        void PTPmove(Frame fr,Point po){
            Solver solver;
            Point point;
              
            point=solver.FrameStandardize(fr,po);
            solver.FrameToJoint(point,arm1,arm2);
                  
        }
};
   
int main(){
    Robot myRobot(4,3);  
      
    Point origin(0,0);
    Point origin1(2,3);
    Point origin2(6,1);
    Point origin3(5,9);  
      
    Point point1(-5,1);
    Point point2(-3,-1);
    Point point3(4,2);
    Point point4(3,7);  
      
    Frame WF(origin,0);
    Frame TF1(origin1,30);
    Frame TF2(origin2,60);
    Frame TF3(origin3,90);
      
    myRobot.PTPmove(WF,point1);
    myRobot.PTPmove(TF1,point2);
    myRobot.PTPmove(TF2,point3);
    myRobot.PTPmove(TF3,point4);
      
      
    return 0;
}

应用到的类：

Point类用来存储坐标系原点、运动目标点

JointFrame关节坐标系。

Frame世界坐标系和任务坐标系

Solver完成对任务坐标系到关节坐标系的转换

robot机器人类中定义每个机械臂的长度，每个关节的转动角度范围和零点

感觉程序还有好多地方不太对，还需要再思考