第二次编程作业

1.建立点类
 1   #ifndef _POINT_H_
 2   #define _POINT_H_
 3   
 4  class Point{
 5   public:
 6       double x;
 7       double y;
 8   public:    
 9      Point(double a=0,double b=0);
10      void updatePoint(double a,double b); 
11  };
12  
13  #endif
Point.h
 1 #include"Point.h" 
 2   #include<iostream>
 3   
 4   Point::Point(double a,double b){
 5      this->x=a;
 6      this->y=b;
 7  }
 8 
 9  void Point::updatePoint(double a,double b){
10      this->x=a;
11      this->y=b;    
12  }
Point.c
2.建立坐标系类
 1  #ifndef _FRAME_H_
 2  #define _FRAME_H_
 3   
 4  #include<Eigen/Dense>
 5  #include<cmath>
 6  #include"Point.h"
 7  using namespace Eigen;
 8  
 9  class Frame{
10      protected:
11          Point OrigialPoint,AimPoint;
12          double OrigialAngal;
13      public:
14          Frame(){
15              OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
16          }
17          Frame(Point OrigialPoint0,double OrigialAngal0=0){
18              this->OrigialPoint=OrigialPoint0;
19              this->OrigialAngal=OrigialAngal0;
20          }
21          void UpdateMatrix(Point Zero){
22              AimPoint=Zero;
23          }
24          Point To_WF(Point twpoint);
25  };
26   
27   
28  class WorldFrame:public Frame{
29      public:
30          WorldFrame(){
31              OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
32          }
33          WorldFrame(Point OrigialPoint0,double OrigialAngal0=0);
34          ~WorldFrame(){
35          }
36  
37  };
38  
39  
40  
41  class JointFrame:public Frame{
42          public:
43          JointFrame(){
44              OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
45          }
46          JointFrame(Point OrigialPoint0,double OrigialAngal0=0);
47          ~JointFrame(){
48          }
49          
50 
51  };
52  
53  
54  
55  class TaskFrame:public Frame{
56          public:
57          TaskFrame(){
58              OrigialPoint.x=0;OrigialPoint.y=0;OrigialAngal=0.0;
59          }
60          TaskFrame(Point OrigialPoint0,double OrigialAngal0=0);
61          ~TaskFrame(){
62          }
63 
64   
65  };
66  
67  
68  #endif
Frame.h
 1  #include"Frame.h"
 2  
 3  Point Frame::To_WF(Point twpoint){
 4       Matrix2d RotateMatrix;
 5       double OrigialAngal0=OrigialAngal/180*M_PI;
 6       Point wfpoint; 
 7       RotateMatrix<< cos(OrigialAngal0),sin(OrigialAngal0),
 8             -sin(OrigialAngal0),cos(OrigialAngal0);
 9       Vector2d v1(twpoint.x,twpoint.y),v2(OrigialPoint.x,OrigialPoint.y),v3;
10       v3=RotateMatrix*v1+v2;
11       wfpoint.updatePoint(v3(0),v3(1));
12       return wfpoint;
13  }
14  
15  WorldFrame::WorldFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
16              this->OrigialPoint=OrigialPoint0;
17              this->OrigialAngal=OrigialAngal0;
18          }
19          
20 JointFrame::JointFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
21              this->OrigialPoint=OrigialPoint0;
22              this->OrigialAngal=OrigialAngal0;
23          }
24         
25 TaskFrame::TaskFrame(Point OrigialPoint0,double OrigialAngal0):Frame(OrigialPoint0,OrigialAngal0){
26              this->OrigialPoint=OrigialPoint0;
27              this->OrigialAngal=OrigialAngal0;
28          }
Frame.c
3.Solver解释器
 1 #ifndef _SOLVER_H_
 2 #define _SOLVER_H_ 
 3 #include<math.h>
 4 #include"Point.h"
 5 #include"Frame.h"
 6 
 7 class Solver{
 8     private:
 9         Point SolverResult;
10     public:
11         Solver();
12         Point PositiveKinematics(double L1,double L2,Point Angles);
13         Point NegativeKinematics(double L1,double L2,Point point);
14         Point FrameToWF(WorldFrame frame,Point AimPoint);
15         Point FrameToWF(JointFrame frame,Point AimPoint);
16         Point FrameToWF(TaskFrame frame,Point AimPoint);
17         
18     };
19 #endif
Solver.h
 1 #include"Solver.h"
 2 #include"Point.h"
 3 #include<cmath>
 4 
 5 Solver::Solver(){
 6     SolverResult.x=0;
 7     SolverResult.y=0;
 8 }
 9 
