• 实时控制软件设计 第二次作业 myRobot


    #include<iostream>
    #include <Eigen/Dense>
    #include <math.h>
    #include <vector>
    using namespace std;
    using namespace Eigen;
    using Eigen::MatrixXd;
    const double Dk=3.1415926/180;
    
    //定义Frame(二维笛卡尔坐标系)类 
    class Frame{
    private:
    double Ox;
    double Oy;//Frame对象在WF(世界坐标系)下的原点坐标 
    double Angle;//方位角(弧度制) 
    public:
    Frame();
    Frame(double ox,double oy,double an);
    MatrixXd CalPose();//获得齐次位姿矩阵 
    };
    Frame::Frame() {
    Ox=0;Oy=0;Angle=0;
    }
    Frame::Frame(double ox,double oy,double an){
    Ox=ox;Oy=oy;Angle=an*Dk;//输入为角度制 
    }
    MatrixXd Frame::CalPose(){
    MatrixXd Pose(3,3);
    Pose(0,0)=cos(Angle);Pose(0,1)=-sin(Angle);Pose(0,2)=Ox;
    Pose(1,0)=sin(Angle);Pose(1,1)=cos(Angle);Pose(1,2)=Oy;
    Pose(2,0)=0;Pose(2,1)=0;Pose(2,2)=1;
    return Pose; 
    }//由原点和方位角计算位姿矩阵 
    //Frame定义结束
    
    
    //定义Solver(运动学求解器)类
    class Solver{
    private:
    vector<Frame> frameVec;
    public:
    MatrixXd CalTheta(double l1,double l2,Frame frame,double px,double py);//运动学反解 
    void addFrame(Frame frame);//添加任务框 
    void deleteFrame();    //移除任务框 
    }; 
    MatrixXd Solver::CalTheta(double l1,double l2,Frame frame,double px,double py) {
    MatrixXd P_task(3,1),P_world(3,1),T(2,1);
    P_task(0,0)=px;P_task(1,0)=py;P_task(2,0)=1;
    P_world=frame.CalPose()*P_task;
    T(0,0)=acos((px*px+py*py-l1*l1-l2*l2)/2/l1/l2);//取0°到180° 
    double beta=atan2(py,px); 
    double lp=sqrt(px*px+py*py);//端点模值 
    double fea=acos((lp*lp+l1*l1-l2*l2)/2/l1/lp);
    T(1,0)=beta-fea;
    return T;
    }
    void Solver::addFrame(Frame frame) {
    frameVec.push_back(frame);
    } 
    void Solver::deleteFrame() {
    frameVec.pop_back();
    }
    //Solver类定义结束
    
     
    
    //定义Robot类
    class Robot{
    public:
    double L1;
    double L2;//臂长 
    double theta1;
    double theta2;//SCARA关节变量 (弧度角) 
    
    Robot();
    Robot(double l1,double l2,double t1,double t2);
    Vector2d getTerminal();//手臂末端的世界坐标(笛卡尔坐标) ,正运动学计算 
    void ShowState();//状态显示 
    void PTPmove(Frame frame,double px,double py);//点到点运动 
    
    }; 
    Robot::Robot() {
    L1=10;L2=10;theta1=0;theta2=0; 
    };
    Robot::Robot(double l1,double l2,double t1,double t2){
    L1=l1;L2=l2;theta1=t1*Dk;theta2=t2*Dk;//输入为角度制角 
    }
    Vector2d Robot::getTerminal() {
    double Tx,Ty;
    Tx=L1*cos(theta1)+L2*cos(theta1+theta2);
    Ty=L1*sin(theta1)+L2*sin(theta1+theta2);
    Vector2d T(Tx,Ty);
    return T;
    }
    void Robot::ShowState(){
    cout<<"theta1="<<theta1/Dk<<endl<<"theta2="<<theta2/Dk<<endl;
    }
    void Robot::PTPmove(Frame frame,double px,double py){
    Solver solver;
    MatrixXd T(2,1);
    T=solver. CalTheta(L1,L2,frame,px,py);
    theta1=T(0,0);theta2=T(1,0);
    }
    //Robot定义结束
    
    
    int main(){
    Robot myRobot(12,12,0,30);//构造Robot对象 
    Frame TF1(1,2,30),TF2(-2,0,12),TF3(0.5,1.5,90);//定义任务坐标系 
    myRobot.ShowState();
    myRobot.PTPmove(TF1,1,1);
    myRobot.ShowState();
    myRobot.PTPmove(TF2,0,1);
    myRobot.ShowState();
    return 0;
    }
  • 相关阅读:
    c# WinForm 定时执行某个后台操作 如把B文件夹下的文件Copy到A文件夹下
    c# 创建指定大小的空字符填充的文本文件 在指定位置读写相关内容
    c# DirectShow 通过IAMVideoProcAmp的Set方法 来设置视频图像的Brightness 调整亮度
    [转]灰度图像的腐蚀算法和细化算法(C#代码)
    利用fleximage实现图片上传
    利用acts_as_ferret实现全文检索
    纯CSS无hacks的跨游览器多列布局
    IE私有CSS样式属性一览
    利用thinking sphinx实现全文检索
    搭建rails运行环境
  • 原文地址:https://www.cnblogs.com/mememagic/p/5046386.html
Copyright © 2020-2023  润新知