Upgrade to Pinocchio 3.3.1 and Drake 1.36.0

examples/collision_along_path.py

@@ -5,7 +5,7 @@
 motion planning components such as inverse kinematics and path planning.
 """
 
-import hppfcl
+import coal
 import pinocchio
 from pinocchio.visualize import MeshcatVisualizer
 import meshcat.geometry as mg
@@ -29,17 +29,17 @@ def prepare_collision_scene(model, collision_model):
     obstacle_0 = pinocchio.GeometryObject(
         "obstacle_0",
         0,
-        hppfcl.Sphere(0.2),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])),
+        coal.Sphere(0.2),
     )
     obstacle_0.meshColor = np.array([0.0, 1.0, 0.0, 0.2])
     collision_model.addGeometryObject(obstacle_0)
 
     obstacle_1 = pinocchio.GeometryObject(
         "obstacle_1",
         0,
-        hppfcl.Box(0.3, 0.3, 0.3),
         pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])),
+        coal.Box(0.3, 0.3, 0.3),
     )
     obstacle_1.meshColor = np.array([0.0, 1.0, 0.0, 0.2])
     collision_model.addGeometryObject(obstacle_1)
@@ -102,10 +102,7 @@ def prepare_collision_scene(model, collision_model):
                 )
                 for contact in cr.getContacts():
                     contacts.extend(
-                        [
-                            contact.pos,
-                            contact.pos - contact.normal * contact.penetration_depth,
-                        ]
+                        [contact.getNearestPoint1(), contact.getNearestPoint2()]
                     )
         if len(contacts) == 0:
             print("Found no collisions!")

examples/intro_pinocchio/intro_pinocchio_manual.py

@@ -7,7 +7,7 @@
 import pinocchio
 from pinocchio.visualize import MeshcatVisualizer
 
-import hppfcl
+import coal
 import numpy as np
 import time
 
@@ -250,53 +250,53 @@ def create_visual_model(model, alpha=0.5):
     link0 = pinocchio.GeometryObject(
         "link0",
         model.getJointId("joint0"),
-        hppfcl.Box(0.25, 0.25, 0.2),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.1])),
+        coal.Box(0.25, 0.25, 0.2),
     )
     link0.meshColor = np.array([1.0, 0.0, 0.0, alpha])
     visual_model.addGeometryObject(link0)
 
     link1 = pinocchio.GeometryObject(
         "link1",
         model.getJointId("joint1"),
-        hppfcl.Cylinder(0.075, 0.5),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.25])),
+        coal.Cylinder(0.075, 0.5),
     )
     link1.meshColor = np.array([0.0, 0.0, 1.0, alpha])
     visual_model.addGeometryObject(link1)
 
     link2 = pinocchio.GeometryObject(
         "link2",
         model.getJointId("joint2"),
-        hppfcl.Cylinder(0.075, 0.5),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.25])),
+        coal.Cylinder(0.075, 0.5),
     )
     link2.meshColor = np.array([0.0, 1.0, 0.0, alpha])
     visual_model.addGeometryObject(link2)
 
     link3 = pinocchio.GeometryObject(
         "link3",
         model.getJointId("joint3"),
-        hppfcl.Sphere(0.1),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.1])),
+        coal.Sphere(0.1),
     )
     link3.meshColor = np.array([1.0, 0.0, 1.0, alpha])
     visual_model.addGeometryObject(link3)
 
     link4 = pinocchio.GeometryObject(
         "link4",
         model.getJointId("joint4"),
-        hppfcl.Sphere(0.1),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.1])),
+        coal.Sphere(0.1),
     )
     link4.meshColor = np.array([0.0, 1.0, 1.0, alpha])
     visual_model.addGeometryObject(link4)
 
     link5 = pinocchio.GeometryObject(
         "link5",
         model.getJointId("joint5"),
-        hppfcl.Sphere(0.1),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.1])),
+        coal.Sphere(0.1),
     )
     link5.meshColor = np.array([1.0, 1.0, 0.0, alpha])
     visual_model.addGeometryObject(link5)
@@ -305,26 +305,26 @@ def create_visual_model(model, alpha=0.5):
     ee_base_link = pinocchio.GeometryObject(
         "ee_base",
         model.getJointId("joint5"),
-        hppfcl.Box(0.2, 0.1, 0.05),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, 0.2])),
+        coal.Box(0.2, 0.1, 0.05),
     )
     ee_base_link.meshColor = np.array([0.6, 0.6, 0.6, alpha])
     visual_model.addGeometryObject(ee_base_link)
 
     ee_left_link = pinocchio.GeometryObject(
         "ee_left",
         model.getJointId("joint5"),
-        hppfcl.Box(0.025, 0.1, 0.2),
         pinocchio.SE3(np.eye(3), np.array([0.1, 0.0, 0.275])),
+        coal.Box(0.025, 0.1, 0.2),
     )
     ee_left_link.meshColor = np.array([0.6, 0.6, 0.6, alpha])
     visual_model.addGeometryObject(ee_left_link)
 
     ee_right_link = pinocchio.GeometryObject(
         "ee_right",
         model.getJointId("joint5"),
-        hppfcl.Box(0.025, 0.1, 0.2),
         pinocchio.SE3(np.eye(3), np.array([-0.1, 0.0, 0.275])),
+        coal.Box(0.025, 0.1, 0.2),
     )
     ee_right_link.meshColor = np.array([0.6, 0.6, 0.6, alpha])
     visual_model.addGeometryObject(ee_right_link)

examples/intro_pinocchio/intro_pinocchio_ur5_sim.py

@@ -120,10 +120,7 @@ def visualize_collisions(viz, collision_model, collision_data):
             )
             for contact in cr.getContacts():
                 contacts.extend(
-                    [
-                        contact.pos,
-                        contact.pos - contact.normal * contact.penetration_depth,
-                    ]
+                    [contact.getNearestPoint1(), contact.getNearestPoint2()]
                 )
         else:
             distances.extend([dr.getNearestPoint1(), dr.getNearestPoint2()])

pyproject.toml

@@ -6,8 +6,8 @@ build-backend = "hatchling.build"
 name = "pyroboplan"
 version = "1.1.0"
 dependencies = [
-  "drake == 1.30.0",
-  "pin == 2.7.0",
+  "drake == 1.36.0",
+  "pin == 3.3.1",
   "matplotlib == 3.9.0",
   "meshcat == 0.3.2",
   "scipy == 1.13.1",

src/pyroboplan/core/utils.py

@@ -512,14 +512,10 @@ def calculate_collision_vector_and_jacobians(
     cr = collision_data.collisionResults[pair_idx]
     dr = collision_data.distanceResults[pair_idx]
 
+    # According to the HPP-FCL documentation, the normal always points from object1 to object2.
     if cr.isCollision():
-        # According to the HPP-FCL documentation, the normal always points from object1 to object2.
-        # TODO: Once HPP-FCL Python bindings expose the getNearestPointN() functions, use those instead.
-        contact = cr.getContact(0)
-        coll_points = [
-            contact.pos,
-            contact.pos - contact.normal * contact.penetration_depth,
-        ]
+        contact = cr.getContacts()[0]
+        coll_points = [contact.getNearestPoint1(), contact.getNearestPoint2()]
     else:
         coll_points = [dr.getNearestPoint1(), dr.getNearestPoint2()]
 

src/pyroboplan/models/panda.py

@@ -1,6 +1,6 @@
 """ Utilities to load example Franka Emika Panda model. """
 
-import hppfcl
+import coal
 import numpy as np
 import os
 import pinocchio
@@ -69,8 +69,8 @@ def add_object_collisions(model, collision_model, visual_model, inflation_radius
     ground_plane = pinocchio.GeometryObject(
         "ground_plane",
         0,
-        hppfcl.Box(2.0, 2.0, 0.3),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.0, -0.151])),
+        coal.Box(2.0, 2.0, 0.3),
     )
     ground_plane.meshColor = np.array([0.5, 0.5, 0.5, 0.5])
     visual_model.addGeometryObject(ground_plane)
@@ -79,8 +79,8 @@ def add_object_collisions(model, collision_model, visual_model, inflation_radius
     obstacle_sphere_1 = pinocchio.GeometryObject(
         "obstacle_sphere_1",
         0,
-        hppfcl.Sphere(0.2 + inflation_radius),
         pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])),
+        coal.Sphere(0.2 + inflation_radius),
     )
     obstacle_sphere_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5])
     visual_model.addGeometryObject(obstacle_sphere_1)
@@ -89,8 +89,8 @@ def add_object_collisions(model, collision_model, visual_model, inflation_radius
     obstacle_sphere_2 = pinocchio.GeometryObject(
         "obstacle_sphere_2",
         0,
-        hppfcl.Sphere(0.25 + inflation_radius),
         pinocchio.SE3(np.eye(3), np.array([0.5, 0.5, 0.5])),
+        coal.Sphere(0.25 + inflation_radius),
     )
     obstacle_sphere_2.meshColor = np.array([1.0, 1.0, 0.0, 0.5])
     visual_model.addGeometryObject(obstacle_sphere_2)
@@ -99,12 +99,12 @@ def add_object_collisions(model, collision_model, visual_model, inflation_radius
     obstacle_box_1 = pinocchio.GeometryObject(
         "obstacle_box_1",
         0,
-        hppfcl.Box(
+        pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])),
+        coal.Box(
             0.25 + 2.0 * inflation_radius,
             0.55 + 2.0 * inflation_radius,
             0.55 + 2.0 * inflation_radius,
         ),
-        pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])),
     )
     obstacle_box_1.meshColor = np.array([1.0, 0.0, 0.0, 0.5])
     visual_model.addGeometryObject(obstacle_box_1)
@@ -113,12 +113,12 @@ def add_object_collisions(model, collision_model, visual_model, inflation_radius
     obstacle_box_2 = pinocchio.GeometryObject(
         "obstacle_box_2",
         0,
-        hppfcl.Box(
+        pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])),
+        coal.Box(
             0.33 + 2.0 * inflation_radius,
             0.33 + 2.0 * inflation_radius,
             0.33 + 2.0 * inflation_radius,
         ),
-        pinocchio.SE3(np.eye(3), np.array([-0.5, -0.5, 0.75])),
     )
     obstacle_box_2.meshColor = np.array([0.0, 0.0, 1.0, 0.5])
     visual_model.addGeometryObject(obstacle_box_2)

src/pyroboplan/models/two_dof.py

@@ -1,6 +1,6 @@
 """ Utilities to load example 2-DOF manipulator. """
 
-import hppfcl
+import coal
 import numpy as np
 import os
 import pinocchio
@@ -41,8 +41,8 @@ def add_object_collisions(model, collision_model, visual_model):
     obstacle_1 = pinocchio.GeometryObject(
         "obstacle_1",
         0,
-        hppfcl.Cylinder(0.3, 0.1),
         pinocchio.SE3(np.eye(3), np.array([1.0, 1.0, 0.0])),
+        coal.Cylinder(0.3, 0.1),
     )
     obstacle_1.meshColor = np.array([0.0, 1.0, 0.0, 0.5])
     visual_model.addGeometryObject(obstacle_1)
@@ -51,8 +51,8 @@ def add_object_collisions(model, collision_model, visual_model):
     obstacle_2 = pinocchio.GeometryObject(
         "obstacle_2",
         0,
-        hppfcl.Box(0.5, 0.5, 0.1),
         pinocchio.SE3(np.eye(3), np.array([-1.0, -0.75, 0.0])),
+        coal.Box(0.5, 0.5, 0.1),
     )
     obstacle_2.meshColor = np.array([1.0, 0.0, 0.0, 0.5])
     visual_model.addGeometryObject(obstacle_2)

test/trajectory/test_trajectory_optimization.py

@@ -43,8 +43,7 @@ def test_start_goal_traj_opt():
         max_jerk=1.0,
     )
     planner = CubicTrajectoryOptimization(model, collision_model, options)
-    with pytest.warns(RuntimeWarning):  # There are some invalid multiply values
-        traj = planner.plan([q_start, q_goal])
+    traj = planner.plan([q_start, q_goal])
 
     assert traj is not None
     assert traj.num_dims == 9
@@ -96,8 +95,7 @@ def test_full_path_traj_opt():
         max_jerk=1.0,
     )
     planner = CubicTrajectoryOptimization(model, collision_model, options)
-    with pytest.warns(RuntimeWarning):  # There are some invalid multiply values
-        traj = planner.plan(q_path)
+    traj = planner.plan(q_path)
 
     assert traj is not None
     assert traj.num_dims == 9
@@ -136,8 +134,7 @@ def test_traj_opt_unreachable_goal():
     # Perform trajectory optimization
     options = CubicTrajectoryOptimizationOptions(num_waypoints=3)
     planner = CubicTrajectoryOptimization(model, collision_model, options)
-    with pytest.warns(RuntimeWarning):  # There are some invalid multiply values
-        assert planner.plan([q_start, q_goal]) is None
+    assert planner.plan([q_start, q_goal]) is None
 
 
 def test_traj_opt_bad_limits():
@@ -239,7 +236,6 @@ def test_traj_opt_collision_avoidance():
         ],
     )
     planner = CubicTrajectoryOptimization(model, collision_model, options)
-    with pytest.warns(RuntimeWarning):  # There are some invalid multiply values
-        traj = planner.plan([q_path[0], q_path[-1]], init_path=q_path)
+    traj = planner.plan([q_path[0], q_path[-1]], init_path=q_path)
 
     assert traj is not None