Skip to content

Commit

Permalink
Update ode.system_velocity_dynamics to use new contact function
Browse files Browse the repository at this point in the history
  • Loading branch information
diegoferigo committed Mar 22, 2024
1 parent d554670 commit e01b5c2
Showing 1 changed file with 5 additions and 15 deletions.
20 changes: 5 additions & 15 deletions src/jaxsim/api/ode.py
Original file line number Diff line number Diff line change
Expand Up @@ -119,28 +119,18 @@ def system_velocity_dynamics(
# with the terrain.
W_f_Li_terrain = jnp.zeros_like(W_f_L).astype(float)

# Initialize the 6D contact forces W_f ∈ ℝ^{n_c × 3} applied to collidable points,
# Initialize the 6D contact forces W_f ∈ ℝ^{n_c × 6} applied to collidable points,
# expressed in the world frame.
W_f_Ci = None

# Initialize the derivative of the tangential deformation ṁ ∈ ℝ^{n_c × 3}.
= jnp.zeros_like(data.state.soft_contacts.tangential_deformation).astype(float)

if len(model.kin_dyn_parameters.contact_parameters.body) > 0:
# Compute the position and linear velocities (mixed representation) of
# all collidable points belonging to the robot.
W_p_Ci, W_ṗ_Ci = js.contact.collidable_point_kinematics(model=model, data=data)

# Build the soft contact model.
soft_contacts = jaxsim.rbda.soft_contacts.SoftContacts(
parameters=data.soft_contacts_params, terrain=model.terrain
)

# Compute the 3D force applied to each collidable point and the
# corresponding material deformation rate.
W_f_Ci, = jax.vmap(soft_contacts.contact_model)(
W_p_Ci, W_ṗ_Ci, data.state.soft_contacts.tangential_deformation
)
# Compute the 6D forces applied to each collidable point and the
# corresponding material deformation rates.
with data.switch_velocity_representation(VelRepr.Inertial):
W_f_Ci, = js.contact.collidable_point_dynamics(model=model, data=data)

# Construct the vector defining the parent link index of each collidable point.
# We use this vector to sum the 6D forces of all collidable points rigidly
Expand Down

0 comments on commit e01b5c2

Please sign in to comment.