由自由体基组成的机器人的广义速度的结构是什么？

Question

我在使用德雷克图书馆时遇到了一个相当理论性的问题。我的机器人由一个自由的身体基础(浮动底座)组成，它有一个传统的机器人手臂，由关节和连杆组成。

假设这个机器人的广义状态向量是63。(从德雷克那里得到)这个状态向量的前7个条目是3个笛卡尔位置和4个四元数的取向。这意味着，广义速度矢量为62 (线性和角速度的6个条目)。如果根据前面描述的顺序方案(首先是3个位置，其次是4个四元数，然后是机器人臂的56个关节状态)构造我的状态向量，然后相应地设置机器人的位置，那么Drake就会运行，不会出现任何错误。

然而，采用机器人末端执行器的雅可比值会产生奇怪的失配现象。在我看来，当用x_dot = J * q_dot命令得到雅可比CalcJacobianTranslationalVelocity时，交换了广义速度矢量的阶数。更确切地说，命令是：

1. ：基
2. 的3角速度，基

H 111的3线性速度>机器人臂上的其他56个广义速度H 212G 213

我对此感到非常困惑，因为我可以使用SetPositions和SetVelocities来操作和设置机器人的状态，并使用状态和速度向量来交换上面列表中的点1和2。我是不是遗漏了什么？

谢谢你的帮忙，

诚挚的问候,

罗伯特

Answer 1

我们遵循的惯例是角量先于平移量。对于浮动基，这意味着四个四元数在xyz平移之前，就像你看到的速度一样。

Answer 2

对于您来说，最安全的方法(就保证正确性而言)可能是使用上下文和Drake的API来实现它。例如：您可以使用MBP::SetFreeBodySpatialVelocity()作为您的浮动基础和连接方法的其余部分(例如，RevoluteJoint::set_angular_rate() )。一旦完成，您就可以使用MultibodyPlant::GetVelocities()检索一致的速度向量。

如果您想要一种更基于“索引”的方法，您可以执行以下操作。每个浮动体的dofs从Body::floating_velocities_start()开始(请阅读文档，这是完整的状态向量)。6按@sherm在上一个答复中所描述的顺序排列的数字)。对于关节，您可以检索Joint::velocity_start()和Joint::num_velocities()。

警告:当Body::floating_velocities_start()索引完全状态向量x时，Joint::velocity_start()只索引广义速度向量v。请看医生。