Get mapping from body state to plant state indices?

Viewed 83

Is there a way to know the mapping between the indices of a plant's state [q, v] and an individual object's [qi, vi]?

Example: if I have object-wise representations of position, velocities, or accelerations (e.g. q_obj1, q_obj2 etc.) and need to interact with a plant's state beyond what SetPositions/SetVelocities allows me to do---for instance computing M.dot(qdd), or feeding into MultibodyPositionToGeometryPose.

From the documentation it seemed like maybe the C++ method MakeStateSelectorMatrix or its inverse could be useful here, but how would one do this in python?

Edit: here's a more clear example

# object-wise positions, velocities, and accelerations
q1, qd1, qdd1, q2, qd2, qdd2 = ...

# set plant positions and velocities for each object
plant.SetPositionsAndVelocities(context, model1, np.concatenate([q1, qd1]))
plant.SetPositionsAndVelocities(context, model2, np.concatenate([q2, qd2]))

# get matrices for the plant manipulator equations
M = plant.CalcMassMatrixViaInverseDynamics(context)
Cv = plant.CalcBiasTerm(context)

# The following line is wrong, because M is in the plant state ordering 
# but qdd is not!
print(M.dot(np.concatenate([qdd1, qdd2])) + Cv)

# here's what I would like, where the imaginary method ConvertToStateIndices
# maps from object indices to plant state indices,
# which SetPositions does under the hood, but I need exposed here
reordered_qdd = np.zeros(plant.num_positions, dtype=qdd.dtype)
reordered_qdd += plant.ConvertToStateIndices(model1, qdd1)
reordered_qdd += plant.ConvertToStateIndices(model2, qdd2)
print(M.dot(reordered_qdd) + Cv)

Edit 2: for future reference, here is a workaround---do the following before casting MultiBodyPlant to AutoDiffXd or Expression:

indices = np.arange(plant.num_positions())
indices_dict = {body: plant.GetPositionsFromArray(model, indices).astype(int)
                for body, model in zip(bodies, models)}

then you can do:

reordered_qdd = np.zeros(plant.num_positions, dtype=qdd.dtype)
reordered_qdd[indices_dict[body1]] += qdd1
reordered_qdd[indices_dict[body2]] += qdd2
print(M.dot(reordered_qdd) + Cv)
1 Answers

Are MultibodyPlant::SetPositionsInArray or SetVelocitiesInArray up to the job you're looking for?

reordered_qdd = np.zeros(plant.num_positions, dtype=qdd.dtype)
plant.SetVelocitiesInArray(model1, qdd1, reordered_qdd)
plant.SetVelocitiesInArray(model2, qdd2, reordered_qdd)
print(M.dot(reordered_qdd) + Cv)

One could say a bit of an abuse of the API (since in this example the values are accelerations), but I think given that qd and qdd would have the same layout it should work fine.

That being said, I didn't actually test the code above, so apologies (and please let me know!) if that doesn't work out.

Related