Trait RobotTorque 

pub trait RobotTorque: RobotBasic {
    // Required method
    fn inverse_dynamics(
        &self,
        q: &[f64],
        dq: &[f64],
        ddq: &[f64],
        tau: &mut [f64],
    ) -> Result<(), RobotDynamicsError>;
}

Robot trait with inverse-dynamics capability.

This trait is mainly required when building COPP2/COPP3 problems with torque/dynamics constraints. For TOPP-only use cases, a direct Constraints workflow is usually enough.

A usize variable can serve as a trivial RobotTorque implementation representing a point-mass model, where tau = ddq. For physical robots, users should implement this trait with their own inverse dynamics.

Required Methods

fn inverse_dynamics( &self, q: &[f64], dq: &[f64], ddq: &[f64], tau: &mut [f64], ) -> Result<(), RobotDynamicsError>

Evaluate inverse dynamics.

tau = M(q) * ddq + C(q, dq) * dq + g(q) + f(q, sgn(dq))

For a robot with dimension dim:

• q joint positions (dim).
• dq: joint velocities (dim).
• ddq: joint accelerations (dim).
• tau: output required torques/forces (dim).
• dq, ddq, and tau are vectors in R^dim.
• M(q) is the inertia/mass matrix in R^(dim x dim).
• C(q, dq) is the Coriolis/centrifugal matrix in R^(dim x dim). It must satisfy C(q, lambda * dq) = lambda * C(q, dq) for any scalar lambda.
• g(q) is the gravity torque/force vector in R^dim.
• f(q, sgn(dq)) is the dry-friction torque/force vector in R^dim, where sgn(dq) is interpreted element-wise. It is required that f(q, sgn(dq)) is zero if dq is zero. An common dry-friction model is biased Coulomb friction.

Important: when implementing the sign function sgn(dq), zero and near-zero velocities must map to 0, not to +1 or -1. A recommended convention is to treat abs(dq[i]) <= 1e-16 as zero.

On success, write every entry of tau and return Ok(()). The tau buffer may contain old values on entry; implementations should overwrite it directly rather than reading or accumulating into existing contents. If the dynamics backend cannot evaluate this state, return Err(RobotDynamicsError::new(message)) or Err(message.into()) with a user-facing reason.

Implementations on Foreign Types

impl RobotTorque for usize

fn inverse_dynamics( &self, _q: &[f64], _dq: &[f64], ddq: &[f64], tau: &mut [f64], ) -> Result<(), RobotDynamicsError>

Evaluate inverse dynamics for point-mass model.

Since tau = ddq, this function copies ddq directly into tau.

Implementors