当前位置:   article > 正文

UR3机械臂运动学反解之解析解

ur3机械臂

UR3机械臂运动学反解之解析解

基于Qt+OpenGL的UR3机械臂仿真与控制系统(四)

反解演示
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

上面三个图片分别是UR3机械臂的逆运动学演示图片

先给定UR3机器人末端固定位置(x=250,y=0,z=450) 与固定姿态(先沿y轴转90°再沿z轴转90°),求解机械臂反解(多解),再将机械臂设置为其中三个解的位置的情况。由此验证反解的正确性。

解析解推导

在这里插入图片描述

建立的DH参数如上图
在这里插入图片描述

在这里插入图片描述

C++解析解代码

ur3_controller.h

#ifndef UR3_CONTROLLER_H
#define UR3_CONTROLLER_H

#include <Eigen/Dense>
#include <QWidget>
#include <iostream>


#define PI acos(-1)


class UR3_CONTROLLER{
public:
  std::vector<double> thetax_;
  std::vector<double> theta_record_;  //用了记录上一组角度,做动画插值
  std::vector<double> dx_;
  std::vector<double> ax_;
  std::vector<double> alphax_;
  Eigen::Matrix4d T_;
  std::vector<std::vector<double>> result_;
  double nx;double ox;double ax;double px;
  double ny;double oy;double ay;double py;
  double nz;double oz;double az;double pz;
  void initDH();
  void initDH(std::vector<float> a,
              std::vector<float> d,
              std::vector<float> alpha);
  void printfDH();
  void setTheta(double theta1,double theta2,
                double theta3,double theta4,
                double theta5,double theta6);
  void setTheta(std::vector<double> thetas);
  void setAngle(std::vector<float> angles);
  void setT(Eigen::Matrix4d T);
  Eigen::Matrix4d getTx(double a,double alpha,double d,double theta);
  void positiveKinematics();  //正运动学
  Eigen::Matrix4d positiveKinematicsMat(std::vector<double> angle);
  std::vector<double> getTheta3(double theta1,double theta5,double theta6);
  double getTheta2(double theta1,double theta3,double theta6);
  double getTheta4(double theta1,double theta2,double theta3,double theta6);
  void negitiveKinematics();  //逆运动学
  void filterSlove(bool positive);  //挑选正确的解
};
#endif // UR3_CONTROLLER_H

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45

ur3_controller.cpp

#include "./ur3_controller.h"
#include <QDebug>

void UR3_CONTROLLER::initDH(){
    //初始化DH参数列表
    dx_.push_back(151.9);
    dx_.push_back(0);
    dx_.push_back(0);
    dx_.push_back(110.4);
    dx_.push_back(83.4);
    dx_.push_back(81.4);
    ax_.push_back(0);
    ax_.push_back(0);
    ax_.push_back(243.65);
    ax_.push_back(213);
    ax_.push_back(0);
    ax_.push_back(0);
    alphax_.push_back(0);
    alphax_.push_back(PI/2);
    alphax_.push_back(0);
    alphax_.push_back(0);
    alphax_.push_back(PI/2);
    alphax_.push_back(-PI/2);
    printfDH();
}

void UR3_CONTROLLER::initDH(std::vector<float> a,
                            std::vector<float> d,
                            std::vector<float> alpha){
    //初始化DH参数列表
    for (int i=0;i<a.size();i++){
        ax_.push_back(a[i]*1000);
        dx_.push_back(d[i]*1000);
        alphax_.push_back(alpha[i]*PI/180);
    }
    printfDH();
}



void UR3_CONTROLLER::printfDH(){
    std::cout<<std::endl;
    std::cout<<"-------DH参数列表加载成功-------"<<std::endl;
    for(int i=0;i<ax_.size();i++){
        std::cout<<"d"<<i<<":"<<dx_[i]<<"  a"<<i<<":"<<ax_[i]
                 <<"  alpha"<<i<<":"<<alphax_[i];
        if (i != ax_.size()-1)
            std::cout<<std::endl;
    }
    std::cout<<std::endl;
    std::cout<<"------------------------------"<<std::endl;
    std::cout<<std::endl;
}


