Code for the Homework2

第二次作业，最近有点忙，一直没写，先发一下，关节角计算有点问题，后面抽时间改

 1 #include<iostream>
 2 #include <Eigen/Dense>
 3 #include "Robot.h"
 4 using namespace Eigen;
 5 using namespace std;
 6  int main(){
 7      const double l1=300,l2 =500;
 8      Vector2d JF_vx(1,0),JF_vy(0,1);
 9      Vector2d WF_vx(1,0),WF_vy(0,1);
10      POINT jf_origin("jf_origin",0,0),wf_origin("wf_origin",0,0);
11      Joint jt1(0,0,0,-180,180,0),jt2(l1,0,0,-180,180,0);
12      Frame JF("jf",JF_vx,JF_vy,jf_origin),WF("jf",WF_vx,WF_vy,wf_origin);
13      Robot myRobot(l1,l2,jt1,jt2,JF,WF);
14      POINT tf1_origin("tf1_origin",400,200),tf2_origin("tf2_origin",100,300),tf3_origin("tf3_origin",200,400);
15      Vector2d TF1_vx(0,1),TF1_vy(-1,0),TF2_vx(-1,0),TF2_vy(0,-1),TF3_vx(0,-1),TF3_vy(1,0);
16      Frame TF1("tf1",TF1_vx,TF1_vy,tf1_origin),TF2("tf2",TF2_vx,TF2_vy,tf2_origin),TF3("tf3",TF3_vx,TF3_vy,tf3_origin);
17      myRobot.TaskFrameCreate(TF1);
18      myRobot.TaskFrameCreate(TF2);
19      myRobot.TaskFrameCreate(TF3);
20      POINT P1(1,2),P2(1,2),P3(1,2),P4(1,2),P5(1,2);
21      myRobot.PTPMove(JF,P1);
22      myRobot.RobotShow();
23      myRobot.PTPMove(WF,P2);
24      myRobot.RobotShow();
25      myRobot.PTPMove(TF1,P3);
26      myRobot.RobotShow();
27      myRobot.PTPMove(TF2,P4);
28      myRobot.RobotShow();
29      myRobot.PTPMove(TF3,P5);
30      myRobot.RobotShow();
31      //cout<<180*atan2(sqrt(3),1)/PI<<endl;
32      return 0;
33  }

