在 Pydrake 中找到一个框架相对于给定模型关节的雅可比行列式

Question

是否有任何方法可以找到框架相对于给定模型的关节（而不是整个植物）的雅可比行列式，或者确定完整植物雅可比行列的哪些列对应于给定模型的关节？我找到了 MultibodyPlant.CalcJacobian*，但我不确定这些方法是否正确。

我还尝试将模型中每个关节的 JointIndex 映射到 MultibodyPlant.CalcJacobian* 的列，但结果没有任何意义——关节索引是连续的（都是一个模型后跟所有其他模型），但雅可比列看起来是交错的（一列对应一个模型，后跟一列对应另一个模型）。

Answer 1

假设您正在计算速度，您需要使用 Joint.velocity_start() 和 Joint.num_velocities() 来创建一个掩码或一组索引。如果您在 Python 中，那么您可以使用 NumPy 的数组切片 select 所需的雅可比行列。

（如果计算 w.r.t. 位置，则确保使用 Joint.position_start() 和 Joint.num_positions()。）

示例笔记本：
https://nbviewer.jupyter.org/github/EricCousineau-TRI/repro/blob/eb7f11d/drake_stuff/notebooks/multibody_plant_jacobian_subset.ipynb
（TODO：指向更官方的来源。）

要注意的主要代码：

def get_velocity_mask(plant, joints):
    """
    Generates a mask according to supplied set of ``joints``.

    The binary mask is unable to preserve ordering for joint indices, thus
    `joints` required to be a ``set`` (for simplicity).
    """
    assert isinstance(joints, set)
    mask = np.zeros(plant.num_velocities(), dtype=np.bool)
    for joint in joints:
        start = joint.velocity_start()
        end = start + joint.num_velocities()
        mask[start:end] = True
    return mask

def get_velocity_indices(plant, joints):
    """
    Generates a list of indices according to supplies list of ``joints``.

    The indices are generated according to the order of ``joints``, thus
    ``joints`` is required to be a list (for simplicity).
    """
    indices = []
    for joint in joints:
        start = joint.velocity_start()
        end = start + joint.num_velocities()
        for i in range(start, end):
            indices.append(i)
    return indices

...

# print(Jv1_WG1)  # Prints 7 dof from a 14 dof plant
[[0.000 -0.707 0.354 0.707 0.612 -0.750 0.256]
 [0.000 0.707 0.354 -0.707 0.612 0.250 0.963]
 [1.000 -0.000 0.866 -0.000 0.500 0.612 -0.079]
 [-0.471 0.394 -0.211 -0.137 -0.043 -0.049 0.000]
 [0.414 0.394 0.162 -0.137 0.014 0.008 0.000]
 [0.000 -0.626 0.020 0.416 0.035 -0.064 0.000]]

在 Pydrake 中找到一个框架相对于给定模型关节的雅可比行列式

Finding the Jacobian of a frame with respect to the joints of a given model in Pydrake

drake