diff --git a/examples/jaxsim_walking.ipynb b/examples/jaxsim_walking.ipynb index 6c57ab4..aacd67d 100644 --- a/examples/jaxsim_walking.ipynb +++ b/examples/jaxsim_walking.ipynb @@ -273,7 +273,6 @@ " counter = 0\n", " mpc_success = True\n", " succeded_controller = True\n", - " contact_model_type = js.contact_model_type\n", "\n", " t = 0.0\n", "\n", @@ -361,10 +360,9 @@ " W_p_rf_sfp_log.append(rf_sfp.transform.translation())\n", " W_p_CoM_tsid_log.append(tsid.COM.toNumPy())\n", " wall_time_step_log.append(toc * 1e3)\n", - " if contact_model_type != JaxsimContactModelEnum.VISCO_ELASTIC:\n", - " f_lf_js, f_rf_js = js.feet_wrench\n", - " f_lf_js_log.append(f_lf_js)\n", - " f_rf_js_log.append(f_rf_js)\n", + " f_lf_js, f_rf_js = js.feet_wrench\n", + " f_lf_js_log.append(f_lf_js)\n", + " f_rf_js_log.append(f_rf_js)\n", "\n", " except Exception as e:\n", " print(f\"Exception during simulation at time{t}: {e}\")\n", @@ -387,9 +385,8 @@ " \"W_p_CoM_tsid\": np.array(W_p_CoM_tsid_log),\n", " \"wall_time_step\": np.array(wall_time_step_log),\n", " }\n", - " if contact_model_type != JaxsimContactModelEnum.VISCO_ELASTIC:\n", - " logs[\"f_lf_js\"] = np.array(f_lf_js_log)\n", - " logs[\"f_rf_js\"] = np.array(f_rf_js_log)\n", + " logs[\"f_lf_js\"] = np.array(f_lf_js_log)\n", + " logs[\"f_rf_js\"] = np.array(f_rf_js_log)\n", "\n", " return logs" ] @@ -436,9 +433,8 @@ "W_p_rf_sfp = logs[\"W_p_rf_sfp\"]\n", "W_p_CoM_tsid = logs[\"W_p_CoM_tsid\"]\n", "wall_time_step = logs[\"wall_time_step\"]\n", - "if js.contact_model_type != JaxsimContactModelEnum.VISCO_ELASTIC:\n", - " f_lf_js = logs[\"f_lf_js\"]\n", - " f_rf_js = logs[\"f_rf_js\"]" + "f_lf_js = logs[\"f_lf_js\"]\n", + "f_rf_js = logs[\"f_rf_js\"]" ] }, { diff --git a/src/comodo/jaxsimSimulator/jaxsimSimulator.py b/src/comodo/jaxsimSimulator/jaxsimSimulator.py index b78dfe4..7938bde 100644 --- a/src/comodo/jaxsimSimulator/jaxsimSimulator.py +++ b/src/comodo/jaxsimSimulator/jaxsimSimulator.py @@ -41,7 +41,6 @@ class JaxsimContactModelEnum(enum.IntEnum): RIGID = enum.auto() RELAXED_RIGID = enum.auto() - VISCO_ELASTIC = enum.auto() SOFT = enum.auto() @@ -163,8 +162,6 @@ def load_model( contact_model = jaxsim.rbda.contacts.RigidContacts.build() case JaxsimContactModelEnum.RELAXED_RIGID: contact_model = jaxsim.rbda.contacts.RelaxedRigidContacts.build() - case JaxsimContactModelEnum.VISCO_ELASTIC: - contact_model = jaxsim.rbda.contacts.ViscoElasticContacts.build() case JaxsimContactModelEnum.SOFT: contact_model = jaxsim.rbda.contacts.SoftContacts.build() case _: @@ -172,6 +169,15 @@ def load_model( f"Invalid contact model type: {self._contact_model_type}" ) + model = js.model.JaxSimModel.build_from_model_description( + model_description=robot_model.urdf_string, + model_name=robot_model.robot_name, + contact_model=contact_model, + contact_params=contact_params, + integrator=js.model.IntegratorType.RungeKutta4, + time_step=self._dt, + ) + if contact_params is None: match self._contact_model_type: case JaxsimContactModelEnum.RIGID: @@ -182,9 +188,9 @@ def load_model( contact_params = ( jaxsim.rbda.contacts.RelaxedRigidContactsParams.build() ) - case JaxsimContactModelEnum.VISCO_ELASTIC | JaxsimContactModelEnum.SOFT: + case JaxsimContactModelEnum.SOFT: contact_params = js.contact.estimate_good_contact_parameters( - model=self._model, + model=model, number_of_active_collidable_points_steady_state=8, max_penetration=0.002_5, damping_ratio=1.0, @@ -194,14 +200,8 @@ def load_model( raise ValueError( f"Invalid contact model type: {self._contact_model_type}" ) - - model = js.model.JaxSimModel.build_from_model_description( - model_description=robot_model.urdf_string, - model_name=robot_model.robot_name, - contact_model=contact_model, - contact_params=contact_params, - time_step=self._dt, - ) + with model.editable(validate=False) as model: + model.contacts_params = contact_params self._model = js.model.reduce( model=model, @@ -304,24 +304,13 @@ def step(self, n_step: int = 1, *, dry_run=False) -> None: for _ in range(n_step): - if self._contact_model_type is JaxsimContactModelEnum.VISCO_ELASTIC: - - self._data = jaxsim.rbda.contacts.visco_elastic.step( - model=self._model, - data=self._data, - link_forces=None, - joint_force_references=self._tau, - ) - - else: - - # All other contact models - self._data = js.model.step( - model=self._model, - data=self._data, - # link_forces=None, - joint_force_references=self._tau, - ) + # All other contact models + self._data = js.model.step( + model=self._model, + data=self._data, + # link_forces=None, + joint_force_references=self._tau, + ) if not dry_run: # Advance simulation time @@ -346,7 +335,7 @@ def step(self, n_step: int = 1, *, dry_run=False) -> None: with self._data.switch_velocity_representation(VelRepr.Mixed): - self._link_contact_forces = js.contact_model.link_contact_forces( + self._link_contact_forces, _ = js.contact.link_contact_forces( model=self._model, data=self._data, joint_torques=self._tau,