第二次編程做業

  1 #include <iostream>
  2 #include<Eigen/Dense>
  3 #include<cmath>
  4 #define PI 3.1415926
  5 using namespace std;
  6 using namespace Eigen;
  7 class Point{
  8     public:
  9         double x;
 10         double y;
 11         
 12         Point(double a=0,double b=0);
 13         void SetP(double a,double b);
 14         
 15 };
 16 Point::Point(double a,double b){
 17     x=a;
 18     y=b;
 19 }
 20 void Point::SetP(double a,double b){
 21     x=a;
 22     y=b;
 23 }
 24 
 25 class WorldFrame{
 26     
 27 };
 28 class TaskFrame{
 29     public:
 30         double x;
 31         double y;
 32         double ang;
 33     
 34         TaskFrame(double a=0,double b=0,double d=0);
 35         void WtoT(Point &p);
 36         void TtoW(Point &p);
 37         void TtoJ(Point &p,double a1,double a2);
 38 };
 39 
 40 TaskFrame::TaskFrame(double a,double b,double A){
 41     x=a;
 42     y=b;
 43     ang=A;
 44 }
 45 
 46 void TaskFrame::WtoT(Point &p){
 47     MatrixXd m(3,3);
 48     MatrixXd pt(1,3);
 49     double deg;
 50     deg=ang*PI/180;
 51     m(0,0)=cos(deg);
 52     m(0,1)=sin(deg);
 53     m(0,2)=0;
 54     m(1,0)=-sin(deg);
 55     m(1,1)=cos(deg);
 56     m(1,1)=0;
 57     m(2,0)=x,
 58     m(2,1)=y;
 59     m(2,2)=0;
 60     pt(0,0)=p.x;
 61     pt(0,0)=p.y;
 62     pt(0,2)=1;
 63     pt*=m;
 64     p.x=pt(0,0);
 65     p.y=pt(0,1);    
 66 }
 67 void TaskFrame::TtoW(Point &p){
 68     MatrixXd m(3,3);
 69     MatrixXd pt(1,3);
 70     double deg;    
 71     deg=ang*PI/180;
 72     m(0,0)=cos(deg);
 73     m(0,1)=sin(deg);
 74     m(0,2)=0;
 75     m(1,0)=-sin(deg);
 76     m(1,1)=cos(deg);
 77     m(1,1)=0;
 78     m(2,0)=x;
 79     m(2,1)=y;
 80     m(2,2)=0;
 81     pt(0,0)=-p.x;
 82     pt(0,0)=-p.y;
 83     pt(0,2)=1;
 84     pt*=m;
 85     p.x=pt(0,0);
 86     p.y=pt(0,1);
 87 }
 88 void TaskFrame::TtoJ(Point &p,double a1,double a2){
 89     double l,deg1,deg2,deg3;
 90     l=sqrt(p.x*p.x+p.y*p.y);
 91     deg1=atan(p.y/p.x);
 92     deg2=acos((a1*a1+l*l-a2*a2)/(2*a1*l));
 93     deg3=acos((a1*a1+a2*a2-l*l)/(2*a1*a2));
 94     p.x=(deg1+deg2)*180/PI;
 95     p.y=deg3*180/PI+180;
 96 }
 97 class JointFrame{
 98 
 99 };
100 
101 class Robot{
102     private:
103         double arm1,arm2,deg1min,deg2min,deg1max,deg2max;
104     public:
105         Robot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
106         void SetRobot(double a1=1,double a2=1,double d1min=0,double d2min=0,double d1max=180,double d2max=360);
107         void PTPMove(WorldFrame wf,TaskFrame tf,Point p);
108         void PTPMove(TaskFrame tf,Point P);
109         void PTPMove(JointFrame jf,Point P);
110         void print(Point &p);
111 };
112 
113 Robot::Robot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
114     arm1=a1;
115     arm2=a2;
116     deg1min=d1min;
117     deg2min=d2min;
118     deg1max=d1max;
119     deg2max=d2max;
120 }
121 void Robot::SetRobot(double a1,double a2,double d1min,double d2min,double d1max,double d2max){
122     arm1=a1;
123     arm2=a2;
124     deg1min=d1min;
125     deg2min=d2min;
126     deg1max=d1max;
127     deg2max=d2max;
128 }
129 void Robot::PTPMove(WorldFrame wf,TaskFrame tf,Point p){
130     tf.WtoT(p);
131     tf.TtoJ(p,arm1,arm2);
132     print(p);
133 }
134 void Robot::PTPMove(TaskFrame tf,Point p){
135     tf.TtoJ(p,arm1,arm2);
136     print(p);
137 }
138 void Robot::PTPMove(JointFrame jf,Point p){
139     print(p);
140 }
141 void Robot::print(Point &p){
142     if(p.x>=deg1min||p.y<=deg1max){
143         cout<<"機器人的關節座標爲：("<<p.x<<','<<p.y<<')'<<endl;
144     }
145     else cout<<"沒法旋轉到該位置"<<endl; 
146 }
147 
148 
149 
150 int main(int argc, char** argv) {
151     Robot myRobot(10,10);
152     WorldFrame WF;
153     TaskFrame TF1(0,0,0),TF2(2,2,30),TF3(1,3,60);
154     JointFrame JF;
155     Point P1(0,0),P2(1,1),P3(2,2),P4(2,1),P5(3,7);
156     myRobot.PTPMove(JF,P1);
157     myRobot.PTPMove(WF,TF1,P2);
158     myRobot.PTPMove(TF1,P3);
159     myRobot.PTPMove(TF2,P4);
160     myRobot.PTPMove(TF3,P5);
161     return 0;
162 }
運算結果：