Enable computing the inverse dynamics in the lumped inertial parameters that the manipulator equations are linear in

[nepfaff]

Is your feature request related to a problem? Please describe. I'm interested in computing the inverse dynamics in terms of the lumped inertial parameters that the manipulator equations are linear in. At the moment, Drake uses mass (m), the centre of mass (p), and rotational unit inertia (G) as its base parameters. However, the dynamics are linear in mass, center of mass moment (h=mp), and rotational inertia (I=mG). Drake's parameter choice prevents me from properly exploiting the structure of the equations symbolically, as I will end up with terms such as m * (h / m) that are hard to simplify (practically impossible to simplify for bigger systems such as a 7 DoF manipulator).

Describe the solution you'd like To enable exploiting this structure, I suggest providing an implementation of the inverse dynamics that uses m, h, and I directly. I have a hacky implementation of this, and it solves all my symbolic issues. Only m, h, and I affect the dynamics, so it should be possible to implement everything in terms of these parameters and then provide a constructor that converts from the more intuitive parameters m, p, and G. However, I might be missing some ways in which this is undesirable.

My hacky implementation can be found in https://github.com/RobotLocomotion/drake/pull/20698. This should be rewritten entirely if we decide to proceed with this idea. I'd be happy to provide a cleaned-up version if deemed helpful.

Additional context I'd be happy to help implement the suggested feature or alternative features (e.g. on the symbolic level) that would enable the same functionality.

[nepfaff]

@amcastro-tri for reference

[jwnimmer-tri]

Drake's parameter choice prevents me from properly exploiting the structure of the equations symbolically, as I will end up with terms such as m * (h / m) that are hard to simplify ...

Perhaps there is a narrower solution here? What if we change something in the low-level plant or tree code (e.g., ToSpatialInertia in parameter_conversion.h) to aggressively perform that symbolic cancellation early on in the computation (with some pattern matching heuristic), so that the higher-layer dynamics code ends up with the lumped form automatically?

[nepfaff]

This would be a much neater solution if it is possible. I don't know much about the feasibility of this. Symbolic fraction simplification is fundamentally hard due to division by zero, etc. so this would need to be very early on. Would this be feasible with Drake's recursive symbolics?

[jwnimmer-tri]

Maybe the easiest answer is just to try it. I think overall this issue would be most clear if we had sample application code that used the lumped parameters in earnest, so we could anchor the use case and then vet various implementation tactics against that anchor.

[sherm1]

cc @mitiguy

[nepfaff]

Maybe the easiest answer is just to try it. I think overall this issue would be most clear if we had sample application code that used the lumped parameters in earnest, so we could anchor the use case and then vet various implementation tactics against that anchor.

I agree with that. What would be a useful way to present such an application? Would python in a separate repo or a python gist work?

[nepfaff]

The following is a decent application example.

def extract_symbolic_data_matrix(
    symbolic_plant_components: ArmPlantComponents,
    simplify: bool = False,
) -> np.ndarray:
    """Uses symbolic differentiation to compute the symbolic data matrix. Requires the
    inverse dynamics to be computed in terms of the lumped parameters.

    Args:
        symbolic_plant_components (ArmPlantComponents): The symbolic plant components.
        simplify (bool, optional): Whether to simplify the symbolic expressions using
            sympy before computing the Jacobian.

    Returns:
        np.ndarray: The symbolic data matrix.
    """
    # Compute the symbolic torques
    forces = MultibodyForces_[Expression](symbolic_plant_components.plant)
    symbolic_plant_components.plant.CalcForceElementsContribution(
        symbolic_plant_components.plant_context, forces
    )
    sym_torques = symbolic_plant_components.plant.CalcInverseDynamics(
        symbolic_plant_components.plant_context,
        symbolic_plant_components.state_variables.q_ddot.T,
        forces,
    )

    # Compute the symbolic data matrix
    # NOTE: Could parallelise this if needed
    sym_parameters_arr = np.concatenate(
        [
            params.get_lumped_param_list()
            for params in symbolic_plant_components.parameters
        ]
    )
    W_sym: List[Expression] = []
    expression: Expression
    start_time = time.time()
    for expression in tqdm(sym_torques, desc="Computing W_sym"):
        if simplify:
            memo = {}
            expression_sympy = to_sympy(expression, memo=memo)
            # NOTE: This simplification step is very slow. We might be able to do better
            # by manually choosing the simplification methods to use (e.g. 'expand'
            # might suffice)
            simplified_expression_sympy = sympy.simplify(expression_sympy)
            simplified_expression: Expression = from_sympy(
                simplified_expression_sympy, memo=memo
            )
            W_sym.append(simplified_expression.Jacobian(sym_parameters_arr))
        else:
            W_sym.append(expression.Jacobian(sym_parameters_arr))
    W_sym = np.vstack(W_sym)
    logging.info(
        f"Time to compute W_sym: {timedelta(seconds=time.time() - start_time)}"
    )

    return W_sym

Noting that W_sym is only tractable for 7 DoF arms when sym_torques is in the form of lumped parameters.

Would it be helpful for me to write an example script that uses this function? The goal then would be to make the script runnable in finite time and the final equations to be small-ish (e.g. terminal doesn't crash when printing them).