第二次编程作业-咖啡角机器人

程序的结构模块有：

1.Point类。

#ifndef _POINT_H_
#define _POINT_H_


class Point{
public:
    double x;
    double y;
public:    
    Point();
    Point(double a,const double b);
    ~Point();
    void updatePoint(double a,double b); 
};


#endif

#include"Point.h" 
#include<iostream>

Point::Point() {
    this->x = 0;
    this->y = 0;
}

Point::Point(double a,double b){
    this->x=a;
    this->y=b;
}

Point::~Point() {
   // std::cout << "调用Point析构函数" << std::endl;
}


void Point::updatePoint(double a,double b){
    this->x=a;
    this->y=b;    
}

2.Frame类，主要定义了3中坐标系，并完成了任务坐标系、关节坐标系与世界坐标系之间的相互转化。

#ifndef _FRAME_H_
#define _FRAME_H_


#include<Eigen/Dense>
#include<cmath>
#include"Point.h"
using namespace Eigen;

#define PI 3.14 


class WorldFrame{
private:
    Point zeropoint;
public:
    //WorldFrame();
    //~WorldFrame();
    void updatezeropoint(Point Zero){
        this->zeropoint=Zero;
    }
    
};



class JointFrame{
private:
    Point angle_2;
public:
    //JointFrame();
    //JointFrame(const Point& Zero){
    //    this->angle_2=Zero;    
    //}
    //~JointFrame();
    void updateangle_2(Point Zero){
        this->angle_2=Zero;
    }
    
};



class TaskFrame{
private:
    Point location_2;
    double deflection_angle;
public:
    // TaskFrame();
    /************
    TaskFrame(Point point, double angle){
        this->location_2=point;
        this->deflection_angle=angle;
    }
    ******************/
    //~TaskFrame(){};
    void updateTaskFrame(Point point, double angle){
        this->location_2=point;
        this->deflection_angle=angle;
    } 
    Point TF_to_WF(Point tfpoint); //将工件坐标系的坐标转换到世界坐标系 
};


#endif

#include"Frame.h"

Point TaskFrame::TF_to_WF(Point tfpoint){
    Matrix2d Rot;
    double angle=deflection_angle/180*PI;
    Point wfpoint; 
    Rot<< cos(angle),sin(angle),
          -sin(angle),cos(angle);
    Vector2d v1(tfpoint.x,tfpoint.y),v2(location_2.x,location_2.y),v3;
    v3=Rot*v1+v2;
    
    wfpoint.updatePoint(v3(0),v3(1));
    return wfpoint;
}

3.Slover类，完成实现机器人的正逆运动学变换，正运动学为把机器人的关节坐标变换成笛卡尔坐标（已完成，完成效果理想），逆运动学为把机器人的笛卡尔坐标变换成关节坐标（采用了几何法，运用余弦定理进行求解，算法还应该考虑象限的问题）。

#ifndef _SLOVER_H_
#define _SLOVER_H_ 


#include<math.h>
#include"Point.h"

#define PI 3.14 

class Slover{
private:
    Point pi;
public:
    Point forwordtrans(double L1,double L2,Point angle_2);
    Point inversetrans(double L1,double L2,Point location_2);
};



#endif

#include"Slover.h"
#include<cmath> 
#define PI 3.14

 Point Slover::forwordtrans(double L1,double L2,Point angle_2){
    this->pi=angle_2;
    Point location_2;
    double xi,yi;
    xi=L1 *sin(pi.x/180*PI)+L2 *sin(pi.y/180*PI);
    yi=L1 *cos(pi.x/180*PI)+L2 *cos(pi.y/180*PI);
    location_2.updatePoint(xi,yi);
    return location_2;      
}

 Point Slover::inversetrans(double l1,double l2,Point location_2){
    this->pi=location_2;
    Point angle_2;
    double xi,yi;
    //****机器人逆向求解算法,利用几何法求解，将坐标点与原点连接，与机械臂1、2构成三角形，用余弦定理求解*********//
    double x=location_2.x,   y=location_2.y;
    double l3=sqrt(x*x+y*y);
    yi=(acos((l1*l1+l2*l2-l3*l3)/(2*l1*l2)))/PI*180;
    xi=(atan(y/x)+acos((l1*l1+l3*l3-l2*l2)/(2*l1*l3)))/PI*180;
    //********************************************************************************************************//
    angle_2.updatePoint(xi,yi);
    return angle_2;
}

4.机器人类

#ifndef _ROBOT_H_
#define _ROBOT_H_ 


#include"Slover.h"
#include"Frame.h"

