如何使用KinematicTrajectoryOptimization在一个上下文中有自由的body对象?

Question

我们正在尝试使用KinematicTrajectoryOptimization来优化我们的基本拾取和放置轨迹。我们使用Russ Tedrake提供的第8台笔记本作为参考Deepnote。通常，它们在示例中没有自由体对象。但是，我们也需要空间上的一个物体。因此，我们添加了一个urdf模型作为砖块使用。然而，在将盒子添加到植物中之后，很明显，上下文维度发生了变化。修改后，会引发以下异常:

SystemExit: Failure at systems/trajectory_optimization/kinematic_trajectory_optimization.cc:351 in AddVelocityBounds(): condition 'lb.size() == num_positions()' failed.

/shared-libs/python3.8/py-core/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3386: UserWarning: To exit: use 'exit', 'quit', or Ctrl-D.

warn("To exit: use 'exit', 'quit', or Ctrl-D.", stacklevel=1)

这个问题有解决办法吗?这可能是因为额外的四分位数。

提前感谢。

Answer 1

我同意你的分析;如果您有额外的主体，num_positions()将增加7(对于x,y,z +四元数)。当调用add * bounds()时，您可以相应地调整添加元素的下界和上界——无限的限制是可以的。

正如文档所述，优化中的速度(例如对于AddVelocityBounds)将对应于qdot，而不是v.另一方面，multibodyplant将使用四元数状态中的角速度来代替qdot。你必须自己在它们之间进行映射(例如使用MapVelocityToQdot)。