pydrake的多体植物是否可以使用RegionOfAttraction（）？

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（它们是否支持转换为符号），或者是否有其他我必须考虑的东西/我的想法中存在一些明显的错误

提前谢谢

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，要么在不动点附近对动力学进行泰勒展开。符号工具支持Taylor扩展，但我恐怕还没有通过添加包装符号调用的方法TaylorExpand(System, Context)来添加使工作流非常适合的工具。这也将是相对容易和有用的做

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