class Robot{    
public:
    double leglth1,leglth2;
    double ja1max,ja1zero,ja1min;
    double ja2max,ja2zero,ja2min;
private:
    double JointAngle1,JointAngle2;
    WorldFrame WF0;
    JointFrame JF0;
    TaskFrame  TF0;    
    Point Poi0;
public:
    //Robot();
    /**************************
    Robot(const double& ll1,const double& ll2,
          const double& Ja1max,const double& Ja1zero,const double& Ja1min,  
          const double& Ja2max,const double& Ja2zero,const double& Ja2min){
           leglth1=ll1;    leglth2=ll2;
           ja1max=Ja1max;  ja1zero=Ja1zero;   ja1min=Ja1min;
           ja2max=Ja2max;  ja2zero=Ja2zero;   ja2min=Ja2min; 
          };
          ***********************/ 
    initRobot( double ll1,double ll2,
          double Ja1max,double Ja1zero,double Ja1min,  
          double Ja2max,double Ja2zero,double Ja2min){
           leglth1=ll1;    leglth2=ll2;
           ja1max=Ja1max;  ja1zero=Ja1zero;   ja1min=Ja1min;
           ja2max=Ja2max;  ja2zero=Ja2zero;   ja2min=Ja2min; 
          }
    void PTPMove( WorldFrame wf,Point point);
    void PTPMove(JointFrame jf,Point point);
    void PTPMove(TaskFrame tf,Point point); 
};



#endif    

#include<iostream>
#include"Robot.h"

void Robot::PTPMove( WorldFrame wf,Point point){
    this->WF0=wf;
    this->Poi0=point;
    Slover sl;
    Point  angle_2;
    angle_2=sl.inversetrans(this->leglth1,this->leglth2,Poi0);
    std::cout<<"完成世界坐标系下的该任务，机器人的两个关节角度是："<<"["<<angle_2.x<<","<<angle_2.y<<"]"<<std::endl;    
    } 
    
void Robot::PTPMove(JointFrame jf,Point point){
    this->Poi0=point;
    std::cout<<"完成关节坐标系下的该任务，机器人的两个关节角度是："<<"["<<point.x<<","<<point.y<<"]"<<";";
    Slover sl;
    Point  location_2;
    location_2=sl.forwordtrans(this->leglth1,this->leglth2,Poi0);
    std::cout<<"此时机器人末端世界坐标为："<<"["<<location_2.x<<","<<location_2.y<<"]"<<std::endl;
}

void Robot::PTPMove(TaskFrame tf,Point point){
    this->TF0=tf;
    this->Poi0=point;
    Point WFpoint;
    WFpoint=TF0.TF_to_WF(Poi0);
    std::cout<<"该任务坐标系下的点，在世界坐标系的坐标为："<<"["<<WFpoint.x<<","<<WFpoint.y<<"]"<<";";
    Slover sl;
    Point  angle_2;
    angle_2=sl.inversetrans(this->leglth1,this->leglth2,point);
    std::cout<<"完成该任务，机器人的两个关节角度是："<<"["<<angle_2.x<<","<<angle_2.y<<"]"<<std::endl;        
}
    
    

 

 5.主程序：主程序新建了一个机器人对象，并对机器人机械参数进行了赋值，同时定义了几个任务坐标系、世界坐标系和工件坐标系，调用Robot类的PTPMove函数，进行了程序验证。

#include<iostream>
#include<vector>
#include"Robot.h"

using namespace std;

 
int main(){
    
    Robot myRobot;
    Point p0(0,0),p1(45,50),p2(8,15),p3(15,8),p4(9,12),p5(10,6); 
    
    WorldFrame WF1;
    WF1.updatezeropoint(p0);
    
    
    TaskFrame TF1,TF2,TF3;
    
    Point tf_location1(2,2),tf_location2(3,2),tf_location3(2,3); //这三个点坐标为任务坐标系与世界坐标系的偏移向量 
    
    TF1.updateTaskFrame(tf_location1,45);
    TF1.updateTaskFrame(tf_location2,60);
    TF1.updateTaskFrame(tf_location3,90);
    
    
    JointFrame JF;
    

    myRobot.initRobot(10,10,180,0,0,360,0,0);
    
    myRobot.PTPMove(JF,p1);
    myRobot.PTPMove(WF1,p2);
    myRobot.PTPMove(TF1,p3);
    myRobot.PTPMove(TF2,p4);
    myRobot.PTPMove(TF3,p5);

    return 0;
 
}

最总得到的结果为：

总结：

程序的不足：

1.部分类，为了使用方便，将成员变量定义为了“public”，未体现封装性；

2.逆向运算算法部分，采用几何法，运用余弦定理进行求解，要考虑坐标点象限问题，对结果进行修正；同时采用这种方法，存在一定局限性，无法扩展到高维度；