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

时间：2015-12-09 23:19:02 阅读：326 评论：0 收藏：0 [点我收藏+]

标签：

程序的结构模块有：

1.Point类。

 1 #ifndef _POINT_H_
 2 #define _POINT_H_
 3 
 4 
 5 class Point{
 6 public:
 7     double x;
 8     double y;
 9 public:    
10     Point();
11     Point(double a,const double b);
12     ~Point();
13     void updatePoint(double a,double b); 
14 };
15 
16 
17 #endif

Point.h

 1 #include"Point.h" 
 2 #include<iostream>
 3 
 4 Point::Point() {
 5     this->x = 0;
 6     this->y = 0;
 7 }
 8 
 9 Point::Point(double a,double b){
10     this->x=a;
11     this->y=b;
12 }
13 
14 Point::~Point() {
15    // std::cout << "调用Point析构函数" << std::endl;
16 }
17 
18 
19 void Point::updatePoint(double a,double b){
20     this->x=a;
21     this->y=b;    
22 }

Point.c

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

 1 #include<iostream>
 2 #include<vector>
 3 #include"Robot.h"
 4 
 5 using namespace std;
 6 
 7  
 8 int main(){
 9     
10     Robot myRobot;
11     Point p0(0,0),p1(45,50),p2(8,15),p3(15,8),p4(9,12),p5(10,6); 
12     
13     WorldFrame WF1;
14     WF1.updatezeropoint(p0);
15     
16     
17     TaskFrame TF1,TF2,TF3;
18     
19     Point tf_location1(2,2),tf_location2(3,2),tf_location3(2,3); //这三个点坐标为任务坐标系与世界坐标系的偏移向量 
20     
21     TF1.updateTaskFrame(tf_location1,45);
22     TF1.updateTaskFrame(tf_location2,60);
23     TF1.updateTaskFrame(tf_location3,90);
24     
25     
26     JointFrame JF;
27     
28 
29     myRobot.initRobot(10,10,180,0,0,360,0,0);
30     
31     myRobot.PTPMove(JF,p1);
32     myRobot.PTPMove(WF1,p2);
33     myRobot.PTPMove(TF1,p3);
34     myRobot.PTPMove(TF2,p4);
35     myRobot.PTPMove(TF3,p5);
36 
37     return 0;
38  
39 }

Main.cpp

 1 #ifndef _FRAME_H_
 2 #define _FRAME_H_
 3 
 4 
 5 #include<Eigen/Dense>
 6 #include<cmath>
 7 #include"Point.h"
 8 using namespace Eigen;
 9 
10 #define PI 3.14 
11 
12 
13 class WorldFrame{
14 private:
15     Point zeropoint;
16 public:
17     //WorldFrame();
18     //~WorldFrame();
19     void updatezeropoint(Point Zero){
20         this->zeropoint=Zero;
21     }
22     
23 };
24 
25 
26 
27 class JointFrame{
28 private:
29     Point angle_2;
30 public:
31     //JointFrame();
32     //JointFrame(const Point& Zero){
33     //    this->angle_2=Zero;    
34     //}
35     //~JointFrame();
36     void updateangle_2(Point Zero){
37         this->angle_2=Zero;
38     }
39     
40 };
41 
42 
43 
44 class TaskFrame{
45 private:
46     Point location_2;
47     double deflection_angle;
48 public:
49     // TaskFrame();
50     /************
51     TaskFrame(Point point, double angle){
52         this->location_2=point;
53         this->deflection_angle=angle;
54     }
55     ******************/
56     //~TaskFrame(){};
57     void updateTaskFrame(Point point, double angle){
58         this->location_2=point;
59         this->deflection_angle=angle;
60     } 
61     Point TF_to_WF(Point tfpoint); //将工件坐标系的坐标转换到世界坐标系 
62 };
63 
64 
65 #endif

Frame.h

 1 #include"Frame.h"
 2 
 3 Point TaskFrame::TF_to_WF(Point tfpoint){
 4     Matrix2d Rot;
 5     double angle=deflection_angle/180*PI;
 6     Point wfpoint; 
 7     Rot<< cos(angle),sin(angle),
 8           -sin(angle),cos(angle);
 9     Vector2d v1(tfpoint.x,tfpoint.y),v2(location_2.x,location_2.y),v3;
10     v3=Rot*v1+v2;
11     
12     wfpoint.updatePoint(v3(0),v3(1));
13     return wfpoint;
14 }

Frame.c

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

 1 #ifndef _SLOVER_H_
 2 #define _SLOVER_H_ 
 3 
 4 
 5 #include<math.h>
 6 #include"Point.h"
 7 
 8 #define PI 3.14 
 9 
10 class Slover{
11 private:
12     Point pi;
13 public:
14     Point forwordtrans(double L1,double L2,Point angle_2);
15     Point inversetrans(double L1,double L2,Point location_2);
16 };
17 
18 
19 
20 #endif

Slover.h

 1 #include"Slover.h"
 2 #include<cmath> 
 3 #define PI 3.14
 4 
 5  Point Slover::forwordtrans(double L1,double L2,Point angle_2){
 6     this->pi=angle_2;
 7     Point location_2;
 8     double xi,yi;
 9     xi=L1 *sin(pi.x/180*PI)+L2 *sin(pi.y/180*PI);
10     yi=L1 *cos(pi.x/180*PI)+L2 *cos(pi.y/180*PI);
11     location_2.updatePoint(xi,yi);
12     return location_2;      
13 }
14 
15  Point Slover::inversetrans(double l1,double l2,Point location_2){
16     this->pi=location_2;
17     Point angle_2;
18     double xi,yi;
19     //****机器人逆向求解算法,利用几何法求解，将坐标点与原点连接，与机械臂1、2构成三角形，用余弦定理求解*********//
20     double x=location_2.x,   y=location_2.y;
21     double l3=sqrt(x*x+y*y);
22     yi=(acos((l1*l1+l2*l2-l3*l3)/(2*l1*l2)))/PI*180;
23     xi=(atan(y/x)+acos((l1*l1+l3*l3-l2*l2)/(2*l1*l3)))/PI*180;
24     //********************************************************************************************************//
25     angle_2.updatePoint(xi,yi);
26     return angle_2;
27 }