10 Point Solver::NegativeKinematics(double L1,double L2,Point point){
11     SolverResult.x=0;
12     SolverResult.y=0;
13     SolverResult.updatePoint(SolverResult.x,SolverResult.y);
14     return SolverResult;
15 }
16 
17 Point Solver::PositiveKinematics(double L1,double L2,Point Angles){
18     SolverResult.x=L1 *cos(Angles.x/180*M_PI)+L2 *cos(Angles.y/180*M_PI);
19     SolverResult.y=L1 *sin(Angles.x/180*M_PI)+L2 *sin(Angles.y/180*M_PI);
20     SolverResult.updatePoint(SolverResult.x,SolverResult.y);
21     return SolverResult;
22 }
23 
24 
25 
26 Point Solver::FrameToWF(WorldFrame frame,Point AimPoint){
27     return frame.To_WF(AimPoint);}
28 Point Solver::FrameToWF(JointFrame frame,Point AimPoint){
29     return frame.To_WF(AimPoint);}
30 Point Solver::FrameToWF(TaskFrame frame,Point AimPoint){
31     return frame.To_WF(AimPoint);}
Solver.c
4，机器人类
 1 #ifndef _ROBOT_H_
 2 #define _ROBOT_H_ 
 3 #include"Solver.h"
 4 //#include"Frame.h"
 5 #include <vector>
 6 
 7 class Robot{
 8     private:
 9         double JointAngle1,JointAngle2,L1,L2;
10         Point InsightPoint;
11         WorldFrame WF0;
12         JointFrame JF0;
13         TaskFrame  TF0;
14         Solver solver;
15         std::vector<TaskFrame> TaskFrameVec;
16     public:
17         Robot(double length1,double length2){
18             L1=length1;L2=length2;
19         }
20         void PTPmove(WorldFrame f,Point p);
21         void PTPmove(JointFrame f,Point p);
22         void PTPmove(TaskFrame f,Point p);
23         void addTFrame(TaskFrame frame);
24         void deleteFrame();
25     };
26 #endif
Robot.h
 1 #include<iostream>
 2 #include"Robot.h"
 3 using namespace std;
 4 
 5 void Robot::PTPmove( WorldFrame f,Point p){
 6     Point WorldframePoint=solver.FrameToWF(f,p);
 7     Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
 8     Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
 9     Point WorldframePoint1(0,30);
10     Point AngleMatrix1=solver.NegativeKinematics(L1,L2,WorldframePoint1);
11     InsightPoint.updatePoint(PKResult.x,PKResult.y);
12     cout<<"机械手两关节转角"<<"<"<<AngleMatrix1.x<<","<<AngleMatrix1.y<<">"<<endl;
13     cout<<"点的位置为"<<"<"<<InsightPoint.x<<"," <<InsightPoint.y<<">"<<endl;
14     
15 }
16 
17 void Robot::PTPmove(JointFrame f,Point p){
18     Point WorldframePoint=solver.FrameToWF(f,p);
19     Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
20     Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
21     InsightPoint.updatePoint(PKResult.x,PKResult.y);
22     cout<<"机械手两关节转角"<<"<"<<AngleMatrix.x<<","<<AngleMatrix.y<<">"<<endl;
23     cout<<"点的位置为"<<"<"<<InsightPoint.x<<"," <<InsightPoint.y<<">"<<endl;
24 }
25 
26 void Robot::PTPmove(TaskFrame f,Point p){
27     Point WorldframePoint=solver.FrameToWF(f,p);
28     Point AngleMatrix=solver.NegativeKinematics(L1,L2,WorldframePoint);
29     Point PKResult=solver.PositiveKinematics(L1,L2,AngleMatrix);
30     InsightPoint.updatePoint(PKResult.x,PKResult.y);
31     cout<<"机械手两关节转角"<<"<"<<AngleMatrix.x<<","<<AngleMatrix.y<<">"<<endl;
32     cout<<"点的位置为"<<"<"<<PKResult.x<<"," <<PKResult.y<<">"<<endl;
33     
34 }
35 
36 void Robot::addTFrame(TaskFrame frame){
37     TaskFrameVec.push_back(frame);
38 }
39 
40 void Robot::deleteFrame(){
41     TaskFrameVec.pop_back();
42 }
43 
44 
45
Robot.c
5 主函数
 1 #include <iostream>
 2 #include "Robot.h"
 3 
 4 int main(){
 5     Point one(5,20),two(0,30),three(0,20),a(0,0),c(10,0),d(5,0),e(10,0);
 6     Robot MyRobot(10,20);
 7     WorldFrame wframe(a,0);
 8     JointFrame jframe1(a,0);
 9     JointFrame jframe2(c,0);
10     TaskFrame tframe1(d,0);
11     TaskFrame tframe2(e,90);
12     MyRobot.PTPmove(tframe1,one);
13     MyRobot.PTPmove(wframe,two);
14     MyRobot.PTPmove(jframe1,three);
15 }
main.cpp

6 结果显示

小结：

1 坐标系类还有可以简化的余地；

2 运动学反解没有算出公式，显示的角度值都是0度；

3 机器人的转动角度范围没有确定；

4.建立坐标系类时用了继承，但感觉用的不好，或许没必要用；

posted @ 2015-12-11 20:57  kossle  阅读(182)  评论(0)  编辑  收藏  举报