第二次编程作业
2015-12-10 22:56
337 查看
//Point2D.h 点类 #ifndef POINT2D #define POINT2D #include<Eigen\Dense> using namespace Eigen; class Point2D { private: double x; double y; public: //Point2D(){}; Point2D(double X = 0.0,double Y = 0.0){x = X; y = Y;} double getX(){return x;} double getY(){return y;} Vector2d PTV(); Point2D & operator+(const Point2D &); Point2D & operator-(const Point2D &); Point2D & operator=(const Point2D &); }; #endif POINT2D //Point2D.cpp #include "Point2D.h" Point2D & Point2D::operator+(const Point2D & p) { Point2D temp(x+p.x,y+p.y); return temp; } Point2D & Point2D::operator-(const Point2D & p) { Point2D temp(x-p.x,y-p.y); return temp; } Point2D & Point2D::operator=(const Point2D & p) { Point2D temp(p.x,p.y); return temp; } Vector2d Point2D::PTV() { Vector2d temp; temp(0) = getX(); temp(1) = getY(); return temp; } //Frames.h 坐标系类 #ifndef Frame #define Frame #include"Point2D.h" class Frames { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; Frames(){}; Frames(double,double,double,double,double,double); ~Frames(){}; Matrix2d rotation(Frames &); //两坐标系之间的旋转矩阵 Vector2d move(Frames &); //两坐标系之间的移动向量 private: Point2D originalPoint; Vector2d XAxis; Vector2d YAxis; }; #endif Frame //Frames.cpp #include"Frames.h" #include"math.h" #include<cmath> #include<iostream> using namespace std; #define pi 3.141592654 Frames::Frames(double px,double py,double vxx,double vxy,double vyx,double vyy) :originalPoint(px,py),XAxis(vxx,vxy),YAxis(vyx,vyy) { if(XAxis.dot(YAxis) != 0.0) { cout << "输入的坐标轴不垂直!" << endl; } } //两坐标系之间的旋转矩阵 Matrix2d Frames::rotation(Frames& fram) { double s1; double c1; double theta; Matrix2d temp; if(fram.XAxis(0) !=0 && XAxis(0)!= 0) theta = atan(fram.XAxis(1)/fram.XAxis(0))-atan(XAxis(1)/XAxis(0)); if(fram.XAxis(0) !=0 && XAxis(0)== 0) { if(XAxis(1)== 1) theta = atan(fram.XAxis(1)/fram.XAxis(0))- 90/180*pi; if(XAxis(1)== -1) theta = atan(fram.XAxis(1)/fram.XAxis(0))- 180/180*pi; } if(fram.XAxis(0) ==0 && XAxis(0)!= 0) { double k = atan(XAxis(1)/XAxis(0)); if(fram.XAxis(1)== 1) theta = (double)90/180*pi - k; if(fram.XAxis(1)== -1) theta = (double)180/180*pi - atan(XAxis(1)/XAxis(0)); } s1 = sin(theta); c1 = cos(theta); if(abs(s1) < 0.000000000000001) s1 = 0; if(abs(c1) < 0.000000000000001) c1 = 0; temp << c1,-s1, s1,c1; return temp; } //两坐标系之间的移动向量 Vector2d Frames::move(Frames & fra) { Vector2d temp; temp = fra.originalPoint.PTV()-originalPoint.PTV(); return temp; } //Solver.h 解算类 #ifndef SOLVER #define SOLVER #include"Frames.h" class Solver { public: Solver(){}; ~Solver(){}; Vector2d directSolution(double link1,double link2,double th1,double th2); Matrix2d inverSolution(double link1,double link2,Frames &, Point2D &); private: }; #endif SOLVER //Solver.cpp #include"Solver.h" #include<iostream> using namespace std; #include<cmath> #define pi 3.141592654 Vector2d Solver::directSolution(double link1,double link2,double th1,double th2) { Vector2d temp; temp(0) = link1*cos(th1) + link2*cos(th1+th2); temp(1) = link1*sin(th1) + link2*sin(th1+th2); return temp; } Matrix2d Solver::inverSolution(double link1,double link2,Frames &f,Point2D & p) { Vector3d point; Vector2d result; //p在基坐标系上的坐标值 Vector2d moveF; //坐标系f与极坐标系原点之间的向量 Matrix2d rota; //坐标系f与基坐标系之间的旋转矩阵 Matrix3d T; //坐标系f与基坐标系之间的转换矩阵 Matrix2d Theta; //关节角 Vector2d temp; Frames base(0.0,0.0,1.0,0.0,0.0,1.0); //把坐标系f的点p转换到基坐标 rota = base.rotation(f); moveF = base.move(f); moveF = moveF.transpose(); T << rota,moveF, 0,0,1; temp = p.PTV(); point << temp,1; point = point.transpose(); point = T*point; result << point(0),point(1); //判断点是否在找工作空间 double dis = sqrt(result.dot(result)); if(dis>(link1+link2) || dis<(link1-link2)) { Matrix2d zeros(2,2); cout << "坐标点不在工作空间!"<< endl; return zeros; } //求反解关节值 else { double beita = atan2(result(1),result(0)); double fi; double theta11,theta12,theta21,theta22; fi = acos((dis*dis+link1*link1-link2*link2)/(2*link1*dis)); theta12 = acos((dis*dis-link1*link1-link2*link2)/(2*link1*link2))*180/pi; if(theta12<0) theta11 = (beita + fi)*180/pi; else theta11 = (beita - fi)*180/pi; theta22 = -acos((dis*dis-link1*link1-link2*link2)/(2*link1*link2))*180/pi; if(theta22<0) theta21 = (beita + fi)*180/pi; else theta21 = (beita - fi)*180/pi; Theta << theta11,theta12, theta21,theta22; return Theta; } } //Robot.h 机器人类 #ifndef ROBOT #define ROBOT #include<vector> #include"Solver.h" using namespace std; class Robot:Solver { public: Robot(){}; Robot(vector<Frames> &fr,double link1 = 100,double link2 = 100, double the1=0,double the2 = 0); ~Robot(){}; void PTPMove(Frames &,Point2D &); void thetaToTCP(double,double); inline double getLink1(){return linkage1;} inline double getLink2(){return linkage2;} inline double getTheta1(){return theta1;} inline double getTheta2(){return theta2;} private: vector <Frames> fra; double linkage1; double linkage2; double theta1; double theta2; }; #endif ROBOT //Robot.cpp #include"Robot.h" #include<iostream> using namespace std; Robot::Robot(vector<Frames>& fr,double link1,double link2 ,double the1,double the2): linkage1(link1),linkage2(link2) ,theta1(the1),theta2(the2) { for(int i=0; i < fr.size(); i++) fra.push_back(fr[i]); /*if (fr.size() == 0) { Frames base(0.0,0.0,1.0,0.0,0.0,1.0); fra.push_back(base); }*/ } void Robot::PTPMove(Frames & f,Point2D & p) { Matrix2d temp; Matrix2d zeros(2,2); temp = inverSolution(linkage1,linkage2,f,p); if (temp != zeros) cout << "关节值: " <<endl << temp << endl; cout << endl; } void Robot::thetaToTCP(double th1,double th2) { Vector2d temp; temp = directSolution(linkage1,linkage2,th1,th2); cout << "运动到的点坐标:" << endl << temp; cout << endl; } //测试程序 #include"Robot.h" #include <iostream> using namespace std; int main() { Point2D p(90,92); //要运动到的目标点坐标 Point2D p1(500,1305); vector<Frames> robotFrames; Frames WF(0.0,0.0,1.0,0.0,0.0,1.0); //世界坐标系 robotFrames.push_back(WF); Frames J1(0.0,0.0,1.0,0.0,0.0,1.0); //关节1坐标系 robotFrames.push_back(J1); Frames J2(200.0,0.0,1.0,0.0,0.0,1.0); //关节2坐标系 robotFrames.push_back(J2); Frames TCP(200.0,150.0,0.0,1.0,-1.0,0.0); //工具坐标系 robotFrames.push_back(TCP); vector<int> b; for(int i = 0; i< 5;i++) b.push_back(i); //工作坐标系 Frames TF1(1.0,1.0,0.8,0.6,-0.6,0.8); Frames TF2(1.0,2.0,0.0,1.0,-1.0,0.0); Robot myrobot(robotFrames,200.0,150.0,0.0,90.0); myrobot.PTPMove(WF,p); myrobot.PTPMove(J1,p); myrobot.PTPMove(J2,p); myrobot.PTPMove(TF1,p); myrobot.PTPMove(TF2,p); myrobot.PTPMove(TCP,p); myrobot.PTPMove(WF,p1); myrobot.PTPMove(J1,p1); myrobot.PTPMove(J2,p1); myrobot.PTPMove(TF1,p1); myrobot.PTPMove(TF2,p1); myrobot.PTPMove(TCP,p1); system("pause"); return 1; }
测试结果
相关文章推荐
- java web 答辩总结
- 关于理解数组与指针
- java.lang.IndexOutOfBoundsException 错误解决
- python 机器学习 scikit-learn 总结与备忘
- Python 多进程开发与多线程开发
- Java线程2-2 固定大小的线程池FixedThreadPool
- [Python标准库]textwrap——格式化文本段落
- Delphi的WebBrowser改造,对网页中Alter等对话框的改造方法(通过COM来改造)
- Java线程2-1 线程池知识学习
- matlab中conv2函数
- python学习之路之案例3--多级菜单显示城市详细信息
- C++ 关联容器
- java中的原子类
- java运算符
- qtree(树链刨分)
- struts常见面试题目
- spring常见面试题目
- Python 基础语法(四)
- C++学习总结
- Python 基础语法(三)