Drake Rigid_body_tree 计算雅可比问题

Drake Rigid_body_tree calculating the jacobian question

我目前正在尝试使用 "rigid_body_tree.cc" 文件计算库卡臂的雅可比方程:Tau = J^T*F,其中 Tau 是库卡臂的 7 个关节力矩,F是末端执行器处的笛卡尔力和力矩,J^T 是雅可比转置。

drake 中有一个名为 transformPointsJacobian 的函数,它接收缓存、点、from_body_or_frame_ind、to_body_or_frame_ind 和 in_terms_of_qdot。

函数首先计算几何雅可比矩阵,输出一个6x7的矩阵(kuka有7个关节)

然后,它采用该矩阵并使用它来确定计算如下的 3x7 雅可比矩阵:

J.template block<kSpaceDimension, 1>(row_start, *it) = Jv.col(col);

J.template block<kSpaceDimension, 1>(row_start, *it).noalias() += Jomega.col(col).cross(points_base.col(i));

这会将 6x7 几何雅可比缩小为 3x7 雅可比,其中前 3 行是通过 Jv + Jw*Transformation 计算得到的。

这段代码确实有效,但我似乎不明白为什么这一步有效。此外,由于我需要笛卡尔末端执行器 space 中的扭矩,因此我需要完整的 6x7 雅可比矩阵。

为了获得雅可比的最后 3 行,我如何使用几何雅可比的输出以便它在等式 Tau = J^T*F 中有效?

谢谢!

请考虑切换到受支持的 class MultibodyPlant,而不是 Drake 阁楼中即将弃用的 RigidBodyTree。那里的 Jacobian 文档要好得多——方法组是 here. An example (with lots of documentation) is here;那个产生 6x7.

是否有需要使用旧代码的原因?