RegionOfAttraction() 是否可用于 pydrake 的多体植物？

Question

为了熟悉吸引代码区域，我尝试估算我有 urdf 描述的 Acrobot 的 RoA。运行这些行：

import numpy as np

from pydrake.all import DiagramBuilder, AddMultibodyPlantSceneGraph, Parser, MultibodyPlant, Variable, Simulator, ConstantVectorSource, RegionOfAttraction, LinearSystem,LinearQuadraticRegulator,Saturation

import pydot
#%% prepare and load urdf
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)

parser = Parser(plant, scene_graph)
parser.AddModelFromFile("acrobot.urdf")

plant.Finalize()

#%% create down context
q=np.array([0.0,0.0])
qd=np.array([0.0,0.0])

stable_ctxt = plant.CreateDefaultContext() 
plant.SetPositions(stable_ctxt, q)
plant.SetVelocities(stable_ctxt, qd)

actuation_port = plant.get_actuation_input_port()
actuation_port.FixValue(stable_ctxt, 0)

#%% calc RoA
V = RegionOfAttraction(system=plant, context=stable_ctxt)

给我这个错误：

SystemExit: Failure at systems/analysis/region_of_attraction.cc:126 in RegionOfAttraction(): condition 'xdot0.template lpNorm<Eigen::Infinity>() <= 1e-14' failed.

当查看 region_of_attraction.cc:123 中的注释时，表明 x0 不是一个稳定的不动点。但是，它对于提供给 RegionOfAttraction().

的 stable_ctxt 应该是稳定的

可以使用 RegionOfAttraction() 函数来估计多体植物的 RoA（它们是否支持转换为符号），或者我的想法是否有其他 consider/some 明显错误？

提前致谢！

Answer 1

感谢您询问并提供最小复制。这个例子中实际上发生了很多事情！

首先，通过调用 AddMultibodyPlantSceneGraph，您创建了一个连接到 SceneGraph 的 MultibodyPlant，但随后仅将植物传递给 RegionOfAttraction 方法，而没有它 SceneGraph。然后它会抱怨，因为它的几何端口未连接。一种解决方法是只做：

plant = MultibodyPlant(time_step=0.0)
parser = Parser(plant)
parser.AddModelFromFile(
    FindResourceOrThrow("drake/examples/acrobot/Acrobot.urdf"))
plant.Finalize()

其次，RegionOfAttraction 编写的代码假设您只传递一个闭环系统（无输入端口），就像您使用 LQR 得到的一样。所以输入端口“FixValue”分配没有被复制到符号工厂。支持固定输入端口的情况也是合理的，所以我只是 PR'd the fix。或者，您可以通过为输入端口添加 ConstantVectorSystem 然后将图表（不是工厂）传递到 RegionOfAttraction 方法来立即解决此问题。

第三，在符号MultibodyPlant中必须注意什么是合理的。我们不支持质量矩阵的符号反转（除了微不足道的情况）；我不认为这是一个缺失的功能，而是我们应该避免的情况。以隐式形式考虑动态要好得多（参见 http://underactuated.csail.mit.edu/lyapunov.html#ex:implicit）。话虽如此，我们可以实际上通过 ABA ldlt 调用足够容易地支持 2x2 符号矩阵求逆（我实际上认为我们已经做到了）。

最后，RegionOfAttraction中使用的平方和工具只支持多项式系统，而Acrobot动力学不是多项式的。您要么想要替换 sin/cos 项，as in this example，要么对不动点周围的动力学进行泰勒展开。符号工具支持泰勒展开，但恐怕我还没有通过添加包装符号调用的方法 TaylorExpand(System, Context) 添加使工作流非常适合此的工具。这也相对容易且有用！

如果有特别缺失的部分具有高价值，那么我们可以打开一个问题，或者更好的是我可以尝试帮助您做出贡献。

RegionOfAttraction() 是否可用于 pydrake 的多体植物？

Is RegionOfAttraction() available for Multibody plants from pydrake?

python

drake