Code for the Homework2
2024-08-21 02:15:22
第二次作业,最近有点忙,一直没写,先发一下,关节角计算有点问题,后面抽时间改
#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
最新文章
- Android/Linux下CGroup框架分析及其使用
- [django]利用xlwt实现文件下载功能
- 大叔也说Xamarin~Android篇~原生登陆与WebView的网站如何共享Session
- tar 打包命令
- Sharing count on Facebook, Twitter, and LinkedIn
- HTTPS 协议降级攻击原理
- Apache Spark源码走读之18 -- 使用Intellij idea调试Spark源码
- Java内存管理和垃圾回收
- 如何彻底卸载MySQL
- cf------(round 2)A. Winner
- Linux网络编程6&mdash;&mdash;使用TCP实现文件服务器
- PCB特征阻抗计算神器Polar SI9000安装及破解指南
- 2015北京网络赛B题 Mission Impossible 6
- [LeetCode#218] The Skyline Problem
- Quick StateMachine状态机
- Javascript:由 “鸭子类型” 得出来的推论
- git push的用法
- 牛客(web 1)
- Python创建随机用户名密码并存放于Access数据库
- NPOI写Excel,Microsoft.Office.Interop.excel.dll 转换Excel为PDF
热门文章
- 关于在Eclipse中构建patch开发环境
- 【模拟ACM排名】ZOJ-2593 Ranking (Andrew Stankevich’s Contest #5)
- 【基本计数方法---加法原理和乘法原理】UVa 11538 - Chess Queen
- 4632 NOIP[2015] 运输计划
- jquery点击控制动画暂停开始
- Linux 命令 - su: 以其他用户和组 ID 的身份来运行 shell
- 详解Win2003 IIS6.0 301重定向带参数的问题(转摘)
- C# 好书一本推荐
- redis学习-day1
- 使用edgesForExtendedLayout遇到的麻烦