Fix actuator_velocity indexing in the LuGre bristle update.
mj_nextActivation indexed actuator_velocity with the actuator id, but the array is laid out by output address. The two coincide only when all preceding actuators are single-output, so a DC motor placed after a multi-output SO3 actuator integrated its bristle state with another actuator's velocity, while act_dot (computed in mj_fwdActuation with the correct velocity) disagreed with the actual evolution of the state. Fixes DCMotorTest.LuGreBristleVelocityOrderInvariance.
This commit is contained in:
@@ -735,7 +735,7 @@ mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
|
||||
mjtNum F_S = biasprm[4]; // static friction
|
||||
mjtNum v_S = biasprm[5]; // Stribeck velocity
|
||||
mjtNum sigma0 = dynprm[5]; // bristle stiffness
|
||||
mjtNum velocity = d->actuator_velocity[actuator_id];
|
||||
mjtNum velocity = d->actuator_velocity[m->actuator_outadr[actuator_id]];
|
||||
mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
|
||||
|
||||
// ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v
|
||||
|
||||
Reference in New Issue
Block a user