为什么 DecomposeLumpedParameters return 可能是未简化的表达式？

Question

我正在尝试使用 Python 中的 Drake 对连接到 KUKA iiwa 的对象执行系统识别。我的目标是使用最小二乘法进行集中参数估计，这涉及使用 symbolic.DecomposeLumpedParameters.

分解多体方程

我运行遇到的问题是，我试图估计的参数中生成的符号方程大致类似于我期望的形式，但是它们在分母中包含可以简化的项。这可以防止整个方程被简化为多项式，并且 returns 数百个系数较小的项。

这在计算上很难处理，并且在给定状态和速度的不同输入的情况下，为完全相同的系统找到的参数也不一致。

我使用的修改后的 ManipulationStation 太大而无法放在这里，但是在这个例子中，普通的 iiwa 模型也会出现同样的问题。这里我加载一个7自由度的iiwa作为我的工厂，并尝试估计第7个参数link:

import numpy as np
import pydrake.symbolic as sym
from pydrake.all import (
    Parser, AddMultibodyPlantSceneGraph, SpatialInertia_, RotationalInertia_, DiagramBuilder,
    FindResourceOrThrow,
)

# Create the plant
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
                                                 time_step=0)
Parser(plant, scene_graph).AddModelFromFile(
    FindResourceOrThrow("drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
plant.Finalize()
diagram = builder.Build()

context = plant.CreateDefaultContext()
sym_plant = plant.ToSymbolic()
sym_context = sym_plant.CreateDefaultContext()
sym_context.SetTimeStateAndParametersFrom(context)
sym_plant.FixInputPortsFrom(plant, context, sym_context)

state = sym_context.get_continuous_state()

# Random state/command inputs
# (Normally these are recorded from the robot executing a trajectory)
q = np.random.random(size=state.num_q())
v = np.random.random(size=state.num_v())
vd = np.random.random(size=state.num_v())
tau = np.random.random(size=state.num_q())  # Remove -1  for fully actuated system

# Parameters
I = sym.MakeVectorVariable(6, 'I')  # Inertia tensor/mass matrix
m = sym.Variable('m')  # mass
cx = sym.Variable('cx')  # center of mass
cy = sym.Variable('cy')
cz = sym.Variable('cz')

sym_plant.get_actuation_input_port().FixValue(sym_context, tau)
sym_plant.SetPositions(sym_context, q)
sym_plant.SetVelocities(sym_context, v)

obj = sym_plant.GetBodyByName('iiwa_link_7')
inertia = SpatialInertia_[sym.Expression].MakeFromCentralInertia(m, [cx, cy, cz],
                                                                 RotationalInertia_[sym.Expression](
                                                                     I[0], I[1], I[2], I[3], I[4], I[5]))
obj.SetSpatialInertiaInBodyFrame(sym_context, inertia)

derivatives = sym_context.Clone().get_mutable_continuous_state()
derivatives.SetFromVector(np.hstack((0 * v, vd)))
residual = sym_plant.CalcImplicitTimeDerivativesResidual(
    sym_context, derivatives)

W, alpha, w0 = sym.DecomposeLumpedParameters(residual[2:],
                                             [m, cx, cy, cz, I[0], I[1], I[2], I[3], I[4], I[5]])

return W, alpha, w0

我在运行时看到的输出太大而无法粘贴到此处，而一些参数没问题（即 m、m * cx、m * cy、 m * cz)，涉及惯性参数的集总参数很长，包含分母中带m的项。

这是一个可以简化的术语示例： ((7.0279621408873449 * (I(5) * m) - 7.0279621408873449 * (pow(m, 2) * cy * cz)) / m)

发生这种情况是否有原因，或者是否有办法避免这种情况？谢谢！

Answer 1

刚刚查了一下代码（感谢转载）。分母中的 m 发生在 MakeFromCentralInertia 步骤中。如果你加上

display(Math(ToLatex(inertia.CopyToFullMatrix6(), 2)))

在惯性创建之后，您就会看到它。我认为我们需要一种不同的方式来构造 SpatialInertia.

我的建议是改为使用 UnitInertia 而不是 RotationalInertia:

参数化惯性

G = sym.MakeVectorVariable(6, 'G')  # Inertia tensor/mass matrix
m = sym.Variable('m')  # mass
cx = sym.Variable('cx')  # center of mass
cy = sym.Variable('cy')
cz = sym.Variable('cz')

sym_plant.get_actuation_input_port().FixValue(sym_context, tau)
sym_plant.SetPositions(sym_context, q)
sym_plant.SetVelocities(sym_context, v)

obj = sym_plant.GetBodyByName('iiwa_link_7')
inertia = SpatialInertia_[sym.Expression](
    m, [cx, cy, cz], UnitInertia_[sym.Expression](G[0], G[1], G[2], G[3],
                                                        G[4], G[5]))
display(Math(ToLatex(inertia.CopyToFullMatrix6(), 2)))

这导致

为什么 DecomposeLumpedParameters return 可能是未简化的表达式？

Why might DecomposeLumpedParameters return unsimplified expressions?

python

system-identification

drake