获取从 body 状态到工厂状态索引的映射？

Question

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

示例：如果我有 object-wise 位置、速度或加速度表示（例如 q_obj1、q_obj2 等）并且需要与植物的状态进行交互，超出 SetPositions/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 或 Expression 之前执行以下操作：

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 会有相同的布局应该可以正常工作。

也就是说，我实际上并没有测试上面的代码，所以很抱歉（请让我知道！）如果没有成功。

获取从 body 状态到工厂状态索引的映射？

Get mapping from body state to plant state indices?

drake