Lagrange Multiplier Effect in Implicit Contact Formulation and Derivative Calculation Error

[Ipuch]

Description:

It appears there is an issue with the implicit formulation for handling contact dynamics.
The Lagrange multipliers for the holonomic constraints do seem to be correctly incorporated into the equations of motion (forward_dynamics and inverse_dynamics), but through the external force modules, which is kind of awkward.

• We should revise this by explicitly displaying the constraint Jacobian.

Several related issues have been identified:

• Misplaced Holonomic Constraint Function: The script holonomic_constraint_function.py has been moved to the limits directory. This is not an appropriate location, as it's a core tool for modifying the model and its equations of motion, defining the holonomic constraints themselves. While it can be used by the optimization, it is fundamentally a model feature.

• Inefficient or Incorrect Acceleration Constraint: The point acceleration constraint is currently implemented in CasADi. This approach does not leverage the sparsity of the problem that biorbd::ContactAcceleration provides, leading to potentially slower computations due to the differentiation of the CasADi graph. It is recommended to replace this with the native biorbd implementation. Incorrect Derivative Calculation for Acceleration Constraints: There seems to be an error in the calculation of the time derivatives of the holonomic constraints to get the acceleration-level constraints (ϕddot (q)). The current implementation appears to be missing terms from the full chain rule expansion.

Proposed Solution / Bug Fix:

The immediate bug in the acceleration constraint calculation can be corrected. Two potential solutions are proposed below.

Version 1: Using ca.jtimes

This version leverages jtimes to correctly apply the chain rule for the second time derivative of the constraint function phi.

# First derivative (velocity constraint): phi_dot = J(q) * q_dot
velocity_constraint = ca.jtimes(constraint, q_sym, q_dot_sym)

# Second derivative (acceleration constraint): phi_double_dot = d(phi_dot)/dt
# phi_dot is a function of q and q_dot.
# Its time derivative needs the chain rule applied to both q and q_dot.
# d(phi_dot)/dt = (d(phi_dot)/dq) * q_dot + (d(phi_dot)/dq_dot) * q_double_dot
# In jtimes, this is done by passing the "state" as vertcat(q, q_dot)
# and its "velocity" as vertcat(q_dot, q_double_dot).
acceleration_constraint = ca.jtimes(
    velocity_constraint,
    ca.vertcat(q_sym, q_dot_sym),    # Variables that 'velocity_constraint' depends on
    ca.vertcat(q_dot_sym, q_ddot_sym) # Their time derivatives
)

Version 2: Explicit Formulation

This version explicitly calculates the two main components of the acceleration constraint:

# Term 1: J_phi(q) * q_double_dot
term_jacobian_ddot = constraints_jacobian @ q_ddot_sym

# Term 2: The vector of (q_dot^T * H_phi_i(q) * q_dot) terms
# This requires iterating over each scalar constraint within the 'constraint' vector
term_hessian_q_dot_squared = ca.MX() # Initialize an empty MX vector to accumulate terms

m_constraints = constraint.shape[0]
for i_const in range(m_constraints):
    phi_i = constraint[i_const] # Get the i-th scalar constraint
    
    # Get the Hessian matrix of the i-th scalar constraint w.r.t. q_sym
    # casadi.hessian(f, x) returns (H, g), so we take the [0] element for H
    H_phi_i = ca.hessian(phi_i, q_sym)[0]
    
    # Compute the quadratic form: q_dot_sym.T @ H_phi_i @ q_dot_sym
    quadratic_form_i = q_dot_sym.T @ H_phi_i @ q_dot_sym
    
    # Vertically concatenate this scalar result to the accumulator vector
    term_hessian_q_dot_squared = ca.vertcat(term_hessian_q_dot_squared, quadratic_form_i)

# Combine both terms to get the full acceleration constraint
acceleration_constraint = term_hessian_q_dot_squared + term_jacobian_ddot