第二次作业,最近有点忙,一直没写,先发一下,关节角计算有点问题,后面抽时间改

 #include<iostream>
 #include <Eigen/Dense>
 #include "Robot.h"
 using namespace Eigen;
 using namespace std;
  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();
      //cout<<180*atan2(sqrt(3),1)/PI<<endl;
      ;
  }

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;
             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<<"aa="<<fr.origin.x<<endl;
  ToJoint();
 }
 void Robot::ToJoint()
 {
    joint1.theta=acos(((pow(joint2.x,)+pow(joint2.y,))+(pow(length1,)-pow(length2,)))/(*length1*sqrt(pow(joint2.x,)+pow(joint2.y,))))+atan2(joint2.y,joint2.x);
    joint2.theta=acos(((pow(joint2.x,)+pow(joint2.y,))+(pow(length2,)-pow(length1,)))/(*length2*sqrt(pow(joint2.x,)+pow(joint2.y,))))+atan2(joint2.y,joint2.x);
 //   cout<<length1<<"  "<<length2<<endl;
 //   cout<<joint2.x<<"  "<<joint2.y<<endl;
 }

 void Robot::JointTo()
 {
    joint2.x=length1*cos(joint1.theta)+length2*cos(joint2.theta);
    joint2.x=length1*sin(joint1.theta)+length2*sin(joint2.theta);
 }
 void Robot::RobotShow(void){
     cout<</PI;
     cout<</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 for the Homework2 改进

    1. 实现了到指定点各个关节的转角计算(多解性),并且所求解满足各个关节的最大角和最小角的限制条件. 2. 对方向向量进行了单位化,保证任意大小的向量都行 #include<iostream&g ...

  2. Visual Studio Code 代理设置

    Visual Studio Code (简称 VS Code)是由微软研发的一款免费.开源的跨平台文本(代码)编辑器,在十多年的编程经历中,我使用过非常多的的代码编辑器(包括 IDE),例如 Fron ...

  3. 我们是怎么做Code Review的

    前几天看了<Code Review 程序员的寄望与哀伤>,想到我们团队开展Code Review也有2年了,结果还算比较满意,有些经验应该可以和大家一起分享.探讨.我们为什么要推行Code ...

  4. Code Review 程序员的寄望与哀伤

    一个程序员,他写完了代码,在测试环境通过了测试,然后他把它发布到了线上生产环境,但很快就发现在生产环境上出了问题,有潜在的 bug. 事后分析,是生产环境的一些微妙差异,使得这种 bug 场景在线下测 ...

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

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

  6. 在Visual Studio Code中配置GO开发环境

    一.GO语言安装 详情查看:GO语言下载.安装.配置 二.GoLang插件介绍 对于Visual Studio Code开发工具,有一款优秀的GoLang插件,它的主页为:https://github ...

  7. 代码的坏味道(14)——重复代码(Duplicate Code)

    坏味道--重复代码(Duplicate Code) 重复代码堪称为代码坏味道之首.消除重复代码总是有利无害的. 特征 两个代码片段看上去几乎一样. 问题原因 重复代码通常发生在多个程序员同时在同一程序 ...

  8. http status code

    属于转载 http status code:200:成功,服务器已成功处理了请求,通常这表示服务器提供了请求的网页 404:未找到,服务器未找到 201-206都表示服务器成功处理了请求的状态代码,说 ...

  9. Visual Studio Code——Angular2 Hello World 之 2.0

    最近看到一篇用Visual Studio Code开发Angular2的文章,也是一篇入门教程,地址为:使用Visual Studio Code開發Angular 2專案.这里按部就班的做了一遍,感觉 ...

随机推荐

  1. 通过使用精简客户端,且不需要安装的客户端,配合PLSQL连接oracle数据库

    通过使用精简客户端,且不需要安装的客户端,配合PLSQL连接oracle数据库. 首先下载安装包在Oralce官方网站上下载Oracle Instantclient Basic package.地址如 ...

  2. Servlet & JSP - UrlRewriteFilter

    重写 URL 的好处有很多: 静态化页面,有利于搜索引擎收录. 隐藏真实的 URL,提高安全性. 当网站的结构发生变化时,无需要求用户修改书签. UrlRewriteFilter 的简单应用 1. M ...

  3. Spring(3.2.3) - Beans(7): 延迟实例化

    默认情况下,Spring IoC 容器启动后,在初始化过程中,会以单例模式创建并配置所有使用 singleton 定义的 Bean 的实例.通常情况下,提前实例化 Bean 是可取的,因为这样在配置中 ...

  4. django 学习-4 模板标签

    1.第一个标签是 if 标签 vim  learn/home.html <!DOCTYPE html><html><head>        <title&g ...

  5. 初识 AutoLayout

    一.使用"公式": 1.frame: 原点以及自身的位置来确定自身的位置 2.autoLayout: 根据参照视图的位置  来定义自己的位置 3.autoLayout: 相对布局  ...

  6. OpenGL7-3快速绘制(索引方式)

    代码下载#include "CELLWinApp.hpp"#include <gl/GLU.h>#include <assert.h>#include &l ...

  7. win7下64位系统memcache/memcached安装教程

    折腾了1个多小时,终于搞定.操作系统时64位的,php5.3.13 类似于上一篇的xdebug安装教程~~ memcache和memcached的区别  在自己的新程序中打算全面应用memcached ...

  8. NodeJS文件读取:感恩常在--抓把糖果,愉悦客人

    通过上一篇文章“NodeJS服务器:一行代码 = 一个的HTTP服务器”,我们已经开启了NodeJS之旅,开发了一个监听在8000端口的HTTP服务器,虽然功能很简单,但是,已经让我们感受到用Node ...

  9. (hdu)1257 最少拦截系统

    题目链接:http://acm.split.hdu.edu.cn/showproblem.php?pid=1257 Problem Description 某国为了防御敌国的导弹袭击,发展出一种导弹拦 ...

  10. office2013发布博客

    0, 喜欢用world记笔记,并查上一些配图.但是再想重新发到博客上,图片不得不重新上传十分蛋疼. world直接发布博客功能太棒了,直接绑定账号和url就可以发送了,爽YY!!! 1,新建一个博客文 ...