如何在其中包含自由体对象的上下文中使用 KinematicTrajectoryOptimization？

Question

我们正在尝试使用 KinematicTrajectoryOptimization 来优化基本拾取和放置的轨迹。 我们使用 Russ Tedrake 提供的第 8 笔记本作为参考Deepnote 。 通常，它们在示例中没有自由体 object。 但是，我们还需要空间上的 object。 因此，我们添加了一个 urdf model 用作砖块。 然而，在将该框添加到工厂之后，显然，上下文维度发生了变化。 通过该更改，它引发了以下异常：

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。另一方面，多体植物将使用 angular 速度在 state 中代替四元数的 qdot。 您必须自己在它们之间使用 map（例如使用MapVelocityToQdot ）。