基于Eigen库的TX90正逆运动学求解

Eigen库可以实现matlab上类似的矩阵运算，非常方便，相关介绍参考后边链接，机器人正逆运动学求解过程可参考我之前的博客，逆运动学在一定范围内基本正确，但还需要完善，以下是相关代码可供参考：
正运动学实现代码：

//师姐的参数，TX90，使用标准DH参数
void TX90robot::forward()
{
	
	float theta1 = ntheta1*PI/180;
	float theta2 = (ntheta2-90)*PI/180;  
	float theta3 = (ntheta3+90)*PI/180;
	float theta4 = ntheta4*PI/180;
	float theta5 = ntheta5*PI/180;
	float theta6 = ntheta6*PI/180;


	float alpha1 =-PI/2; float alpha2 = 0; float alpha3 = PI/2; float alpha4 = -PI/2; float alpha5 = PI/2; float alpha6 = 0;
	float d1 = 0;float d2 = 0; float d3 = 50;float d4 = 425;float d5 = 0;float d6 = 100;
	float a1 =50;float a2 =425;float a3 =0; float a4 = 0;float a5 = 0; float a6 = 0;

	Matrix4d T1; 
	T1 <<  cos(theta1)  ,  -sin(theta1)*cos(alpha1)  , sin(theta1)*sin(alpha1) , a1*cos(theta1),
		 sin(theta1) , cos(theta1)*cos(alpha1) , -cos(theta1)*sin(alpha1), a1*sin(theta1),
		  0  ,  sin(alpha1)  ,  cos(alpha1) ,  d1  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T2; 
	T2 <<  cos(theta2)  ,  -sin(theta2)*cos(alpha2)  , sin(theta2)*sin(alpha2) , a2*cos(theta2),
		sin(theta2) , cos(theta2)*cos(alpha2) , -cos(theta2)*sin(alpha2), a2*sin(theta2),
		0  ,  sin(alpha2)  ,  cos(alpha2) ,  d2  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T3; 
	T3 <<  cos(theta3)  ,  -sin(theta3)*cos(alpha3)  , sin(theta3)*sin(alpha3) , a3*cos(theta3),
		sin(theta3) , cos(theta3)*cos(alpha3) , -cos(theta3)*sin(alpha3), a3*sin(theta3),
		0  ,  sin(alpha3)  ,  cos(alpha3) ,  d3  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T4; 
	T4 <<  cos(theta4)  ,  -sin(theta4)*cos(alpha4)  , sin(theta4)*sin(alpha4) , a4*cos(theta4),
		sin(theta4) , cos(theta4)*cos(alpha4) , -cos(theta4)*sin(alpha4), a4*sin(theta4),
		0  ,  sin(alpha4)  ,  cos(alpha4) ,  d4  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T5; 
	T5 <<  cos(theta5)  ,  -sin(theta5)*cos(alpha5)  , sin(theta5)*sin(alpha5) , a5*cos(theta5),
		sin(theta5) , cos(theta5)*cos(alpha5) , -cos(theta5)*sin(alpha5), a5*sin(theta5),
		0  ,  sin(alpha5)  ,  cos(alpha5) ,  d5  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T6; 
	T6 <<  cos(theta6)  ,  -sin(theta6)*cos(alpha6)  , sin(theta6)*sin(alpha6) , a6*cos(theta6),
		sin(theta6) , cos(theta6)*cos(alpha6) , -cos(theta6)*sin(alpha6), a6*sin(theta6),
		0  ,  sin(alpha6)  ,  cos(alpha6) ,  d6  ,
		0 ,        0   ,      0    ,1.0000;

	Matrix4d T;
	T = T1*T2*T3*T4*T5*T6;

	float nx;float ny; float nz;
	float ox;float oy;float oz;
	float ax;float ay;float az;
	float px;float py;float pz;

	nx = T(0,0);
	ny = T(1,0);
	nz = T(2,0);
	ox = T(0,1);
	oy = T(1,1);
	oz = T(2,1);
	ax = T(0,2);
	ay = T(1,2);
	az = T(2,2);
	px = T(0,3);
	py = T(1,3);
	pz = T(2,3);


	//double b;
	//b = floor(a * 10000.000f + 0.5) / 10000.000f; /*保留小数点后四位*/

	//四舍五入，保留4位
	float output[12] = {0};
	output[0] =  floor(nx * 10000.000f + 0.5) / 10000.000f;  output[1]  = floor(ny * 10000.000f + 0.5) / 10000.000f; output[2]  = floor(nz * 10000.000f + 0.5) / 10000.000f;
	output[3]  = floor(ox * 10000.000f + 0.5) / 10000.000f;  output[4]  = floor(oy * 10000.000f + 0.5) / 10000.000f; output[5]  = floor(oz * 10000.000f + 0.5) / 10000.000f;
	output[6]  = floor(ax * 10000.000f + 0.5) / 10000.000f;  output[7]  = floor(ay * 10000.000f + 0.5) / 10000.000f; output[8]  = floor(az * 10000.000f + 0.5) / 10000.000f;
	output[9]  = floor(px * 10000.000f + 0.5) / 10000.000f;  output[10]  = floor(py * 10000.000f + 0.5) / 10000.000f; output[11]  = floor(pz * 10000.000f + 0.5) / 10000.000f;
	
	//int i;
	//double epsilon = 1.0e-6;
	//for (i = 0 ;i<12 ;i++ )
	//{
	//	if ((epsilon >=output[i] )&&(-epsilon<=output[i]))
	//	{
	//		output[i] = 0;
	//	}
	//}

	Poperator->m_edit_nx = output[0];Poperator->m_edit_ny = output[1]; Poperator->m_edit_nz = output[2];
	Poperator->m_edit_ox = output[3] ;Poperator->m_edit_oy =output[4]; Poperator->m_edit_oz = output[5];
	Poperator->m_edit_ax = output[6];Poperator->m_edit_ay = output[7]; Poperator->m_edit_az =output[8];
	Poperator->m_edit_px = output[9];Poperator->m_edit_py =output[10]; Poperator->m_edit_pz =  output[11];

}

逆运动学代码如下：

void::TX90robot::inverse()
{

	//连杆参数
	float a1 = 50;
	float a2 = 425;
	float a3 = 0;
	float a4 = 0;
	floa