void UR3_CONTROLLER::setTheta(double theta1,double theta2,
                              double theta3,double theta4,
                              double theta5,double theta6){
    if (thetax_.size()!=6){
        thetax_.clear();
        thetax_.push_back(theta1);
        thetax_.push_back(theta2);
        thetax_.push_back(theta3);
        thetax_.push_back(theta4);
        thetax_.push_back(theta5);
        thetax_.push_back(theta6);
    }
    else{
        thetax_[0] = theta1;
        thetax_[1] = theta2;
        thetax_[2] = theta3;
        thetax_[3] = theta4;
        thetax_[4] = theta5;
        thetax_[5] = theta6;
    }
}


void UR3_CONTROLLER::setTheta(std::vector<double> thetas){
    thetax_.clear();
    for (int i=0;i<thetas.size();i++)
        thetax_.push_back(thetas[i]);
}


void UR3_CONTROLLER::setAngle(std::vector<float> angles){
    thetax_.clear();
    for (int i=0;i<angles.size();i++)
        thetax_.push_back(angles[i]*PI/180);
}


Eigen::Matrix4d UR3_CONTROLLER::getTx(double a,double alpha,double d,double theta){
    Eigen::Matrix4d Tx;
    double T11 = cos(theta);
    double T12 = -1*sin(theta);
    double T14 = a;
    double T21 = sin(theta)*cos(alpha);
    double T22 = cos(theta)*cos(alpha);
    double T23 = -1*sin(alpha);
    double T24 = -1*d*sin(alpha);
    double T31 = sin(theta)*sin(alpha);
    double T32 = cos(theta)*sin(alpha);
    double T33 = cos(alpha);
    double T34 = d*cos(alpha);
    Tx << T11  ,T12  ,0    ,T14  ,
          T21  ,T22  ,T23  ,T24  ,
          T31  ,T32  ,T33  ,T34  ,
            0  ,  0  ,  0  ,  1;
    Eigen::Matrix4d mat1;
    mat1<<1,0,0,a,
          0,cos(alpha),-1*sin(alpha),0,
          0,sin(alpha),cos(alpha),0,
          0,0,0,1;
    Eigen::Matrix4d mat2;
    mat2<<cos(theta),-1*sin(theta),0,0,
              sin(theta),cos(theta),0,0,
              0,0,1,d,
              0,0,0,1;
    return mat1*mat2;
}

void UR3_CONTROLLER::setT(Eigen::Matrix4d T){
    T_ = T;
    nx = T_(0,0);ox = T_(0,1);ax = T_(0,2);px = T_(0,3);
    ny = T_(1,0);oy = T_(1,1);ay = T_(1,2);py = T_(1,3);
    nz = T_(2,0);oz = T_(2,1);az = T_(2,2);pz = T_(2,3);
}

//正运动学
void UR3_CONTROLLER::positiveKinematics(){
    Eigen::Matrix4d T10 = getTx(ax_[0],alphax_[0],dx_[0],thetax_[0]);
    Eigen::Matrix4d T21 = getTx(ax_[1],alphax_[1],dx_[1],thetax_[1]);
    Eigen::Matrix4d T32 = getTx(ax_[2],alphax_[2],dx_[2],thetax_[2]);
    Eigen::Matrix4d T43 = getTx(ax_[3],alphax_[3],dx_[3],thetax_[3]);
    Eigen::Matrix4d T54 = getTx(ax_[4],alphax_[4],dx_[4],thetax_[4]);
    Eigen::Matrix4d T65 = getTx(ax_[5],alphax_[5],dx_[5],thetax_[5]);
    T_ = T10*T21*T32*T43*T54*T65;
    nx = T_(0,0);ox = T_(0,1);ax = T_(0,2);px = T_(0,3);
    ny = T_(1,0);oy = T_(1,1);ay = T_(1,2);py = T_(1,3);
    nz = T_(2,0);oz = T_(2,1);az = T_(2,2);pz = T_(2,3);
    //std::cout<<"正运动学解:"<<T_<<std::endl;
}

