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

本文介绍了如何使用Eigen库来解决TX90机器人的正逆运动学问题。通过提供的代码示例,展示了Eigen库在矩阵运算上的便利性,并引用了相关资源作为参考,以辅助理解运动学的求解过程。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

在这里插入图片描述
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
评论 4
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值