第二次编程

 1 #ifndef POINT
 2 #define POINT
 3 class Point{
 4     public:
 5         double x;
 6         double y;
 7         
 8         Point(double a=0,double b=0);
 9         void SetP(double a,double b);
10         void PrintP();
11         
12 };
13 #endif

 1 #include "Point.h"
 2 #include <iostream>
 3 using namespace std;
 4 
 5 Point::Point(double a,double b){
 6     x=a;
 7     y=b;
 8 }
 9 void Point::SetP(double a,double b){
10     x=a;
11     y=b;
12 }
13 void Point::PrintP(){
14     cout<<‘(‘<<x<<‘,‘<<y<<‘)‘<<endl;
15 }

 1 #ifndef FRAME
 2 #define FRAME
 3 class WorldFrame{
 4     
 5 };
 6 class TaskFrame{
 7     public:
 8         double x;
 9         double y;
10         double deg;
11     
12         TaskFrame(double a=0,double b=0,double d=0);
13 };
14 class JointFrame{
15 
16 };
17 #endif

1 #include "Frame.h"
2 TaskFrame::TaskFrame(double a,double b,double d){
3     x=a;
4     y=b;
5     deg=d;
6 }

 1 #ifndef SOLVER
 2 #define SOLVER
 3 #include "Frame.h"
 4 #include "Point.h"
 5 class Solver{
 6     public:
 7         void WtoT(TaskFrame t,Point &p);
 8         void TtoW(TaskFrame t,Point &p);
 9         void TtoJ(Point &p,double a1,double a2);
10         void JtoT(Point &p,double a1,double a2);
11 };
12 #endif

 1 #include "Solver.h"
 2 #include<Eigen/Dense>
 3 using namespace Eigen;
 4 #define PI 3.1415926
 5 void Solver::WtoT(TaskFrame t,Point &p){
 6     MatrixXd m(3,3);
 7     MatrixXd pt(1,3);
 8     m(0,0)=cos(t.deg*PI/180);
 9     m(0,1)=sin(t.deg*PI/180);
10     m(0,2)=0;
11     m(1,0)=-sin(t.deg*PI/180);
12     m(1,1)=cos(t.deg*PI/180);
13     m(1,1)=0;
14     m(2,0)=t.x,
15     m(2,1)=t.y;
16     m(2,2)=0;
17     pt(0,0)=p.x;
18     pt(0,1)=p.y;
19     pt(0,2)=1;
20     pt*=m;
21     p.x=pt(0,0);
22     p.y=pt(0,1);    
23 }
24 void Solver::TtoW(TaskFrame t,Point &p){
25     MatrixXd m(3,3);
26     MatrixXd pt(1,3);
27     m(0,0)=cos(-t.deg*PI/180);
28     m(0,1)=sin(-t.deg*PI/180);
29     m(0,2)=0;
30     m(1,0)=-sin(-t.deg*PI/180);
31     m(1,1)=cos(-t.deg*PI/180);
32     m(1,1)=0;
33     m(2,0)=t.x;
34     m(2,1)=t.y;
35     m(2,2)=0;
36     pt(0,0)=-p.x;
37     pt(0,1)=-p.y;
38     pt(0,2)=1;
39     pt*=m;
40     p.x=pt(0,0);
41     p.y=pt(0,1);
42 }
43 void Solver::TtoJ(Point &p,double a1,double a2){
44     double l,deg1,deg2,deg3;
45     l=sqrt(p.x*p.x+p.y*p.y);
46     deg1=atan(p.y/p.x);
47     deg2=acos((a1*a1+l*l-a2*a2)/(2*a1*l));
48     deg3=acos((a1*a1+a2*a2-l*l)/(2*a1*a2));
49     p.x=(deg1+deg2)*180/PI;
50     p.y=deg3*180/PI+180;
51 }
52 void Solver::JtoT(Point &p,double a1,double a2){
53     p.x=a1*cos(p.x)+a2*cos(p.y);
54     p.y=a1*sin(p.x)+a2*sin(p.y);
55 }

 1 #ifndef ROBOT
 2 #define ROBOT
 3 #include "Solver.h"
 4 class Robot{
 5     private:
 6         double arm1,arm2,deg1min,deg2min,deg1max,deg2max;
 7     public:
 8         Robot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
 9         void SetRobot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
10         void PTPMove(WorldFrame wf,TaskFrame tf,Point p);
11         void PTPMove(TaskFrame tf,Point P);
12         void PTPMove(JointFrame jf,Point P);
13         void print(Point &p);
14 };
15 #endif

 1 #include "Robot.h"
 2 #include <iostream>
 3 using namespace std;
 4 Robot::Robot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
 5     arm1=a1;
 6     arm2=a2;
 7     deg1min=d1min;
 8     deg2min=d2min;
 9     deg1max=d1max;
10     deg2max=d2max;
11 }
12 void Robot::SetRobot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
13     arm1=a1;
14     arm2=a2;
15     deg1min=d1min;
16     deg2min=d2min;
17     deg1max=d1max;
18     deg2max=d2max;
19 }
20 void Robot::PTPMove(WorldFrame wf,TaskFrame tf,Point p){
21     Solver s;
22     s.WtoT(tf,p);
23     s.TtoJ(p,arm1,arm2);
24     print(p);
25 }
26 void Robot::PTPMove(TaskFrame tf,Point p){
27     Solver s;
28     s.TtoJ(p,arm1,arm2);
29     print(p);
30 }
31 void Robot::PTPMove(JointFrame jf,Point p){
32     print(p);
33 }
34 void Robot::print(Point &p){
35     if(p.x>=deg1min||p.y<=deg1max){
36         cout<<"关节坐标为：("<<p.x<<‘,‘<<p.y<<‘)‘<<endl;
37     }
38     else cout<<"无法旋转到该位置"<<endl; 
39 }

 1 #include "Robot.h"
 2 /* run this program using the console pauser or add your own getch, system("pause") or input loop */
 3 
 4 int main(int argc, char** argv) {
 5     Robot myRobot(8,8);
 6     WorldFrame WF;
 7     TaskFrame TF1(0,0,0),TF2(1,2,30),TF3(3,5,90);
 8     JointFrame JF;
 9     Point P1(0,0),P2(1,2),P3(2,2),P4(3,5),P5(0,2);
10     myRobot.PTPMove(JF,P1);
11     myRobot.PTPMove(WF,TF1,P2);
12     myRobot.PTPMove(TF1,P3);
13     myRobot.PTPMove(TF2,P4);
14     myRobot.PTPMove(TF3,P5);
15     return 0;
16 
17 }