//逆运动学
void UR3_CONTROLLER::negitiveKinematics(){
    result_.clear();
    //求解theta1
    double m = py-dx_[5]*ay;
    double n = dx_[5]*ax-px;
    double phi = atan2(m,n);
    double theta11 = std::atan2(-1*dx_[3],sqrt(m*m+n*n-dx_[3]*dx_[3]))-phi;
    double theta12 = std::atan2(-1*dx_[3],-1*sqrt(m*m+n*n-dx_[3]*dx_[3]))-phi;
    if (theta11<-1*PI){
        theta11 = theta11+2*PI;
    }
    else if(theta11>PI){
        theta11 = theta11-2*PI;
    }
    if (theta12<-1*PI){
        theta12 = theta12+2*PI;
    }
    else if (theta12>PI){
        theta12 = theta12-2*PI;
    }
    double angle11 = theta11*180/PI;
    double angle12 = theta12*180/PI;
//    std::cout<<"angle11:"<<angle11<<std::endl;
//    std::cout<<"angle12:"<<angle12<<std::endl;

    //求解theta5,一个theta1对应两个theta5
    double theta51 = acos(sin(theta11)*ax-cos(theta11)*ay);
    double theta52 = -1*acos(sin(theta11)*ax-cos(theta11)*ay);
    double theta53 = acos(sin(theta12)*ax-cos(theta12)*ay);
    double theta54 = -1*acos(sin(theta12)*ax-cos(theta12)*ay);
    double angle51 = theta51*180/PI;
    double angle52 = theta52*180/PI;
    double angle53 = theta53*180/PI;
    double angle54 = theta54*180/PI;
//    std::cout<<"angle51:"<<angle51<<std::endl;
//    std::cout<<"angle52:"<<angle52<<std::endl;
//    std::cout<<"angle53:"<<angle53<<std::endl;
//    std::cout<<"angle54:"<<angle54<<std::endl;

    //求解theta6,一个theta1对应一个theta6,由atan()计算所得的角都应该注意值域的问题
    double theta61,theta62,theta63,theta64;

    double fenzi1 = (-1*sin(theta11)*ox+cos(theta11)*oy)/sin(theta51);
    double fenmu1 = (sin(theta11)*nx-cos(theta11)*ny)/(sin(theta51));
//    std::cout<<"fenzi1:"<<fenzi1<<std::endl;
//    std::cout<<"fenmu1:"<<fenmu1<<std::endl;
    theta61 = std::atan2(fenzi1,fenmu1);

    double fenzi2 = (-1*sin(theta11)*ox+cos(theta11)*oy)/sin(theta52);
    double fenmu2 = (sin(theta11)*nx-cos(theta11)*ny)/(sin(theta52));
//    std::cout<<"fenzi2:"<<fenzi2<<std::endl;
//    std::cout<<"fenmu2:"<<fenmu2<<std::endl;
    theta62 = std::atan2(fenzi2,fenmu2);

    double fenzi3 = (-1*sin(theta12)*ox+cos(theta12)*oy)/sin(theta53);
    double fenmu3 = (sin(theta12)*nx-cos(theta12)*ny)/(sin(theta53));
    theta63 = std::atan2(fenzi3,fenmu3);

    double fenzi4 = (-1*sin(theta12)*ox+cos(theta12)*oy)/sin(theta54);
    double fenmu4 = (sin(theta12)*nx-cos(theta12)*ny)/(sin(theta54));
    theta64 = std::atan2(fenzi4,fenmu4);

    double angle61 = theta61*180/PI;
    double angle62 = theta62*180/PI;
    double angle63 = theta63*180/PI;
    double angle64 = theta64*180/PI;
//    std::cout<<"angle61:"<<angle61<<std::endl;
//    std::cout<<"angle62:"<<angle62<<std::endl;
//    std::cout<<"angle63:"<<angle63<<std::endl;
//    std::cout<<"angle64:"<<angle64<<std::endl;

    //求解theta3
    std::vector<double> theta3;  //中间变量暂存返回结果
    theta3 = getTheta3(theta11,theta51,theta61);
    double theta31 = theta3[0];
    double theta32 = theta3[1];
    double angle31 = theta31*180/PI;
    double angle32 = theta32*180/PI;
    theta3 = getTheta3(theta11,theta52,theta62);
    double theta33 = theta3[0];
    double theta34 = theta3[1];
    double angle33 = theta33*180/PI;
    double angle34 = theta34*180/PI;
    theta3 = getTheta3(theta12,theta53,theta63);
    double theta35 = theta3[0];
    double theta36 = theta3[1];
    double angle35 = theta35*180/PI;
    double angle36 = theta36*180/PI;
    theta3 = getTheta3(theta12,theta54,theta64);
    double theta37 = theta3[0];
    double theta38 = theta3[1];
    double angle37 = theta37*180/PI;
    double angle38 = theta38*180/PI;
//    std::cout<<"angle31:"<<angle31<<std::endl;
//    std::cout<<"angle32:"<<angle32<<std::endl;
//    std::cout<<"angle33:"<<angle33<<std::endl;
//    std::cout<<"angle34:"<<angle34<<std::endl;
//    std::cout<<"angle35:"<<angle35<<std::endl;
//    std::cout<<"angle36:"<<angle36<<std::endl;
//    std::cout<<"angle37:"<<angle37<<std::endl;
//    std::cout<<"angle38:"<<angle38<<std::endl;

    //求解theta2
    double theta21 = getTheta2(theta11,theta31,theta61);
    double theta22 = getTheta2(theta11,theta32,theta61);
    double theta23 = getTheta2(theta11,theta33,theta62);
    double theta24 = getTheta2(theta11,theta34,theta62);
    double theta25 = getTheta2(theta12,theta35,theta63);
    double theta26 = getTheta2(theta12,theta36,theta63);
    double theta27 = getTheta2(theta12,theta37,theta64);
    double theta28 = getTheta2(theta12,theta38,theta64);
    double angle21 = theta21*180/PI;
    double angle22 = theta22*180/PI;
    double angle23 = theta23*180/PI;
    double angle24 = theta24*180/PI;
    double angle25 = theta25*180/PI;
    double angle26 = theta26*180/PI;
    double angle27 = theta27*180/PI;
    double angle28 = theta28*180/PI;
//    std::cout<<"angle21:"<<angle21<<std::endl;
//    std::cout<<"angle22:"<<angle22<<std::endl;
//    std::cout<<"angle23:"<<angle23<<std::endl;
//    std::cout<<"angle24:"<<angle24<<std::endl;
//    std::cout<<"angle25:"<<angle25<<std::endl;
//    std::cout<<"angle26:"<<angle26<<std::endl;
//    std::cout<<"angle27:"<<angle27<<std::endl;
//    std::cout<<"angle28:"<<angle28<<std::endl;

    //求解theta4
    double theta41 = getTheta4(theta11,theta21,theta31,theta61);
    double theta42 = getTheta4(theta11,theta22,theta32,theta61);
    double theta43 = getTheta4(theta11,theta23,theta33,theta62);
    double theta44 = getTheta4(theta11,theta24,theta34,theta62);
    double theta45 = getTheta4(theta12,theta25,theta35,theta63);
    double theta46 = getTheta4(theta12,theta26,theta36,theta63);
    double theta47 = getTheta4(theta12,theta27,theta37,theta64);
    double theta48 = getTheta4(theta12,theta28,theta38,theta64);
    double angle41 = theta41*180/PI;
    double angle42 = theta42*180/PI;
    double angle43 = theta43*180/PI;
    double angle44 = theta44*180/PI;
    double angle45 = theta45*180/PI;
    double angle46 = theta46*180/PI;
    double angle47 = theta47*180/PI;
    double angle48 = theta48*180/PI;
//    std::cout<<"angle41:"<<angle41<<std::endl;
//    std::cout<<"angle42:"<<angle42<<std::endl;
//    std::cout<<"angle43:"<<angle43<<std::endl;
//    std::cout<<"angle44:"<<angle44<<std::endl;
//    std::cout<<"angle45:"<<angle45<<std::endl;
//    std::cout<<"angle46:"<<angle46<<std::endl;
//    std::cout<<"angle47:"<<angle47<<std::endl;
//    std::cout<<"angle48:"<<angle48<<std::endl;
    //压入结果容器
    result_.clear();
    std::vector<double> middle_result;
    middle_result.push_back(angle11);middle_result.push_back(angle21);middle_result.push_back(angle31);
    middle_result.push_back(angle41);middle_result.push_back(angle51);middle_result.push_back(angle61);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle22);middle_result.push_back(angle32);
    middle_result.push_back(angle42);middle_result.push_back(angle51);middle_result.push_back(angle61);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle23);middle_result.push_back(angle33);
    middle_result.push_back(angle43);middle_result.push_back(angle52);middle_result.push_back(angle62);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle24);middle_result.push_back(angle34);
    middle_result.push_back(angle44);middle_result.push_back(angle52);middle_result.push_back(angle62);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle25);middle_result.push_back(angle35);
    middle_result.push_back(angle45);middle_result.push_back(angle53);middle_result.push_back(angle63);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle26);middle_result.push_back(angle36);
    middle_result.push_back(angle46);middle_result.push_back(angle53);middle_result.push_back(angle63);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle27);middle_result.push_back(angle37);
    middle_result.push_back(angle47);middle_result.push_back(angle54);middle_result.push_back(angle64);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle28);middle_result.push_back(angle38);
    middle_result.push_back(angle48);middle_result.push_back(angle54);middle_result.push_back(angle64);
    result_.push_back(middle_result);
    middle_result.clear();

    //输出结果(可能含有不正确的解)
    for (int i=0;i<result_.size();i++){
        std::cout<<"第"<<i<<"组解:";
        for (int j=0;j<result_[i].size();j++){
            std::cout<<"   "<<result_[i][j];
        }
        std::cout<<std::endl;
    }
    std::cout<<std::endl;
}


