使用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;
}