第二次编程作业
1.建立点类
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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 }
2.建立坐标系类
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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 }
3.Solver解释器
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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);}
4,机器人类
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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
5 主函数
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
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 }
6 结果显示
小结:
1 坐标系类还有可以简化的余地;
2 运动学反解没有算出公式,显示的角度值都是0度;
3 机器人的转动角度范围没有确定;
4.建立坐标系类时用了继承,但感觉用的不好,或许没必要用;