RobotLocomotion
Enable computing the inverse dynamics in the lumped inertial parameters that the manipulator equations are linear in
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.
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?
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?
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.
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?
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).
For my part, I would feel comfortable exploring on my own (without a fully-worked, end-to-end example), but the thing I would like to see beyond the example above would be the specific plant you'd like to use in the example (i.e., how to load or set up the arm model(s)).
Thank you for coming back to this!
I have a script that uses the above function here. The script also includes the option to load a one-link arm as a sanity check.
That script loads the iiwa and creates a symbolic plant for it:
directives:
- add_model:
name: iiwa
file: package://drake_models/iiwa_description/sdf/iiwa7_no_collision.sdf
default_joint_positions:
iiwa_joint_1: [0]
iiwa_joint_2: [0.1]
iiwa_joint_3: [0]
iiwa_joint_4: [-1.2]
iiwa_joint_5: [0]
iiwa_joint_6: [1.6]
iiwa_joint_7: [0]
- add_weld:
parent: world
child: iiwa::iiwa_link_0
The iiwa symbolic data matrix would help both efficient system identification and optimal excitation trajectory design. Optimal excitation trajectory design in particular would need to take the derivative of that data matrix and thus doesn't work without the symbolic form of the data matrix (ignoring the black box optimization option). The Drake panda model would be an equivalent example.