【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., Vector3d(0.0, 0.0, 0.0), Vector3d(1., 1., 1.)); // 给定连杆的属性：质量参数、质心坐标、相对于质心坐标系下的惯性矩阵
	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
	body_2 = Body(1., Vector3d(105.0, 0.0, 0.0), Vector3d(1., 1., 1.));
	joint_2 = Joint(JointTypeRevoluteZ);
	body_2_id = model->AddBody(body_1_id, Xrotx(-M_PI/2.0), joint_2, body_2);

	// joint 3
	body_3 = Body(1., Vector3d(105.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
	joint_3 = Joint(JointTypeRevoluteZ);
	body_3_id = model->AddBody(body_2_id, Xtrans(Vector3d(500.0, 0., 475.0)), joint_3, body_3);

	// joint 4
	body_4 = Body(1., Vector3d(105.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
	joint_4 = Joint(JointTypeRevoluteZ);
	transform4 = Xrotx(-M_PI / 2.0); transform4.r = Vector3d(50.0, 370.0, 0.0);
	body_4_id = model->AddBody(body_3_id, transform4, joint_4, body_4);

	// joint 5
	body_5 = Body(1., Vector3d(105.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
	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(1., Vector3d(105.0, 0.0, 0.0), Vector3d(1.0, 1.0, 1.0));
	joint_6 = Joint(JointTypeRevoluteZ);
	transform6 = Xrotx(-M_PI / 2.0);
	body_6_id = model->AddBody(body_5_id, transform6, joint_6, body_6);


	// 正运动学变量
	Vector3d EndPoint(0.0, 0.0, 100.0);
	VectorNd Q = VectorNd::Zero(model->dof_count);
	VectorNd QDot = VectorNd::Zero(model->dof_count);
	VectorNd QDDot = VectorNd::Zero(model->dof_count);

	// 逆运动学变量
	InverseKinematicsConstraintSet cs;
	VectorNd Qinit = VectorNd::Zero(model->dof_count); // 最好使用前一个点
	Qinit[1] = 10.0 / 180.0 * M_PI;
	Qinit[5] = 0.0/ 180.0 * M_PI;
	VectorNd Qres = VectorNd::Zero(model->dof_count);
	vector<unsigned int> body_id{ body_6_id };
	vector<Vector3d> body_point{ EndPoint };

	// 雅可比矩阵计算 G(q(t))
	MatrixNd G = MatrixNd::Zero(3, model->dof_count);
	MatrixNd G_6D = MatrixNd::Zero(6, 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;

		// 正运动学计算 CalcBodyToBaseCoordinates（）
			// 相对于模型最后一个关节坐标系下的坐标点Vector3d(350, 0, 0)在base坐标系下的描述
		if (false)
		{
			Vector3d result = CalcBodyToBaseCoordinates(*model, Q, body_6_id, EndPoint, true);
			file << result[0] << "  " << result[1] << "  " << result[2] << endl;
		}

		//	CalcBodyWorldOrientation()
			// 计算方向 3x3  ==> rbdl的旋转矩阵 = robotics旋转矩阵的逆（参考原始公式）
		if (false)
		{
			Matrix3d resBodyWorldOrientation = CalcBodyWorldOrientation(*model, Q, body_6_id, true);
			file << resBodyWorldOrientation(0, 0) << "  " << resBodyWorldOrientation(0, 1) << "  " << resBodyWorldOrientation(0, 2) << "  ";
			file << resBodyWorldOrientation(1, 0) << "  " << resBodyWorldOrientation(1, 1) << "  " << resBodyWorldOrientation(1, 2) << "  ";
			file << resBodyWorldOrientation(2, 0) << "  " << resBodyWorldOrientation(2, 1) << "  " << resBodyWorldOrientation(2, 2) << endl;
		}

		// CalcBaseToBodyCoordinates()
			// 在物体坐标系下，基坐标值 --> CalcBaseToBodyCoordinates() = CalcBodyToBaseCoordinates（）的逆（条件是参考坐标系的原点是一致的）
		if (false)
		{
			// 函数 {Vector3d resBodyToBaseCoordinates = CalcBodyToBaseCoordinates(*model, Q, body_3_id, Vector3d(0, 0, 0), true); } 计算结果的变换矩阵的逆运算
			Vector3d resBaseToBodyCoordinates = CalcBaseToBodyCoordinates(*model, Q, body_6_id, Vector3d(0, 0, 0), true);
			file << resBaseToBodyCoordinates[0] << "  " << resBaseToBodyCoordinates[1] << "  " << resBaseToBodyCoordinates[2] << endl;
		}


		// 逆运动学计算 InverseKinematics（）
			// 阻尼Levenberg-Marquardt方法(也称为阻尼最小二乘法)迭代计算逆运动学
		if (false)
		{
			Vector3d result = CalcBodyToBaseCoordinates(*model, Q, body_6_id, Vector3d(0.0, 0.0, 100.0), true);
			vector<Vector3d> target_pos{ result };
			bool err = InverseKinematics(*model, Qinit, body_id, body_point, target_pos, Qres);
			file << Qres[0] << "  " << Qres[1] << "  " << Qres[2] << "  " 
				<< Qres[3] << "  " << Qres[4] << "  " << Qres[5] << endl;
			//if (err)
			//{
			//	cout << "inverse kinematics: " << Qres.transpose() << endl;
			//}
			//else
			//{
			//	cout << "inverse kinematics: " << i << "" << Qres.transpose() << endl;
			//}

			// Qinit的初始值影响到迭代结果
			Qinit[0] = Qres[0];
			Qinit[1] = Qres[1];
			Qinit[2] = Qres[2];
			Qinit[3] = Qres[3];
			Qinit[4] = Qres[4];
			Qinit[5] = Qres[5];
		}

		// 逆运动学计算 InverseKinematics（）
		if (true)
		{
			Vector3d result = CalcBodyToBaseCoordinates(*model, Q, body_6_id, EndPoint, true);
			Matrix3d resBodyWorldOrientation = CalcBodyWorldOrientation(*model, Q, body_6_id, true);

			cs.ClearConstraints();
			cs.AddFullConstraint(body_6_id, EndPoint, result, resBodyWorldOrientation);
			bool err = InverseKinematics(*model, Qinit, cs, Qres);

			// 检查计算结果与前一个计算结果的误差
			for (size_t i = 0; i < 6; i++)
			{
				if (abs(Qinit[i]- Qres[i]) > 0.1)
				{
					Qres[i] = Qinit[i];
				}
			}
			
			file << Qres.transpose() << endl;
			if (err)
			{
				cout << "inverse kinematics: " << Qres.transpose() << endl;
			}
			else
			{
				cout << "inverse kinematics: " << i << "  " << Qres.transpose() << endl;
			}

			// Qinit的初始值影响到迭代结果
			Qinit[0] = Qres[0];
			Qinit[1] = Qres[1];
			Qinit[2] = Qres[2];
			Qinit[3] = Qres[3];
			Qinit[4] = Qres[4];
			Qinit[5] = Qres[5];
		}

		//	CalcPointJacobian()
			// 计算位置相关的雅可比矩阵 3xN，N=model->dof_count
		if (false)
		{
			G.setZero();
			CalcPointJacobian(*model, Q, body_6_id, Vector3d(0, 0, 0), G, true);
			file << G(0, 0) << "  " << G(0, 1) << "  " << G(0, 2) << "  " << G(0, 3) << "  " << G(0, 4) << "  " << G(0, 5) << "  ";
			file << G(1, 0) << "  " << G(1, 1) << "  " << G(1, 2) << "  " << G(1, 3) << "  " << G(1, 4) << "  " << G(1, 5) << "  ";
			file << G(2, 0) << "  " << G(2, 1) << "  " << G(2, 2) << "  " << G(2, 3) << "  " << G(2, 4) << "  " << G(2, 5) << endl;
		}

		//	CalcPointJacobian6D()
			// 计算6D雅可比矩阵 6xN，N=model->dof_count
		if (false)
		{
			G_6D.setZero();
			CalcPointJacobian6D(*model, Q, body_6_id, Vector3d(0, 0, 0), G_6D, true);
			file << G_6D.reshaped().transpose() << endl;
		}

		// CalcBodySpatialJacobian()
			// 计算body i 的 body Jacobian 6xN，N=model->dof_count
		if (false)
		{
			G_6D.setZero();
			CalcBodySpatialJacobian(*model, Q, body_6_id, G_6D, true);
			file << G_6D.reshaped().transpose() << endl;
		}

		// CalcPointVelocity()
			// 计算物体上一点在全局坐标系下的速度（线速度）
		if (false)
		{
			Vector3d resPointVelocity = CalcPointVelocity(*model, Q, QDot, body_6_id, Vector3d(0, 0, 0), true);
			file << resPointVelocity.reshaped().transpose() << endl;
		}

		// CalcPointVelocity6D()
			// 计算空间速度（含角速度、线速度）
		if (false)
		{
			auto resPointVelocity6D = CalcPointVelocity6D(*model, Q, QDot, body_6_id, Vector3d(0, 0, 0), true);
			file << resPointVelocity6D.reshaped().transpose() << endl;
		}

		// CalcPointAcceleration()
			// 计算物体上一点在全局坐标系下的加速度（线加速度）
			// 如果这个函数在ForwardDynamics()之后被调用，而没有更新运动学状态，那么就必须在结果中添加重力加速度。
		if (false)
		{
			auto resPointAcceleration = CalcPointAcceleration(*model, Q, QDot, QDDot, body_1_id, Vector3d(0, 0, 0), true);
			file << resPointAcceleration.reshaped().transpose() << endl;
			//cout << resPointAcceleration.reshaped().transpose() << endl;
		}

		// CalcPointAcceleration6D()
			// 计算空间加速度（含角加速度、线加速度）
		if (false)
		{
			auto resPointAcceleration6D = CalcPointAcceleration6D(*model, Q, QDot, QDDot, body_6_id, Vector3d(0, 0, 0), true);
			file << resPointAcceleration6D.reshaped().transpose() << endl;
			//cout << resPointAcceleration6D.reshaped().transpose() << endl;
		}

	}
	
	delete model;
	return 0;
}

参考资料：
Rigid Body Dynamics Library