main.cpp

  1 #include<iostream>
  2 #include <Eigen/Dense>
  3 #include<vector>
  4 using namespace Eigen;
  5 using namespace std;
  6 #define PI 3.141592653
  7  class POINT
  8 {
  9     public:
 10         double x, y;
 11         string name;
 12         POINT(){
 13         };
 14         POINT(double xx,double yy){
 15             x=xx;
 16             y=yy;
 17         };
 18         POINT(string nam,double xx,double yy){
 19             name=nam;
 20             x=xx;
 21             y=yy;
 22         }
 23         POINT(const POINT &p){
 24             name=p.name;
 25             x=p.x;
 26             y=p.y;
 27         }
 28         POINT operator =(const POINT &pt)
 29         {
 30             POINT ptt(pt);
 31             return ptt;
 32         }
 33         void copyto(POINT &p);
 34         void get_cin_point(void);
 35         void display();
 36         void rotate(double &angle);
 37         void move(Vector2d &vec);
 38 };
 39 class Frame
 40 {
 41     public:
 42         string name;
 43         Vector2d vector_X;
 44         Vector2d vector_Y;
 45         POINT origin;
 46         Frame(){
 47         }
 48         Frame(string nam,Vector2d &vx,Vector2d &vy,POINT &oripoint)
 49         {
 50             name=nam;
 51             vector_X=vx;
 52             vector_Y=vy;
 53             //origin=oripoint;
 54             oripoint.copyto(origin);
 55         }
 56         Frame(const Frame &fr)
 57         {
 58             name=fr.name;
 59             vector_X=fr.vector_X;
 60             vector_Y=fr.vector_Y;
 61             origin=fr.origin;
 62         }
 63         Frame operator =(const Frame &fr)
 64         {
 65             Frame fra(fr);
 66             return fra;
 67         }
 68 };
 69 class Joint
 70 {
 71     public:
 72     double x,y,theta;
 73     double thetamin,thetamax,thetazero;
 74     Joint(){
 75     }
 76     Joint(double xx,double yy,double thetaa,double thetaminn,double thetamaxx,double thetazeroo)
 77     {
 78         x=xx;
 79         y=yy;
 80         theta=thetaa;
 81         thetamin=thetaminn;
 82         thetamax=thetamaxx;
 83         thetazero=thetazeroo;
 84     }
 85     CopyTo(Joint &jt)
 86     {
 87         jt.x=x;
 88         jt.y=y;
 89         jt.theta=theta;
 90         jt.thetamin=thetamin;
 91         jt.thetamax=thetamax;
 92         jt.thetazero=thetazero;
 93     }
 94 };
 95 class Robot
 96 {
 97    public:
 98         double length1,length2;
 99         Joint joint1,joint2;
100         Frame JointFrame,WorldFrame;
101         vector<Frame> fv;
102         Robot(){
103         }
104         Robot(double l1,double l2,Joint jt1,Joint jt2,Frame JF,Frame WF)
105         {
106               length1=l1;
107               length2=l2;
108             jt1.CopyTo(joint1);
109             jt2.CopyTo(joint2);
110             JointFrame=JF;
111             WorldFrame=WF;
112         }
113         void TaskFrameCreate(const Frame &tf);
114         void PTPMove(const Frame &fr,const POINT &pt);
115         void RobotShow(void);
116         void ToJoint();
117          void JointTo();
118  };
119 // class Solver
120 // {
121 //     public:
122 //         //friend void PTPMove(Frame &fr,POINT &pt);
123 //         void ToJoint(Robot &myrobot);
124 //         void JointTo(Robot &myrobot);
125 // };

Robot.h

 1 #include "Robot.h"
 2 #include "math.h"
 3 void Robot::TaskFrameCreate(const Frame &tf)
 4 {
 5     fv.push_back(tf);
 6 }
 7 void Robot::PTPMove(const Frame &fr,const POINT &pt)
 8 {
 9  double theta=atan2(fr.vector_X[1],fr.vector_X[0]);
10  joint2.x=fr.origin.x+pt.x*cos(theta)-pt.y*sin(theta);
11  joint2.y=fr.origin.y+pt.x*sin(theta)+pt.y*cos(theta);
12  //cout<<"aa="<<fr.origin.x<<endl;
13  ToJoint();
14 }
15 void Robot::ToJoint()
16 {
17    joint1.theta=acos(((pow(joint2.x,2)+pow(joint2.y,2))+(pow(length1,2)-pow(length2,2)))/(2*length1*sqrt(pow(joint2.x,2)+pow(joint2.y,2))))+atan2(joint2.y,joint2.x);
18    joint2.theta=acos(((pow(joint2.x,2)+pow(joint2.y,2))+(pow(length2,2)-pow(length1,2)))/(2*length2*sqrt(pow(joint2.x,2)+pow(joint2.y,2))))+atan2(joint2.y,joint2.x);
19 //   cout<<length1<<"  "<<length2<<endl;
20 //   cout<<joint2.x<<"  "<<joint2.y<<endl;
21 }
22
23 void Robot::JointTo()
24 {
25    joint2.x=length1*cos(joint1.theta)+length2*cos(joint2.theta);
26    joint2.x=length1*sin(joint1.theta)+length2*sin(joint2.theta);
27 }
28 void Robot::RobotShow(void){
29     cout<<"关节1转角"<< joint1.theta*180/PI;
30     cout<<"关节2转角"<< joint2.theta*180/PI;
31     cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl;
32 }
33 void POINT::copyto(POINT &p){
34     p.name=name;
35     p.x=x;
36     p.y=y;
37 }

Robot.cpp