Function impedance_torques 

pub fn impedance_torques(
    options: &ImpedanceOptions,
    jacobian: &[f64; 42],
    q_goal: &[f64; 7],
    dq_goal: &[f64; 7],
    q: &[f64; 7],
    dq: &[f64; 7],
    coriolis: &[f64; 7],
) -> [f64; 7]

The torque backend’s law, offline:

Kp  = Jᵀ Kx J + diag(Kq)        Kd = Jᵀ Kxd J + diag(Kqd)
tau = Kp (q_goal - q) + Kd (g dq_goal - dq) + coriolis      clamped to torque_limits

g = velocity_feedforward_gain, 0 when velocity_feedforward is off

with the gains of options; dq_goal is taken as zero without velocity_feedforward, and the joint gains are confined to the nullspace of J with project_joint_gains. jacobian is the zero Jacobian at the end effector in the measured configuration, column-major 6x7 as Model::zero_jacobian returns it.

The torque loop fades out this torque along a joint’s motion as the joint speeds from the fade’s start (FADE_BAND) to the barrier’s onset, adds the velocity barrier (VELOCITY_BARRIER_GAIN) and clamps the sum to torque_limits again.