【RBDL】——逆动力学计算代码

说明：使用RBDL库对PUMA560进行逆动力学计算

#include <iostream>
#include <fstream>
#include <rbdl/rbdl.h>
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;


int main(int argc, char* argv[]) {
	// 初始化模型
	Model* model = NULL;
	model = new Model();
	model->gravity = Vector3d(0., 0., -9.81);

	// 建立body和joint
	unsigned int body_1_id, body_2_id, body_3_id, body_4_id, body_5_id, body_6_id;
	Body body_1, body_2, body_3, body_4, body_5, body_6;
	Joint joint_1, joint_2, joint_3, joint_4, joint_5, joint_6;
	SpatialTransform transform1, transform2, transform3, transform4, transform5, transform6;
	
	// joint 1
	// 给定连杆的属性：质量参数、质心坐标、相对于质心坐标系下的惯性矩阵
	body_1 = Body(1.5169716e02, Vector3d(0.0, 1.9711689e-01, 0.0), Vector3d(4.8657650e0, 8.0656606e-1, 4.8386171e0));
	joint_1 = Joint(JointTypeRevoluteZ); // z轴旋转——给定关节的信息：关节类型、运动轴
	body_1_id = model->AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_1, body_1); // 父类id（base=0），关节之间的坐标转换矩阵6*6，关节模型、连杆属性

	// joint 2
	Matrix3d Ic2 = { 1.1099332e-1, -2.1408301e-06,  0.0000000e+00,
					-2.1408301e-06,  1.4242639e0,  0.0000000e+00,
					0.0000000e+00,  0.0000000e+00,  1.5175183e0 };
	body_2 = Body(4.2573547e01, Vector3d(2.1602233e-01, 0.0, 5.2500000e-01), Ic2);
	joint_2 = Joint(JointTypeRevoluteZ);
	body_2_id = model->AddBody(body_1_id, Xrotx(-M_PI/2.0), joint_2, body_2);

	// joint 3
	Matrix3d Ic3 = { 1.2572857e-1, -2.5635707e-2,  0.0000000e+00,
					-2.5635707e-2,  3.0412096e-2,  0.0000000e+00,
					0.0000000e+00,  0.0000000e+00,  1.3991337e-1 };
	body_3 = Body(1.5016164e01, Vector3d(2.7797697e-02, 1.1158399e-01, 0.0), Ic3);
	joint_3 = Joint(JointTypeRevoluteZ);
	body_3_id = model->AddBody(body_2_id, Xtrans(Vector3d(0.5, 0., 0.475)), joint_3, body_3);

	// joint 4
	body_4 = Body(1.3394044e01, Vector3d(0.0, 0.0, -8.5230887e-02), Vector3d(3.8447017e-2, 4.0217134e-2, 3.5221322e-2));
	joint_4 = Joint(JointTypeRevoluteZ);
	transform4 = Xrotx(-M_PI / 2.0); transform4.r = Vector3d(0.050, 0.370, 0.0);
	body_4_id = model->AddBody(body_3_id, transform4, joint_4, body_4);

	// joint 5
	body_5 = Body(4.0643721e0, Vector3d(0.0, 4.3130210e-02, 0.0), Vector3d(7.3768163e-3, 2.9234594e-3, 7.8628976e-3));
	joint_5 = Joint(JointTypeRevoluteZ);
	transform5 = Xrotx(M_PI / 2.0);
	body_5_id = model->AddBody(body_4_id, transform5, joint_5, body_5);

	// joint 6
	body_6 = Body(7.2366096e-01, Vector3d(0.0, 0.0, 1.1679712e-01), Vector3d(2.3002405e-04, 2.3002400e-04, 3.0415291e-04));
	joint_6 = Joint(JointTypeRevoluteZ);
	transform6 = Xrotx(-M_PI / 2.0);
	body_6_id = model->AddBody(body_5_id, transform6, joint_6, body_6);


	// 动力学变量
	VectorNd Q = VectorNd::Zero(model->dof_count);
	VectorNd QDot = VectorNd::Zero(model->dof_count);
	VectorNd QDDot = VectorNd::Zero(model->dof_count);
	VectorNd Tau = VectorNd::Zero(model->dof_count);


	ofstream file;
	file.open("data.txt");

	// 输入关节角度
	double a0, a2, a3;
	for (int i = 0; i < 1000; i++)
	{
		// 关节输入
		a0 = 0.0; a2 = (3.0 / 999.0 / 999.0) * (150.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (150.0 - a0);
		Q[0] = (a0 + a2 * i *i + a3 * i *i*i) / 180.0 * M_PI;
		QDot[0] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i *i) / 180.0 * M_PI;
		QDDot[0] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;

		a0 = 10.0; a2 = (3.0 / 999.0 / 999.0) * (80.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (80.0 - a0);
		Q[1] = (a0 + a2 * i *i + a3 * i * i * i) / 180.0 * M_PI;
		QDot[1] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i * i) / 180.0 * M_PI;
		QDDot[1] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;
		
		a0 = 0.0; a2 = (3.0 / 999.0 / 999.0) * (80.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (80.0 - a0);
		Q[2] = (a0 + a2 * i * i + a3 * i * i * i) / 180.0 * M_PI;
		QDot[2] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i * i) / 180.0 * M_PI;
		QDDot[2] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;

		a0 = 0.0; a2 = (3.0 / 999.0 / 999.0) * (-100.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (-100.0 - a0);
		Q[3] = (a0 + a2 * i * i + a3 * i * i * i) / 180.0 * M_PI;
		QDot[3] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i * i) / 180.0 * M_PI;
		QDDot[3] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;

		a0 = 0.0; a2 = (3.0 / 999.0 / 999.0) * (30.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (30.0 - a0);
		Q[4] = (a0 + a2 * i * i + a3 * i * i * i) / 180.0 * M_PI;
		QDot[4] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i * i) / 180.0 * M_PI;
		QDDot[4] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;

		a0 = 0.0; a2 = (3.0 / 999.0 / 999.0) * (50.0 - a0); a3 = -(2.0 / 999.0 / 999.0 / 999.0) * (50.0 - a0);
		Q[5] = (a0 + a2 * i * i + a3 * i * i * i) / 180.0 * M_PI;
		QDot[5] = (0.0 + 2.0 * a2 * i + 3.0 * a3 * i * i) / 180.0 * M_PI;
		QDDot[5] = (0.0 + 2.0 * a2 + 6.0 * a3 * i) / 180.0 * M_PI;

		// 动力学计算
		InverseDynamics(*model, Q, QDot, QDDot, Tau);
		file << Tau.transpose() << endl;
		std::cout << Tau.transpose() << std::endl;
	}

	file.close();
	delete model;
	return 0;
}