Skip to content

Commit

Permalink
Convert base post impact velocity to inertial before setting
Browse files Browse the repository at this point in the history
  • Loading branch information
flferretti committed Feb 10, 2025
1 parent 9b2a238 commit ea4f1c5
Showing 1 changed file with 11 additions and 9 deletions.
20 changes: 11 additions & 9 deletions src/jaxsim/api/model.py
Original file line number Diff line number Diff line change
Expand Up @@ -2148,17 +2148,19 @@ def step(
)
)

BW_ν_post_impact_inertial = data_tf.other_representation_to_inertial(
array=BW_ν_post_impact[0:6],
other_representation=VelRepr.Mixed,
transform=data_tf._base_transform.at[0:3, 0:3].set(jnp.eye(3)),
is_force=False,
)

# Reset the generalized velocity.
data_tf = data_tf.replace(
model=model,
base_linear_velocity=BW_ν_post_impact[0:3],
base_angular_velocity=BW_ν_post_impact[3:6],
data_tf = dataclasses.replace(
velocity_representation=data.velocity_representation,
base_linear_velocity=BW_ν_post_impact_inertial[0:3],
base_angular_velocity=BW_ν_post_impact_inertial[3:6],
joint_velocities=BW_ν_post_impact[6:],
)

# Restore the input velocity representation
data_tf = data_tf.replace(
velocity_representation=data.velocity_representation, validate=False
)

return data_tf

0 comments on commit ea4f1c5

Please sign in to comment.