机器人动力学库

g++ dynamictest.cpp -o dynamictest_RBDyn -lRBDyn -lRBDynParsers -lTasks

std::string urdf_path ="the/path/to/your/urdf";
// 法一：不含浮动基
pinocchio::urdf::buildModel(urdf_path, model);
data = pinocchio::Data(model);

// 法二：含浮动基，nq = dof + 7 ，nv = dof + 6 
pinocchio::JointModelFreeFlyer root_joint;
pinocchio::urdf::buildModel(urdf_path, root_joint, model);
data = pinocchio::Data(model);

// 法三：含浮动基，nq = dof + 6 ，nv = dof + 6 
pinocchio::JointModelComposite jointComposite(2);  // 添加两个关节
jointComposite.addJoint(pinocchio::JointModelTranslation());  // 添加平动3个自由度
jointComposite.addJoint(pinocchio::JointModelSphericalZYX()); // 添加旋转3个自由度
pinocchio::urdf::buildModel(urdf_path, jointComposite, model);
data = pinocchio::Data(model);

import pinocchio as pin

# 补
joint.placement/frame.placement # 表示在其父关节坐标系下的变换（SE3 变换矩阵）,不会随机器人姿态变化而变化

# Spatial quantities
# 1.Transforms
M = pin.SE3(1) or pin.SE3.Identity() # 单位变化
M.rotation  # 旋转矩阵
M.translation  # 平移向量
aMb = pin.SE3(aRb, apb)# 创建一个从坐标系b到坐标系a的变换矩阵
pin.SE3.Random() # 生成一个随机的变换矩阵
bMa = aMb.inverse()# 计算变换矩阵的逆
aMc = aMb * bMc # 变换矩阵的乘法
aXb = aMb.action # 变换矩阵对空间向量的作用
aHb = aMb.homogeneous # 获取齐次变换矩阵
pin.log(M) # 将SE3变换矩阵转换为6D向量（对数映射）
pin.exp(M) # 将6D向量转换为SE3变换矩阵（指数映射）

# Data
data = pin.Data(model) 或 data = model.createData() # 创建与模型相关的数据结构
data.joints # 关节数据
data.oMi[jid] # 关节jid在世界坐标系中的变化（SE3变换矩阵），随机器人姿态变化而变化
data.oMf[fid] # 坐标系fid在世界坐标系中的变化（SE3变换矩阵），随机器人姿态变化而变化
data.v # 关节速度
data.a # 关节加速度
data.f # 关节力
data.M # 质量矩阵
data.nle # 非线性效应
data.hg # 质心动量
data.Ag # 质心矩阵
data.Ig # 质心惯性

# Model
model = pin.Model() # 创建机器人模型的运动学树
model.name # 模型名称
model.names # 关节名称
model.joints # 关节模型
model.placements # 关节相对位置，不会随机器人姿态变化而变化
model.inertias # 连杆惯性
model.frames # 获取某个坐标系
model.nq # 位置变量的数量
model.nv # 速度变量的数量


# 输出所有关节
for joint_id in range(robot.model.njoints):  
     joint_name = robot.model.names[joint_id]  
     print(f"Joint ID: {joint_id}, Name: {joint_name}")

# 输出所有坐标系            
for frame_id in range(robot.model.nframes):
    frame_name = robot.model.frames[frame_id].name
    print(f"Frame ID: {frame_id}, Name: {frame_name}")

# 返回 q0 to q1的速度向量（注意，输出的是的向量大小为nv，nq中四元数会单独处理）
pinocchio::difference(model_, x1.head(nq), x2.head(nq), dx.head(nv));

// -------- 注意：以下为伪代码 --------
// 1）根据dof设置对应的关节角度q
q<< 0, 0, 0, 1.57, .. , dof;

// 2）调用正运动学函数，将命令q输入，并将结果保存在data中
forwardKinematics(model, data , q)

// 3）获取末端关节索引（注意x的大小，x需要自己定义，使该索引值指向机器人的末端）
JointIndex end_effector_id = model.njoints - x

// 4）通过data结构体与末端索引获取末端位姿
// position
Eigen::Vector3d position = data.oMi[end_effector_id].translation();