Slover.c

4.机器人类

 1 #ifndef _ROBOT_H_
 2 #define _ROBOT_H_ 
 3 
 4 
 5 #include"Slover.h"
 6 #include"Frame.h"
 7 
 8 class Robot{    
 9 public:
10     double leglth1,leglth2;
11     double ja1max,ja1zero,ja1min;
12     double ja2max,ja2zero,ja2min;
13 private:
14     double JointAngle1,JointAngle2;
15     WorldFrame WF0;
16     JointFrame JF0;
17     TaskFrame  TF0;    
18     Point Poi0;
19 public:
20     //Robot();
21     /**************************
22     Robot(const double& ll1,const double& ll2,
23           const double& Ja1max,const double& Ja1zero,const double& Ja1min,  
24           const double& Ja2max,const double& Ja2zero,const double& Ja2min){
25            leglth1=ll1;    leglth2=ll2;
26            ja1max=Ja1max;  ja1zero=Ja1zero;   ja1min=Ja1min;
27            ja2max=Ja2max;  ja2zero=Ja2zero;   ja2min=Ja2min; 
28           };
29           ***********************/ 
30     initRobot( double ll1,double ll2,
31           double Ja1max,double Ja1zero,double Ja1min,  
32           double Ja2max,double Ja2zero,double Ja2min){
33            leglth1=ll1;    leglth2=ll2;
34            ja1max=Ja1max;  ja1zero=Ja1zero;   ja1min=Ja1min;
35            ja2max=Ja2max;  ja2zero=Ja2zero;   ja2min=Ja2min; 
36           }
37     void PTPMove( WorldFrame wf,Point point);
38     void PTPMove(JointFrame jf,Point point);
39     void PTPMove(TaskFrame tf,Point point); 
40 };
41 
42 
43 
44 #endif

Robot.h

 1 #include<iostream>
 2 #include"Robot.h"
 3 
 4 void Robot::PTPMove( WorldFrame wf,Point point){
 5     this->WF0=wf;
 6     this->Poi0=point;
 7     Slover sl;
 8     Point  angle_2;
 9     angle_2=sl.inversetrans(this->leglth1,this->leglth2,Poi0);
10     std::cout<<"完成世界坐标系下的该任务，机器人的两个关节角度是："<<"["<<angle_2.x<<","<<angle_2.y<<"]"<<std::endl;    
11     } 
12     
13 void Robot::PTPMove(JointFrame jf,Point point){
14     this->Poi0=point;
15     std::cout<<"完成关节坐标系下的该任务，机器人的两个关节角度是："<<"["<<point.x<<","<<point.y<<"]"<<";";
16     Slover sl;
17     Point  location_2;
18     location_2=sl.forwordtrans(this->leglth1,this->leglth2,Poi0);
19     std::cout<<"此时机器人末端世界坐标为："<<"["<<location_2.x<<","<<location_2.y<<"]"<<std::endl;
20 }
21 
22 void Robot::PTPMove(TaskFrame tf,Point point){
23     this->TF0=tf;
24     this->Poi0=point;
25     Point WFpoint;
26     WFpoint=TF0.TF_to_WF(Poi0);
27     std::cout<<"该任务坐标系下的点，在世界坐标系的坐标为："<<"["<<WFpoint.x<<","<<WFpoint.y<<"]"<<";";
28     Slover sl;
29     Point  angle_2;
30     angle_2=sl.inversetrans(this->leglth1,this->leglth2,point);
31     std::cout<<"完成该任务，机器人的两个关节角度是："<<"["<<angle_2.x<<","<<angle_2.y<<"]"<<std::endl;        
32 }
33     
34

Robot.c

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

 1 #include<iostream>
 2 #include<vector>
 3 #include"Robot.h"
 4 
 5 using namespace std;
 6 
 7  
 8 int main(){
 9     
10     Robot myRobot;
11     Point p0(0,0),p1(45,50),p2(8,15),p3(15,8),p4(9,12),p5(10,6); 
12     
13     WorldFrame WF1;
14     WF1.updatezeropoint(p0);
15     
16     
17     TaskFrame TF1,TF2,TF3;
18     
19     Point tf_location1(2,2),tf_location2(3,2),tf_location3(2,3); //这三个点坐标为任务坐标系与世界坐标系的偏移向量 
20     
21     TF1.updateTaskFrame(tf_location1,45);
22     TF1.updateTaskFrame(tf_location2,60);
23     TF1.updateTaskFrame(tf_location3,90);
24     
25     
26     JointFrame JF;
27     
28 
29     myRobot.initRobot(10,10,180,0,0,360,0,0);
30     
31     myRobot.PTPMove(JF,p1);
32     myRobot.PTPMove(WF1,p2);
33     myRobot.PTPMove(TF1,p3);
34     myRobot.PTPMove(TF2,p4);
35     myRobot.PTPMove(TF3,p5);
36 
37     return 0;
38  
39 }