got error when using drake's symbolic method to get regressor matrix of floating base equation of motion

Question

I want to use drake to do some system identification of a floating base legged robots.And I follow the procedure in Underactuated Robotics Ch.18's notebook from Professor Russ Tedrake. And This is my code:

plant=MultibodyPlant(0.0)
parser = Parser(plant)
parser.package_map().Add("rsc", "/home/ros/workspaces/rsc/")
wlr4p_url = (
    "package://rsc/wlr4p/urdf/"
    "wlr4p.urdf"
)
(wlr4p,)=parser.AddModels(url=wlr4p_url)
plant.Finalize()

sym_plant = plant.ToSymbolic()

sym_context = sym_plant.CreateDefaultContext()

links_name = ("base_link", "body", "thigh_left", "calf_left", "wheel_left", "thigh_right", "calf_right", "wheel_right")

m = []
l = []
L = []
for i, link_name in enumerate(links_name):
    m += [Variable("m"+str(i))]
    l += [Variable("l" + tmp + str(i)) for tmp in ["x", "y", "z"]]
    L += [Variable("L" + tmp + str(i)) for tmp in ["xx", "xy", "xz", "yy", "yz", "zz"]]

q = MakeVectorVariable(sym_plant.num_positions(), "q")
v = MakeVectorVariable(sym_plant.num_velocities(), "v")
vd = MakeVectorVariable(sym_plant.num_velocities(), "\dot{v}")
tau = MakeVectorVariable(sym_plant.num_actuated_dofs(), "u")

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

for i, link in enumerate(links_name):
    rbt_body = sym_plant.GetBodyByName(link)
    body_inertia = SpatialInertia_[Expression](
        m[i], 
        [l[3*i], l[3*i+1], l[3*i+2]],
        UnitInertia_[Expression](L[6*i], L[6*i+3], L[6*i+5], L[6*i+1], L[6*i+2], L[6*i+4]) 
    )
    rbt_body.SetSpatialInertiaInBodyFrame(sym_context, body_inertia)

derivatives = sym_context.Clone().get_mutable_continuous_state()

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

W, alpha , w0 = DecomposeLumpedParameters(residual[sym_plant.num_positions():], m+l+L)

When I run the DecomposeLumpedParameters(), it gives:

RuntimeError                              Traceback (most recent call last)
/tmp/ipykernel_18187/1339963687.py in <module>
----> 1 W, alpha , w0 = DecomposeLumpedParameters(residual[sym_plant.num_positions():], m+l+L)

RuntimeError: ((-2048 * m4 * Lxz4 * q(0) * q(1) * pow(q(2), 2) * pow(q(3), 4) * v(7) * v(9) * sin(q(8)) * sin(q(9)) * sin(q(10)) * cos(q(8)) * cos(q(9)) * cos(q(10))) / (24 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(2), 2) * pow(q(3), 2)) + 12 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(2), 4)) + 12 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(3), 4)) + 12 * (pow(q(0), 2) * pow(q(1), 4) * pow(q(2), 2)) + 12 * (pow(q(0), 2) * pow(q(1), 4) * pow(q(3), 2)) + 4 * (pow(q(0), 2) * pow(q(1), 6)) + 12 * (pow(q(0), 2) * pow(q(2), 2) * pow(q(3), 4)) + 12 * (pow(q(0), 2) * pow(q(2), 4) * pow(q(3), 2)) + 4 * (pow(q(0), 2) * pow(q(2), 6)) + 4 * (pow(q(0), 2) * pow(q(3), 6)) + 12 * (pow(q(0), 4) * pow(q(1), 2) * pow(q(2), 2)) + 12 * (pow(q(0), 4) * pow(q(1), 2) * pow(q(3), 2)) + 6 * (pow(q(0), 4) * pow(q(1), 4)) + 12 * (pow(q(0), 4) * pow(q(2), 2) * pow(q(3), 2)) + 6 * (pow(q(0), 4) * pow(q(2), 4)) + 6 * (pow(q(0), 4) * pow(q(3), 4)) + 4 * (pow(q(0), 6) * pow(q(1), 2)) + 4 * (pow(q(0), 6) * pow(q(2), 2)) + 4 * (pow(q(0), 6) * pow(q(3), 2)) + 12 * (pow(q(1), 2) * pow(q(2), 2) * pow(q(3), 4)) + 12 * (pow(q(1), 2) * pow(q(2), 4) * pow(q(3), 2)) + 4 * (pow(q(1), 2) * pow(q(2), 6)) + 4 * (pow(q(1), 2) * pow(q(3), 6)) + 12 * (pow(q(1), 4) * pow(q(2), 2) * pow(q(3), 2)) + 6 * (pow(q(1), 4) * pow(q(2), 4)) + 6 * (pow(q(1), 4) * pow(q(3), 4)) + 4 * (pow(q(1), 6) * pow(q(2), 2)) + 4 * (pow(q(1), 6) * pow(q(3), 2)) + 4 * (pow(q(2), 2) * pow(q(3), 6)) + 6 * (pow(q(2), 4) * pow(q(3), 4)) + 4 * (pow(q(2), 6) * pow(q(3), 2)) + pow(q(0), 8) + pow(q(1), 8) + pow(q(2), 8) + pow(q(3), 8))) 
cannot be factored into lumped parameters, since it depends on both parameters and non-parameter variables.