// position为列向量，将其转置后更方便输出 
std::cout<< position.transpose()<<std::endl;

// Rotation
Eigen::Quaternion quaternion = data.oMi[end_effector_id].rotation();

// 将旋转矩阵转化为四元数后，输出更容易检查
std::cout<< quaternion.coeffs().transpose() <<std::endl;

// 5)可以使用rviz进行显示，并通过tf来检验两者之间的变换
rosrun tf tf_echo /base_link /end_effector_link 

// -------- 注意：以下为伪代码 --------
// 1）设置迭代相关的参数
const double eps = 1e-4;
const int IT_MAX = 100;
const double DT = 6e-1;
const double damp = 1e-2;

// 2)设置目标位姿（相对于世界坐标系）
// 法一：设置一个目标
Eigen::Vector3d desRotation_eul = {0, 0, 0};// 为了方便，可以先设置欧拉角，调用函数将其转化为旋转矩阵
Eigen::Matrix3d desRotation = eul2Rot(desRotation_eul(0), desRotation_eul(1), desRotation_eul(2));
Eigen::Vector3d desPosition = {0, 0, -0.4};
const pinocchio::SE3 target(desRotation, desPosition);// 目标姿态得用SE3数据类型

// 法二：设置多个目标
 std::vector<pinocchio::SE3> targets {pinocchio::SE3(Eigen::Quaternion(x,x,x,x),Eigen::Vector3d(x,x,x)), ...} 

// 4）开始利用雅可比进行迭代
int itr_count{0};
for (itr_count = 0;; itr_count++)
{
   // 5)正运动学更新，数据存储在data中（注意，第一次运行该命令之后模型才真正被初始化！）
   pinocchio::forwardKinematics(model, data , q)

   // 6)通过末端关节索引以及目标姿态，调用函数计算误差矩阵
   // oMi表示某关节在全局坐标系下的变换矩阵
   // oMi[xx].actInv(xxx)目标在当前坐标系下的矩阵  
   const pinocchio::SE3 iMd = data.oMi[end_effector_id].actInv(target);

   // 7)将误差矩阵转化为行向量，方便后续计算与输出 
   errL = pinocchio::log6(iMd).toVector(); // in joint frame
   std::cout<<"err大小为"<<err<<std::endl;

   // 8)判断是否满足精度，若满足则退出
   if (err.norm() < eps) // 满足精度后退出
   {
     success = true;
     break;
   }
   if (itr_count >= IT_MAX)
   {
     success = false;
     break;
   }
   // 9)根据当前关节角q，获取雅可比矩阵矩阵J
   pinocchio::computeJointJacobian(model, data, q, end_effector_id, J); 
   std::cout<<"J的大小为"<<J<<std::endl;

   // 10)计算了李代数形式的雅可比矩阵的对数，然后获取修正后的雅可比矩阵J
   pinocchio::Data::Matrix6 Jlog;
   pinocchio::Jlog6(iMd.inverse(), Jlog);
   std::cout << "Jlog的大小为"<< Jlog << std::endl;
   J = -Jlog * J
   Eigen::Matrix<double, 6, 6> JJt;  // 注意维度，其与雅可比矩阵维度对应
   JJt.noalias() = J * W * J.transpose(); // 这里可以通过一个权益矩阵w，来选择哪些关节不参与逆运动学的计算

   // 11)在JJt的对角线加上了阻尼项 damp，以确保矩阵可逆
   JJt.diagonal().array() += damp;

   // 12）通过求解线性方程组JJt * v = - J^T * err,计算关节误差，
   v.noalias() = -W * J.transpose() * JJt.ldlt().solve(err);
   std::cout << "v的大小为"<< v << std::endl;  
   
   // 13）求解关节增量，并获取最终关节角的值
   q = pinocchio::integrate(model, q, v * DT); 
   std::cout << "q的大小为"<< q << std::endl;
}

g++ -std=c++11 overview-simple.cpp -o overview-simple $(pkg-config --cflags --libs pinocchio)

pinocchio::urdf::buildModel(urdf_path, model);