获取从body状态到植物状态索引的映射?

Question

有没有办法知道植物状态[q，v]和单个物体的[qi，vi]指数之间的映射？

例如：如果我有位置、速度或加速度(例如q_obj1、q_obj2等(的对象表示，并且需要与植物的状态进行交互，超出了 SetPosition/SetVelocities 允许我做的事情---例如计算 M.dot(qdd(，或馈入 MultibodyPositionToGeometryPose。

从文档中看，似乎C++方法MakeStateSelectorMatrix或其逆方法在这里可能很有用，但是如何在python中做到这一点？

编辑：这是一个更清晰的例子

# 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)

编辑 2：供将来参考，这里有一个解决方法---在将 MultiBodyPlant 转换为 AutoDiffXd 或表达式之前执行以下操作：

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

然后你可以做：

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)

Answer 1

MultibodyPlant::SetPositionsInArray或SetVelocitiesInArray适合您正在寻找的工作吗？

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)

有人可能会说有点滥用 API(因为在这个例子中，值是加速(，但我认为鉴于qd和qdd将具有相同的布局，它应该可以正常工作。

话虽如此，我实际上并没有测试上面的代码，所以如果这不起作用，请道歉(请告诉我！