#include?“mainwindow.h“
#include?“ui_mainwindow.h“
#include?
#include?
#include?
using?namespace?std;
using?namespace?Eigen;
#define?PI?3.1415926
//*q是六個(gè)關(guān)節(jié)角的指針，*T是末端位姿的指針?4*4
void?forward（const?double*?q?double*?T）
{
????//C++里的三角函數(shù)用的是弧度
????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?<????????????s1?c1?0?0
????????????0010
????????????0001;??//輸入數(shù)據(jù)（初始化）
????T12?<????????????001149.09
????????????-s2?-c200
????????????0001;
????T23?<????????????s3?c3?0?0
????????????0010
????????????0001;
????T34?<????????????001433.07
????????????-s4?-c400
????????????0001;
????T45?<????????????00-10
????????????s5?c500
????????????0001;
????T56?<????????????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<????????????????//把計(jì)算的末端數(shù)值賦值給*T
????????????????*T=T6（ij）;
????????????????T++;
????????????}
????????cout<????}


}

void?inverse（）
{
	//，T06是末端位姿?4*4
????//賦初值
????Eigen::Matrix?T06;
????T06<????????????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