pythondrakesystem-identification

Why might DecomposeLumpedParameters return unsimplified expressions?

I am trying to perform system identification on an object attached to a KUKA iiwa using Drake in Python. My goal is to do lumped parameter estimation using least squares, which involves decomposing the multibody equations using symbolic.DecomposeLumpedParameters.

The issue I am running into is that the resulting symbolic equation in the parameters I am trying to estimate roughly resemble the form I would expect, however they contain terms in the denominator that could be simplified. This prevents the entire equation from being simplified as a polynomial, and it returns hundreds of terms with small coefficients.

This is challenging to deal with computationally, and it also is inconsistent in the parameters that are found for the exact same system, given different inputs for the state and velocities.

The modified ManipulationStation I am using would be too large to put here, but the same problem occurs with a normal iiwa model in this example. Here I load up a 7-DOF iiwa as my plant, and try to estimate the parameters of the 7th 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

The output I see when running this is too large to paste here, while some of the parameters are okay (i.e. m, m * cx, m * cy, m * cz), the lumped parameters involving the inertial parameters are very long and contain terms with m in the denominator.

Here is an example of a term that could be simplified: ((7.0279621408873449 * (I(5) * m) - 7.0279621408873449 * (pow(m, 2) * cy * cz)) / m)

Is there a reason this may be happening, or is there a way to avoid it? Thank you!

Solution

  • I just checked the code (thanks for the reproduction). The m in the denominator is happening in the MakeFromCentralInertia step. If you add

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

    right after the inertia is created, you'll see it. I think we need a different way to construct that SpatialInertia.

    My recommendation is to change to parameterizing the inertia with the UnitInertia instead of the 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)))

    which results in symbolic inertia