//求解theta3
std::vector<double> UR3_CONTROLLER::getTheta3(double theta1,double theta5,double theta6){
    std::vector<double> theta3;  // 存放运算结果
    double a = dx_[4]*(sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)+cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy))-dx_[5]*(cos(theta1)*ax+sin(theta1)*ay)+cos(theta1)*px+sin(theta1)*py;
    double b = dx_[4]*(sin(theta6)*nz+cos(theta6)*oz)-dx_[5]*az+pz-dx_[0];

    double fenzi = (a*a+b*b-ax_[3]*ax_[3]-ax_[2]*ax_[2]);
    double fenmu = (2*ax_[2]*ax_[3]);

    //std::cout<<"fenzi2:"<<fenzi2/fenmu2<<std::endl;

    double theta31 = acos(fenzi/fenmu);
    double theta32 = -1*acos(fenzi/fenmu);
    theta3.clear();
    theta3.push_back(theta31);
    theta3.push_back(theta32);
    return theta3;
}


//求解theta2
double UR3_CONTROLLER::getTheta2(double theta1,double theta3,double theta6){
    double a = dx_[4]*(sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)+cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy))-dx_[5]*(cos(theta1)*ax+sin(theta1)*ay)+cos(theta1)*px+sin(theta1)*py;
    double b = dx_[4]*(sin(theta6)*nz+cos(theta6)*oz)-dx_[5]*az+pz-dx_[0];
    double fenzi = (b*(ax_[3]*cos(theta3)+ax_[2])-ax_[3]*sin(theta3)*a)/(ax_[2]*ax_[2]+ax_[3]*ax_[3]+2*ax_[2]*ax_[3]*cos(theta3));
    double fenmu = (a*(ax_[3]*cos(theta3)+ax_[2])+ax_[3]*sin(theta3)*b)/(ax_[2]*ax_[2]+ax_[3]*ax_[3]+2*ax_[2]*ax_[3]*cos(theta3));
    double theta2 = std::atan2(fenzi,fenmu);
    return theta2;
}


