Code for the Homework2 改进
1. 实现了到指定点各个关节的转角计算(多解性),并且所求解满足各个关节的最大角和最小角的限制条件。
2. 对方向向量进行了单位化,保证任意大小的向量都行
#include<iostream> #include <Eigen/Dense> #include "Robot.h" int main(){ ,l2 =; Vector2d JF_vx(,),JF_vy(,); Vector2d WF_vx(,),WF_vy(,); POINT jf_origin(,),wf_origin(,); Joint jt1(,,,-,,),jt2(l1,,,-,,); Frame JF("jf",JF_vx,JF_vy,jf_origin),WF("jf",WF_vx,WF_vy,wf_origin); Robot myRobot(l1,l2,jt1,jt2,JF,WF); POINT tf1_origin(,),tf2_origin(,),tf3_origin(,); Vector2d TF1_vx(,),TF1_vy(-,),TF2_vx(-,),TF2_vy(,-),TF3_vx(,-),TF3_vy(,); Frame TF1("tf1",TF1_vx,TF1_vy,tf1_origin),TF2("tf2",TF2_vx,TF2_vy,tf2_origin),TF3("tf3",TF3_vx,TF3_vy,tf3_origin); myRobot.TaskFrameCreate(TF1); myRobot.TaskFrameCreate(TF2); myRobot.TaskFrameCreate(TF3); POINT P1(,),P2(,),P3(,),P4(,),P5(,); myRobot.PTPMove(JF,P1); // myRobot.RobotShow(); myRobot.PTPMove(WF,P2); // myRobot.RobotShow(); myRobot.PTPMove(TF1,P3); // myRobot.RobotShow(); myRobot.PTPMove(TF2,P4); // myRobot.RobotShow(); myRobot.PTPMove(TF3,P5); // myRobot.RobotShow(); ; }
main.cpp
#include<iostream> #include <Eigen/Dense> #include<vector> using namespace Eigen; using namespace std; #define PI 3.141592653 class POINT { public: double x, y; string name; POINT(){ }; POINT(double xx,double yy){ x=xx; y=yy; }; POINT(string nam,double xx,double yy){ name=nam; x=xx; y=yy; } POINT(const POINT &p){ name=p.name; x=p.x; y=p.y; } POINT operator =(const POINT &pt) { POINT ptt(pt); return ptt; } void copyto(POINT &p); void get_cin_point(void); void display(); void rotate(double &angle); void move(Vector2d &vec); }; class Frame { public: string name; Vector2d vector_X; Vector2d vector_Y; POINT origin; Frame(){ } Frame(string nam,Vector2d &vx,Vector2d &vy,POINT &oripoint) { name=nam; ],)+pow(vx[],)); //化为单位方向向量 vx[]=vx[]/a; vx[]=vx[]/a; ],)+pow(vy[],)); //化为单位方向向量 vy[]=vy[]/a; vy[]=vy[]/a; vector_X=vx; vector_Y=vy; //origin=oripoint; oripoint.copyto(origin); } Frame(const Frame &fr) { name=fr.name; vector_X=fr.vector_X; vector_Y=fr.vector_Y; origin=fr.origin; } Frame operator =(const Frame &fr) { Frame fra(fr); return fra; } }; class Joint { public: double x,y,theta; double thetamin,thetamax,thetazero; Joint(){ } Joint(double xx,double yy,double thetaa,double thetaminn,double thetamaxx,double thetazeroo) { x=xx; y=yy; theta=thetaa; thetamin=thetaminn; thetamax=thetamaxx; thetazero=thetazeroo; } CopyTo(Joint &jt) { jt.x=x; jt.y=y; jt.theta=theta; jt.thetamin=thetamin; jt.thetamax=thetamax; jt.thetazero=thetazero; } }; class Robot { public: double length1,length2; Joint joint1,joint2; Frame JointFrame,WorldFrame; vector<Frame> fv; Robot(){ } Robot(double l1,double l2,Joint jt1,Joint jt2,Frame JF,Frame WF) { length1=l1; length2=l2; jt1.CopyTo(joint1); jt2.CopyTo(joint2); JointFrame=JF; WorldFrame=WF; } void TaskFrameCreate(const Frame &tf); void PTPMove(const Frame &fr,const POINT &pt); void RobotShow(void); void ToJoint(); void JointTo(); }; // class Solver // { // public: // //friend void PTPMove(Frame &fr,POINT &pt); // void ToJoint(Robot &myrobot); // void JointTo(Robot &myrobot); // };
Robot.h
#include "Robot.h" #include "math.h" void Robot::TaskFrameCreate(const Frame &tf) { fv.push_back(tf); } void Robot::PTPMove(const Frame &fr,const POINT &pt) { ],fr.vector_X[]); joint2.x=fr.origin.x+pt.x*cos(theta)-pt.y*sin(theta); joint2.y=fr.origin.y+pt.x*sin(theta)+pt.y*cos(theta); cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl; ToJoint(); } void Robot::ToJoint() { double t1,t2; ]={false}; bool flag1=false; ][]; ; )+pow(joint2.y,))+(pow(length1,)-pow(length2,)))/(*length1*sqrt(pow(joint2.x,)+pow(joint2.y,))); |a>) flag1=false; else{ //多解性 t1=acos(a);//acos默认取值 0~PI t2=atan2(joint2.y,joint2.x); theta[][]=t1+t2; ][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){ theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][])); ][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){ ][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01) { flag[]=true; } } } theta[][]=-t1+t2; ][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){ theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][])); ][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){ ][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01) { flag[]=true; } } } theta[][]=t1-t2; ][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){ theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][])); ][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){ ][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01) { flag[]=true; } } } theta[][]=-t1-t2; ][]*/PI<joint1.thetamax&theta[][]*/PI>joint1.thetamin){ theta[][]=atan2(joint2.y-length1*sin(theta[][]),joint2.x-length1*cos(theta[][])); ][]*/PI<joint2.thetamax&theta[][]*/PI>joint2.thetamin){ ][])-length2*cos(theta[][]))<][])-length2*sin(theta[][]))<0.01) { flag[]=true; } } } ;i<;i++) { if(flag[i]==true) { flag1=true; cnt++; joint1.theta=theta[i][]; joint2.theta=theta[i][]; cout<<"第"<<cnt<<"种解为:"; cout<<]*/PI; cout<<]*/PI<<endl; } } } if(flag1==false) cout<<"无法达到指定位置"<<endl; } void Robot::JointTo() { joint2.x=length1*cos(joint1.theta)+length2*cos(joint2.theta); joint2.y=length1*sin(joint1.theta)+length2*sin(joint2.theta); } void Robot::RobotShow(void){ // cout<<"关节1转角"<< joint1.theta*180/PI; // cout<<"关节2转角"<< joint2.theta*180/PI; // cout<<"末端关节坐标("<<joint2.x<<","<<joint2.y<<")"<<endl; } void POINT::copyto(POINT &p){ p.name=name; p.x=x; p.y=y; }
Robot.cpp
Code for the Homework2 改进的更多相关文章
- Code alignment 代码对齐改进(VS2017)
In mathematics you always keep your equals lined up directly underneath the one above. It keeps it c ...
- Code for the Homework2
第二次作业,最近有点忙,一直没写,先发一下,关节角计算有点问题,后面抽时间改 #include<iostream> #include <Eigen/Dense> #includ ...
- Code for the Homework1 改进
#include <iostream> #include <vector> #include "shape.h" //using namespace std ...
- PHP5.4最新特性
PHP5.4最新特性 官网:ChangeLog-5.php#5.4.0 原文Oracle:LAMP 体系有了新的竞争,但此版本中的特性使 PHP 再次挑战极限. 稍微做了修改.: 概述总结:1. ...
- 《精通python网络爬虫》笔记
<精通python网络爬虫>韦玮 著 目录结构 第一章 什么是网络爬虫 第二章 爬虫技能概览 第三章 爬虫实现原理与实现技术 第四章 Urllib库与URLError异常处理 第五章 正则 ...
- iOS开发 - 开发版+企业版无线发布一键打包
背景:项目进入快速迭代期,需要快速地交付出AdHoc版本和企业无线发布版本.每次打包都要来回切换bundle identifier和code signing,浪费很多时间. 示例项目名称名称为Test ...
- 3D-camera结构光原理
3D-camera结构光原理 目前主流的深度探测技术是结构光,TOF,和双目.具体的百度就有很详细的信息. 而结构光也有双目结构光和散斑结构光等,没错,Iphone X 的3D深度相机就用 散斑结构光 ...
- 算法所产生的性能改进已经超过了硬件所带来的性能提升 The future is algorithms, not code
w 大数据的发展,伴随的将是软件工程师的渐退,算法工程师的崛起 http://mp.weixin.qq.com/s/XTz2HnzwlgTD3g5zU2u5Tg
- 从Script到Code Blocks、Code Behind到MVC、MVP、MVVM
刚过去的周五(3-14)例行地主持了技术会议,主题正好是<UI层的设计模式——从Script.Code Behind到MVC.MVP.MVVM>,是前一天晚上才定的,中午花了半小时准备了下 ...
随机推荐
- Tamperdata工具使用(登陆时就修改用户名),篡改post数据
Tamperdata是firefox上的一款插件,它可以实现篡改数据的功能,这样可以做一些安全的测试验证,他的原理就是在发出请求前,在本地就开始改动数据,下面修改post请求 1.安装Tamperda ...
- MyBatis(3.2.3) - Configuring MyBatis using XML, Properties
The properties configuration element can be used to externalize the configuration values into a prop ...
- Linux 命令 - netstat: 检查网络设置及相关统计数据
netstat 程序可以用于查看不同的网络设置及数据.通过使用其丰富的参数选项,我们可以查看网络启动过程的许多特性. 命令格式 netstat [options] 命令参数 -r, --route 显 ...
- Linux 命令 - at: 在指定的时间执行任务
在指定的时间执行任务. 命令格式 at [-V] [-q queue] [-f file] [-mldbv] TIMEat [-V] [-q queue] [-f file] [-mldbv] -t ...
- ios 一个app启动另一个app
问题描述:需要从一个ios应用程序中,能启动另一个ios应用程序. 开发环境:xcode7.3.1 关键词:白名单(LSApplicationQueriesSchemes).注册自己的URL Demo ...
- SOA 的基本概念及设计原则浅议
SOA是英文词语"Service Oriented Architecture"的缩写,中文有多种翻译,如"面向服务的体系结构"."以服务为中心的体系结 ...
- (转)MSSQL 各个发行版本版本号以及Compact 版本号
终于开始写博客了. 不要笑啊. 下面是MSSQL 的发行版本以及版本号.自己整理的. http://support.microsoft.com/kb/321185/zh-cn SQL Server 2 ...
- 关于css3 中filter的各种特效
做项目时遇到了一个有趣的css特效. 目前各大浏览器对于css3的兼容已经非常好了,最新版本都可以支持css3,老版本的ie9以下的还是不支持,不过这不是重点,微软都准备放弃这些老古董了. 现在规范中 ...
- 【转载】干货再次来袭!Linux小白最佳实践:《超容易的Linux系统管理入门书》(连载八)用命令实现批量添加用户
Windows添加用户需要至少5个界面,而Linux一条命令就搞定了,这是不是高效人士办公第一法则呢.本文不给你一堆参数和选项,不让你见识教条主义,只给你最实用的代码. 想每天能听到小妞的语音播报,想 ...
- [GeekBand] 面向对象的设计模式(C++)(1)
一个模式描述了一个在我们周围不断重复发生的问题,以及该问题的解决方案的核心. 这样,你就能一次又一次 地使用该方案而不必做重复劳动. 0. 从面向对象谈起 底层思维与抽象思维: 底层思维要求程序员&q ...