2

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)
Joao Loula
  • 91
  • 4

1 Answers1

1

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.

sammy-tri
  • 121
  • 2
  • Ah, I really like this and it would have been perfect, except that I'm working with an AutoDiffXd plant and it looks like arrays with dtype=object can't be referenced (cf. https://github.com/RobotLocomotion/drake/blob/28dff225df67a06f0e606d536183775453bad8de/bindings/pydrake/multibody/test/plant_test.py#L552). I think Russ had a hackier workaround, but I couldn't try it in time and it looks like his comment got deleted? – Joao Loula May 18 '20 at 18:35
  • I coded up a hacky workaround inspired by your answer that I've added as an edit to my original post---it's not ideal and if there are better solutions I'd love to know, but for now this suffices for me. Thank you! – Joao Loula May 18 '20 at 19:50