如何在其中包含自由體對象的上下文中使用 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 ）。