My robot is a floating base robot with 8 links and 7 actuated dofs,so nq = 14, nv = 13 and n_actuator = 7.I have wirte down the formula in the error traceback, it has form

m x L x f(q,q^2,q^4,v,sin(q),cos(q))+f(q^2,q^4,q^6,q^8)+...

Which supposed to be wrong. Because I think there should no 4th or higher powers of q exists. And this row is also not in the general form like A(q,v,dv) \phi.

Is there anything wrong about my code or I have misunderstood the concept about equation of motion for floating base robots? Thanks in advance for your help,Sincerely.

Answer 1

I agree that the limitation you've run into is real, and I've opened up an issue on Drake to track it.

A work-around would be to replace the quaternion floating base with a roll-pitch-yaw floating base. One can fix the minimal reproduction I posted in the issue by adding the AddFloatingRpyJoint from my underactuated repo:

import numpy as np
from IPython.display import Markdown, display
from pydrake.all import MultibodyPlant, Parser, SpatialInertia_, UnitInertia_, MakeVectorVariable, Variable, Expression, DecomposeLumpedParameters, ToLatex
from underactuated.scenarios import AddFloatingRpyJoint


plant = MultibodyPlant(0.0)
parser = Parser(plant)
parser.AddModelsFromUrl("package://drake/examples/quadrotor/quadrotor.urdf")
AddFloatingRpyJoint(plant, plant.GetBodyByName("base_link").body_frame(), plant.GetModelInstanceByName("quadrotor"), use_ball_rpy=False)
plant.Finalize()

sym_plant = plant.ToSymbolic()
sym_context = sym_plant.CreateDefaultContext()

# State variables
nq = plant.num_positions()
nv = plant.num_velocities()
nu = plant.num_actuators()
q = MakeVectorVariable(nq, "q")
v = MakeVectorVariable(nv, "v")
vd = MakeVectorVariable(nv, "\dot{v}")
tau = MakeVectorVariable(nu, "u")

# Parameters
m = Variable("m")

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

base = sym_plant.GetBodyByName("base_link")
base.SetMass(sym_context, m)

derivatives = sym_context.Clone().get_mutable_continuous_state()
derivatives.SetFromVector(np.hstack(([0] * nq, vd)))
residual = sym_plant.CalcImplicitTimeDerivativesResidual(
    sym_context, derivatives
)
print("Symbolic acceleration residuals: ")
display(Markdown(f"${ToLatex(residual[nq:], 2)}$"))


W, alpha, w0 = DecomposeLumpedParameters(residual[nq:], [m])