实时控制软件设计第二次作业

代码如下：

Point.hpp

#ifndef _POINT_HPP_
#define _POINT_HPP_

#include <Eigen/Dense>

using namespace Eigen;

class Point {
    
    private:
        Vector2d pos;
        
    public:
        Point();
        
        Point(Vector2d v);
        
        Vector2d getPoint();
        
        void setPoint(Vector2d v);        
};

#endif

 

Point.cpp

#include "Point.hpp"

Point::Point() {};

Point::Point(Vector2d v) {
    pos = v;
}

Vector2d Point::getPoint() {
    return pos;
}

void Point::setPoint(Vector2d v) {
    pos = v;
}

 

Frame.hpp

#include "Eigen/Dense"
#include <iostream>
#include "Point.hpp"

using namespace std;

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;
        }
         
};

#endif 

 

JoinFrame.hpp

#ifndef _JOINTFRAME_HPP_
#define _JOINTFRAME_HPP_

#include "Frame.hpp"


class JointFrame {
    
    private:
        double jointDegree[2] ;
    
    public:
        JointFrame();
        JointFrame(double d1,double d2);
        void setDeg(double d1,double d2);
        double getDeg1();
        double getDeg2();
        
};        

#endif 

 

JointFrame.cpp

#include "JointFrame.hpp"

JointFrame::JointFrame() {
    jointDegree[0] = 0;
    jointDegree[1] = 0;
}

JointFrame::JointFrame(double d1,double d2) {
    jointDegree[0] = d1;
    jointDegree[1] = d2;
}

void JointFrame::setDeg(double d1,double d2) {
    jointDegree[0] = d1;
    jointDegree[1] = d2;
}

double JointFrame::getDeg1() {
    return jointDegree[0];
}

double JointFrame::getDeg2() {
    return jointDegree[1];
}

 

Solver.hpp

#ifndef _SOLVER_HPP_
#define _SOLVER_HPP_
#define PI 3.1415926

#include "Frame.hpp"
#include "JointFrame.hpp"
#include "Point.hpp"
#include "math.h"

class Solver {
    
    private: 
        JointFrame jointframe;
        Frame frame;
        
    public:
        Solver();
        Solver(JointFrame jf,Frame f);
        Point move(Point p1,Point p2);
        Point rotate(Point p,double deg);
        Point FrameToWF(Frame f,Point p);
        void FrameToJF(Point p,double arm1,double arm2);        
};

#endif 

 

Solver.cpp

#include "solver.hpp"

Solver::Solver(){};

Solver::Solver(JointFrame jf,Frame f) {
    jointframe = jf;
    frame = f;
}

Point Solver::move(Point p1,Point p2) {
    Vector2d v;
    v = p1.getPoint()+p2.getPoint();
    Point point(v);
    return point;
}

Point Solver::rotate(Point p,double deg) {
    Matrix2d m;
    Vector2d v;
    double rad = deg*PI/180;
    m(0,0) = cos(rad);
    m(0,1) = - sin(rad);
    m(1,0) = sin(rad);
    m(1,1) = cos(rad);
    v = m*p.getPoint();
    Point point(v);
    return point;
}

Point Solver::FrameToWF(Frame f,Point p) {
    Point p1 = rotate(p,f.getDegree());
    Point p2 = move(p1,f.getPoint());
    return p2;
}

void Solver::FrameToJF(Point p,double arm1,double arm2) {
    Vector2d v = p.getPoint();
    double l = sqrt(v[0]*v[0]+v[1]*v[1]);
    if(l>=(arm1+arm2)||l<=abs(arm1-arm2)) {
        cout<<"机械手无法到达该位置！"<<endl;
    } else {
        double rad1=acos((arm1*arm1+l*l-arm2*arm2)/(2*arm1*l));
        double rad2=acos((arm1*arm1+arm2*arm2-l*l)/(2*arm1*arm2));
        double rad3=atan(v[1]/v[0]);
        jointframe.setDeg(rad1+rad3,rad2+PI);
        cout<<"关节1转角为："<<jointframe.getDeg1()*180/PI<<'\t';
        cout<<"关节2转角为："<<jointframe.getDeg2()*180/PI<<endl;
    }    
}

 

Robot.hpp

#ifndef _ROBOT_HPP_
#define _ROBOT_HPP_

#include "Frame.hpp"
#include "Solver.hpp"
#include <vector>

class Robot {
    private:
        double armLength[2];
        vector<Frame> fv;
        
    public:
        Robot();
        Robot(double a1,double a2);
        void PTPmove(Frame f,Point p);
        void addFrame(Frame frame);
        void deleteFrame();
                
};


#endif 

 

Robot.cpp

#include "robot.hpp"

Robot::Robot() {};

Robot::Robot(double a1,double a2) {
    armLength[0] = a1;
    armLength[1] = a2;
}

void Robot::PTPmove(Frame f,Point p) {
    Solver solver;
    Point point;
    point = solver.FrameToWF(f,p);
    solver.FrameToJF(point,armLength[0],armLength[1]);
}

void Robot::addFrame(Frame frame) {
    fv.push_back(frame);
} 

void Robot::deleteFrame() {
    fv.pop_back();
}

 

main.cpp

#include <iostream>
#include "robot.hpp"
#include "Frame.hpp"


int main(int argc, char** argv) {
    
    Robot myRobot(5,7);   
    Vector2d v1(0,0);
    Vector2d v2(1,1);
    Vector2d v3(5,7);
    Vector2d v4(5,9);
    
    Vector2d p1(2,2);
    Vector2d p2(2,2);
    Vector2d p3(-1,3);
    Vector2d p4(-10,-8);    
    
     
    Point origin(v1);
    Point origin1(v2);
    Point origin2(v3);
    Point origin3(v4);   
     
    Point point1(p1);
    Point point2(p2);
    Point point3(p3);
    Point point4(p4);   
     
    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;
}

 

程序解释：

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

2、JointFrame，关节坐标系。

3、Frame，世界坐标系和任务坐标系

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

5、robot，机器人类。

 

程序不足：

1、其实这个程序完成得不是很好，我的本意其实是想建立一个虚类Frame，然后用WorldFrame，TaskFrame，JointFrame作为其子类继承，这样在Solver类里面就可以利用多态，直接传一个父类Frame的引用进去。但是实现的时候我发现，这种多态的方式，实际上主要是要用虚函数，然后子类进行重写。但是我这里的要求是把其他坐标系全部转换为JointFrame，然后我就没办法在父类写一个虚函数来抽象出这个功能。所以就折中选择了两个类，Frame和JointFrame来完成这个功能。但是到现在我还是觉得坐标系就是应该作为一个整体Frame，应该包含JointFrame。之后会对抽象类和继承进行进一步的学习。

2、点类这个设定也不太合理，这里我用了点类将robot和frame联系起来了，但是实际上并不是这样，不论是robot还是frame都不应该跟点类扯上关系，点类应该是封装在他们内部的一些属性。通过这次作业我发现我对面向对象的概念实际上还不是很理解，虽然听上去面向对象很简单，但是对实际问题的对象化抽象，我觉得很有难度，需要多思考，多练习才能掌握。

3、总之，这次的程序只是看上去实现了功能，其实有很多地方都没能达到期望。多思考吧~