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 改进的更多相关文章

  1. Code alignment 代码对齐改进(VS2017)

    In mathematics you always keep your equals lined up directly underneath the one above. It keeps it c ...

  2. Code for the Homework2

    第二次作业,最近有点忙,一直没写,先发一下,关节角计算有点问题,后面抽时间改 #include<iostream> #include <Eigen/Dense> #includ ...

  3. Code for the Homework1 改进

    #include <iostream> #include <vector> #include "shape.h" //using namespace std ...

  4. PHP5.4最新特性

     PHP5.4最新特性   官网:ChangeLog-5.php#5.4.0 原文Oracle:LAMP 体系有了新的竞争,但此版本中的特性使 PHP 再次挑战极限. 稍微做了修改.: 概述总结:1. ...

  5. 《精通python网络爬虫》笔记

    <精通python网络爬虫>韦玮 著 目录结构 第一章 什么是网络爬虫 第二章 爬虫技能概览 第三章 爬虫实现原理与实现技术 第四章 Urllib库与URLError异常处理 第五章 正则 ...

  6. iOS开发 - 开发版+企业版无线发布一键打包

    背景:项目进入快速迭代期,需要快速地交付出AdHoc版本和企业无线发布版本.每次打包都要来回切换bundle identifier和code signing,浪费很多时间. 示例项目名称名称为Test ...

  7. 3D-camera结构光原理

    3D-camera结构光原理 目前主流的深度探测技术是结构光,TOF,和双目.具体的百度就有很详细的信息. 而结构光也有双目结构光和散斑结构光等,没错,Iphone X 的3D深度相机就用 散斑结构光 ...

  8. 算法所产生的性能改进已经超过了硬件所带来的性能提升 The future is algorithms, not code

    w 大数据的发展,伴随的将是软件工程师的渐退,算法工程师的崛起  http://mp.weixin.qq.com/s/XTz2HnzwlgTD3g5zU2u5Tg

  9. 从Script到Code Blocks、Code Behind到MVC、MVP、MVVM

    刚过去的周五(3-14)例行地主持了技术会议,主题正好是<UI层的设计模式——从Script.Code Behind到MVC.MVP.MVVM>,是前一天晚上才定的,中午花了半小时准备了下 ...

随机推荐

  1. [改善Java代码]优先使用整型池

    建议28: 优先使用整型池 看如下代码: public class Client { public static void main(String[] args) { Scanner input = ...

  2. (转).net项目技术选型总结

    原文作者:mcgrady 原文地址:.net项目技术选型总结 做.net开发已经几年了,也参与开发了很多大大小小的项目,所以现在希望总结出一套开发.net项目的常用技术,也为以后做项目技术选型的时候作 ...

  3. select选项框特效

    <!DOCTYPE html> <html> <head lang="en"> <meta charset="UTF-8&quo ...

  4. 关于IE6的一些常见的CSS BUG处理

    CSS BUG:样式在各浏览器中解析不一致的情况,或者说CSS样式在浏览器中不能正确显示的问题称为CSS BUG: CSS Hack:css hack是指一种兼容css 在不同浏览器中正确显示的技巧方 ...

  5. CXF(2.7.10) - Writing a service with Spring

    1. 定义服务接口. package com.huey.demo.ws; import javax.jws.WebParam; import javax.jws.WebService; @WebSer ...

  6. Lombok(1.14.8) - @NoArgsConstructor, @RequiredArgsConstructor & @AllArgsConstructor

    @NoArgsConstructor @NoArgsConstructor,提供一个无参的构造方法. package com.huey.hello.bean; import java.util.Dat ...

  7. SQL Server内存数据写入磁盘方法比较

    众所周知,SQLServer增删改数据最先都是在内存中进行的,这可以大大加快数据操作的速度: 当内存中的数据被修改了,而磁盘中的数据还没有被修改时,就产生了所谓的“脏页”,SQLServer是如何同步 ...

  8. C# 正则表达式(一)

    首先来复习一下正则表达式的基础知识,本篇文章分为2个部分,第一个部分复习正则表达式中的元字符和简写表达式,第二部分复习正则表达式的匹配和提取. 1.正则表达式中的元字符和简写表达式 ".&q ...

  9. 第五篇、微信小程序-swiper组件

    常用属性: 效果图: swiper.wxml添加代码: <swiper indicator-dots="{{indicatorDots}}" autoplay="{ ...

  10. IOS UI 笔记整理回顾

    注意手势会冒泡上抛,一个view没有实现的手势,如果父类view有实现,父视图就处理,如果不想让父视图处理,就把本视图添加到底层window上 setMasksToBounds:YES imageVi ...