//求解theta4
double UR3_CONTROLLER::getTheta4(double theta1,double theta2,double theta3,double theta6){
    double nx = T_(0,0);double ny = T_(1,0);double nz = T_(2,0);
    double ox = T_(0,1);double oy = T_(1,1);double oz = T_(2,1);
    double fenzi = -1*sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)-cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy);
    double fenmu = nz*sin(theta6)+oz*cos(theta6);
    //因为atan(x)所求的结果落在-PI/2到PI/2上,theta2+theta3+theta4显然不一定落在此区域上
    double theta234;
    if ((fenzi>0 && fenmu<0)){
        theta234 = PI+atan(fenzi/fenmu);
    }
    else if (fenzi<0 && fenmu<0){
        theta234 = atan(fenzi/fenmu)-PI;
    }
    else{
        theta234 = atan(fenzi/fenmu);
    }
    //因为theta2+theta3+theta4的取值范围可以到[-540,540] (该范围可以在后期传入范围参数确定)
    double theta4;
    //尝试覆盖[-540,540]所有theta2+theta3+theta4的可能性
    if (2*PI+theta234-theta2-theta3<PI && 2*PI+theta234>-1*PI){
        theta4 = theta234+2*PI-theta2-theta3;
    }
    else if (-2*PI+theta234-theta2-theta3<PI && -2*PI+theta234>-1*PI){
        theta4 = theta234-2*PI-theta2-theta3;
    }
    else
        theta4 = theta234-theta2-theta3;
    return theta4;
}

