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

user_3769033 0
Viewed 64

Is there any way to find the Jacobian of a frame with respect to the joints of a given model (as opposed to the whole plant), or alternatively to determine which columns of the full plant Jacobian correspond to a given model’s joints? I’ve found MultibodyPlant.CalcJacobian*, but I’m not sure if those are the right methods.

I also tried mapping the JointIndex of each joint in the model to a column of MultibodyPlant.CalcJacobian*, but the results didn't make sense -- the joint indices are sequential (all of one model followed by all of the other), but the Jacobian columns look interleaved (a column corresponding to one model followed by one corresponding to the other).

drake
1 Answers

Assuming you are computing with respect to velocities, you'll want to use Joint.velocity_start() and Joint.num_velocities() to create a mask or set of indices. If you are in Python, then you can use NumPy's array slicing to select the desired columns of your Jacobian.

(If you compute w.r.t. position, then make sure you use Joint.position_start() and Joint.num_positions().)

Example notebook:
https://nbviewer.jupyter.org/github/EricCousineau-TRI/repro/blob/eb7f11d/drake_stuff/notebooks/multibody_plant_jacobian_subset.ipynb
(TODO: Point to a more official source.)

Main code to pay attention to:

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]]
user_78295250
Related