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