removed hardcoded numbers

examples/drake_walking.ipynb

@@ -148,7 +148,7 @@
     "# Define simulator and initial position\n",
     "drake_instance = DrakeSimulator()\n",
     "drake_instance.set_visualize_robot_flag(True)\n",
-    "drake_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im)\n",
+    "drake_instance.load_model(robot_model_init, s=s_des, xyz_rpy=xyz_rpy, kv_motors=kv, Im=Im, keep_collision_meshes=['l_foot_rear', 'l_foot_front', 'r_foot_rear', 'r_foot_front'])\n",
     "s, ds, tau = drake_instance.get_state()\n",
     "t = drake_instance.get_simulation_time()\n",
     "H_b = drake_instance.get_base()\n",

src/comodo/drakeSimulator/drakeSimulator.py

@@ -33,14 +33,14 @@ def __init__(self) -> None:
         self.active_meshcat = False
         super().__init__()
 
-    def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None):
+    def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None, keep_collision_meshes=[]):
         # load the robot model construct the diagram and store the simulator
         self.robot_model = robot_model
         self.urdf_string = robot_model.urdf_string
 
         # convert the urdf string to be drake compatible
         self.duh.load_urdf(urdf_string=self.urdf_string)
-        self.duh.remove_all_collisions()
+        self.duh.remove_all_collisions(keep_collision_meshes)
         self.duh.fix_not_in_joint_list(self.robot_model.joint_name_list)
         self.duh.convert_xml_to_odio()
         self.duh.add_acutation_tags()
@@ -66,12 +66,6 @@ def load_model(self, robot_model, s, xyz_rpy, kv_motors=None, Im=None):
         # add the ground and the feet
         self.duh.add_ground_with_friction(plant)
 
-        # configure feet collisions
-        xMinMax = [-0.225, 0.225]
-        yMinMax = [-0.05, 0.05]
-        foot_frames = [robot_model.left_foot_frame, robot_model.right_foot_frame]
-        self.duh.add_soft_feet_collisions(plant, xMinMax=xMinMax, yMinMax=yMinMax, foot_frames = foot_frames)
-
         plant.Finalize()
         builder.ExportInput(plant.get_actuation_input_port(), "control_input")
         builder.ExportOutput(plant.get_state_output_port(), "state_output")
@@ -143,8 +137,6 @@ def set_visualize_robot_flag(self, visualize_robot):
             self.meshcat = StartMeshcat()
             self.active_meshcat = True
         pass
-
-    # does this need to simulator specific?
     def set_base_pose_in_drake(self, xyz_rpy):
         base_xyz_quat = np.zeros(7)
         # order -- quaternion+xyz

src/comodo/drakeSimulator/drakeURDFHelper.py

@@ -68,9 +68,8 @@ def convert_meshes_to_obj(self):
 
         logging.info("Converted all the meshes from .stl to .obj")
 
-    def remove_all_collisions(self):
+    def remove_all_collisions(self, list=[]):
         """Removes all the collision tags from the URDF"""
-        list = []
         for link in self.root.findall("./link"):
             if link.attrib["name"] not in list:
                 for col in link.findall("./collision"):
@@ -143,8 +142,9 @@ def add_ground_with_friction(self, plant):
             static_friction=1.0, dynamic_friction=1.0
         )
         proximity_properties_ground = ProximityProperties()
+        # https://drake.mit.edu/pydrake/pydrake.geometry.html?highlight=addcontactmaterial#pydrake.geometry.AddContactMaterial
         AddContactMaterial(
-            1e4, 1e7, surface_friction_ground, proximity_properties_ground
+            dissipation=1e4, point_stiffness=1e7, friction=surface_friction_ground, properties=proximity_properties_ground, 
         )
         AddRigidHydroelasticProperties(0.01, proximity_properties_ground)
 
@@ -160,16 +160,19 @@ def add_ground_with_friction(self, plant):
             RigidTransform(),
             HalfSpace(),
             "ground_visual",
-            np.array([0.5, 0.5, 0.5, 0.0]),
+            np.array([0.5, 0.5, 0.5, 0.0]), # modify to set the RGBA color of the ground plane
         )
 
     def add_soft_feet_collisions(self, plant, xMinMax, yMinMax, foot_frames):
         surface_friction_feet = CoulombFriction(
             static_friction=1.0, dynamic_friction=1.0
         )
         proximity_properties_feet = ProximityProperties()
-        AddContactMaterial(1e4, 1e7, surface_friction_feet, proximity_properties_feet)
-        AddCompliantHydroelasticProperties(0.01, 1e8, proximity_properties_feet)
+        AddContactMaterial(
+            dissipation=1e4, point_stiffness=1e7, friction=surface_friction_feet, properties=proximity_properties_feet, 
+        )
+        # https://drake.mit.edu/pydrake/pydrake.geometry.html?highlight=addcontactmaterial#pydrake.geometry.AddCompliantHydroelasticProperties
+        AddCompliantHydroelasticProperties(resolution_hint=0.01, hydroelastic_modulus=1e8, properties=proximity_properties_feet)
 
         for ii in foot_frames:
             # for collision
@@ -187,5 +190,5 @@ def add_soft_feet_collisions(self, plant, xMinMax, yMinMax, foot_frames):
                 RigidTransform(np.array([0, 0, 0])),
                 BoxDrake((xMinMax[1] - xMinMax[0]) / 2, yMinMax[1] - yMinMax[0], 2e-3),
                 ii + "_collision",
-                np.array([1.0, 1.0, 1.0, 1]),
+                np.array([1.0, 1.0, 1.0, 1]), # modify to set the RGBA color of the feet collisions
             )