puma560的运动学正解和逆解的C++源码.cpp

#include “mainwindow.h“
#include “ui_mainwindow.h“
#include 
#include 
#include 
using namespace std;
using namespace Eigen;
#define PI 3.1415926
//*q是六个关节角的指针，*T是末端位姿的指针 4*4
void forward（const double* q double* T）
{
    //C++里的三角函数用的是弧度
    double jointnum[6];
    double s1 = sin（*q） c1 = cos（*q）; q++;
    double s2 = sin（*q） c2 = cos（*q）; q++;
    double s3 = sin（*q） c3 = cos（*q）; q++;
    double s4 = sin（*q） c4 = cos（*q）; q++;
    double s5 = sin（*q） c5 = cos（*q）; q++;
    double s6 = sin（*q） c6 = cos（*q）; q++;

    Eigen::Matrix T01T12T23T34T45T56T6;

    T01 << c1 -s1 00
            s1 c1 0 0
            0010
            0001;  //输入数据（初始化）
    T12 << c2 -s2 0 0
            001149.09
            -s2 -c200
            0001;
    T23 << c3 -s3 0431.8
            s3 c3 0 0
            0010
            0001;
    T34 << c4 -s4 0 20.32
            001433.07
            -s4 -c400
            0001;
    T45 << c5 -s5 0 0
            00-10
            s5 c500
            0001;
    T56 << c6 -s6 0 0
            0010
            -s6 -c600
            0001;
    T6=T01*T12*T23*T34*T45*T56;
    for （int i=0; i<4; i++） {
        for （int j=0; j<4; j++）
            {
                cout<                //把计算的末端数值赋值给*T
                *T=T6（ij）;
                T++;
            }
        cout<    }


}

void inverse（）
{
	//，T06是末端位姿 4*4
    //赋初值
    Eigen::Matrix T06;
    T06<< 0.00001.00000.0000-149.0900
            0.0000-0.00001.0000864.8700
            1.00000-0.000020.3200
            0001.0000;
    double a0=0;
    double a1=0;
    double  a2=431.8;
    double a3=20.32;
    double a4=0;
    double a5=0;
    double d1=0;
    double d2=149.09;
    double d3=0;
    double d4=433.07;
    double d5=0;
    double d6=0;
    double n_xn_yn_zo_xo_yo_za_xa_ya_zp_xp_yp_z;
    n_x=T06（00）;   n_y=T06（10）;  n_z=T06（20）;
    o_x=T06（01）;   o_y=T06（11）;  o_z=T06（21）;
    a_x=T06（02）;   a_y=T06（12）;  a_z=T06（22）;
    p_x=T06（03）;  p_y=T06（13）;   p_z=T06（23）;

    for（int i =0;i<2;i++）
        for（int j =0;j<2;j++）
            for（int k =0;k<2;k++）
            {
                {
                    {
                        //求解theta1（为弧度）
                        double sqr1[2]ta1;
                        sqr1[0]=sqrt（p_x*p_x+p_y*p_y-d2*d2）;
                        sqr1[1]=-sqrt（p_x*p_x+p_y*p_y-d2*d2）;
                        ta1=atan2（p_yp_x）-atan2（d2sqr1[i]）;
                        //求解theta3（弧度表示）
                        double k1;
                        double sqr3[2];
                        double ta3fs23sc23ta23;
                        k1=（p_x*p_x+p_y*p_y+p_z*p_z-a2*a2-a3*a3-d2*d2-d4*d4）/（2*a2）;
                        sqr3[0]=s