Eigen::Matrix4d UR3_CONTROLLER::positiveKinematicsMat(std::vector<double> angle){
    double theta1 = angle[0]*PI/180;
    double theta2 = angle[1]*PI/180;
    double theta3 = angle[2]*PI/180;
    double theta4 = angle[3]*PI/180;
    double theta5 = angle[4]*PI/180;
    double theta6 = angle[5]*PI/180;
    Eigen::Matrix4d T10 = getTx(ax_[0],alphax_[0],dx_[0],theta1);
    Eigen::Matrix4d T21 = getTx(ax_[1],alphax_[1],dx_[1],theta2);
    Eigen::Matrix4d T32 = getTx(ax_[2],alphax_[2],dx_[2],theta3);
    Eigen::Matrix4d T43 = getTx(ax_[3],alphax_[3],dx_[3],theta4);
    Eigen::Matrix4d T54 = getTx(ax_[4],alphax_[4],dx_[4],theta5);
    Eigen::Matrix4d T65 = getTx(ax_[5],alphax_[5],dx_[5],theta6);
    return T10*T21*T32*T43*T54*T65;
}


void UR3_CONTROLLER::filterSlove(bool positive){
    if (positive == false){//如果是逆运动学
        std::vector<std::vector<double>> right_result;
        for (int i=0;i<result_.size();i++){
            bool status = true;
            for (int j=0;j<result_[i].size();j++){  //检查每个元素的取值
                if (status == false)
                    break;
                if (result_[i][j]<180&&result_[i][j]>-1*180){
                }
                else{
                    status = false;
                }
            }
            if (status == true){
                right_result.push_back(result_[i]);
            }
        }
        result_.clear();
        result_ = right_result;
    }
    else{  //如果是正运动学
        std::vector<std::vector<double>> right_result;
        std::cout<<"输出矩阵查看问题:"<<T_<<std::endl;
        for (int i=0;i<result_.size();i++){
            Eigen::Matrix4d T = positiveKinematicsMat(result_[i]);
            std::cout<<"对比矩阵:"<<T<<std::endl;
            //计算两矩阵偏差
            double dv = pow(T(0,0)-T_(0,0),2)+pow(T(0,1)-T_(0,1),2)+pow(T(0,2)-T_(0,2),2)+pow(T(0,3)-T_(0,3),2)
                       +pow(T(1,0)-T_(1,0),2)+pow(T(1,1)-T_(1,1),2)+pow(T(1,2)-T_(1,2),2)+pow(T(1,3)-T_(1,3),2)
                       +pow(T(2,0)-T_(2,0),2)+pow(T(2,1)-T_(2,1),2)+pow(T(2,2)-T_(2,2),2)+pow(T(2,3)-T_(2,3),2);
            if (dv<0.1){
//                std::cout<<"第"<<i<<"组解正确"<<std::endl;
                right_result.push_back(result_[i]);
            }
        }
        result_.clear();
        result_ = right_result;
    }
    //输出正确结果
    for (int i=0;i<result_.size();i++){
        std::cout<<"第"<<i<<"组解:";
        for (int j=0;j<result_[i].size();j++){
            std::cout<<"   "<<result_[i][j];
        }
        std::cout<<std::endl;
    }
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221
  • 222
  • 223
  • 224
  • 225
  • 226
  • 227
  • 228
  • 229
  • 230
  • 231
  • 232
  • 233
  • 234
  • 235
  • 236
  • 237
  • 238
  • 239
  • 240
  • 241
  • 242
  • 243
  • 244
  • 245
  • 246
  • 247
  • 248
  • 249
  • 250
  • 251
  • 252
  • 253
  • 254
  • 255
  • 256
  • 257
  • 258
  • 259
  • 260
  • 261
  • 262
  • 263
  • 264
  • 265
  • 266
  • 267
  • 268
  • 269
  • 270
  • 271
  • 272
  • 273
  • 274
  • 275
  • 276
  • 277
  • 278
  • 279
  • 280
  • 281
  • 282
  • 283
  • 284
  • 285
  • 286
  • 287
  • 288
  • 289
  • 290
  • 291
  • 292
  • 293
  • 294
  • 295
  • 296
  • 297
  • 298
  • 299
  • 300
  • 301
  • 302
  • 303
  • 304
  • 305
  • 306
  • 307
  • 308
  • 309
  • 310
  • 311
  • 312
  • 313
  • 314
  • 315
  • 316
  • 317
  • 318
  • 319
  • 320
  • 321
  • 322
  • 323
  • 324
  • 325
  • 326
  • 327
  • 328
  • 329
  • 330
  • 331
  • 332
  • 333
  • 334
  • 335
  • 336
  • 337
  • 338
  • 339
  • 340
  • 341
  • 342
  • 343
  • 344
  • 345
  • 346
  • 347
  • 348
  • 349
  • 350
  • 351
  • 352
  • 353
  • 354
  • 355
  • 356
  • 357
  • 358
  • 359
  • 360
  • 361
  • 362
  • 363
  • 364
  • 365
  • 366
  • 367
  • 368
  • 369
  • 370
  • 371
  • 372
  • 373
  • 374
  • 375
  • 376
  • 377
  • 378
  • 379
  • 380
  • 381
  • 382
  • 383
  • 384
  • 385
  • 386
  • 387
  • 388
  • 389
  • 390
  • 391
  • 392
  • 393
  • 394
  • 395
  • 396
  • 397
  • 398
  • 399
  • 400
  • 401
  • 402
  • 403
  • 404
  • 405
  • 406
  • 407
  • 408
  • 409
  • 410
  • 411
  • 412
  • 413
  • 414
  • 415
  • 416
  • 417
  • 418
  • 419
  • 420
  • 421
  • 422
  • 423
  • 424
  • 425
  • 426
  • 427
  • 428
  • 429
  • 430
  • 431
  • 432
  • 433
  • 434
  • 435
  • 436
  • 437
  • 438
  • 439
  • 440
  • 441
  • 442
  • 443
  • 444
  • 445
  • 446
  • 447
  • 448
  • 449
  • 450
  • 451
  • 452
  • 453
  • 454
  • 455
  • 456
  • 457
  • 458
  • 459
  • 460
  • 461
  • 462
  • 463
  • 464
  • 465
  • 466
  • 467
  • 468
  • 469
  • 470
  • 471
  • 472
  • 473
  • 474
  • 475
  • 476
  • 477
  • 478
  • 479
  • 480

完整代码

运动学代码

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/秋刀鱼在做梦/article/detail/1008093
推荐阅读
相关标签
  

闽ICP备14008679号