Add example for collision checking along a path

examples/example_collision_along_path.py

@@ -0,0 +1,130 @@
+import copy
+import hppfcl
+import pinocchio
+from pinocchio.visualize import MeshcatVisualizer
+import meshcat.geometry as mg
+import numpy as np
+from os.path import dirname, join, abspath
+import time
+
+from pyroboplan.core.utils import extract_cartesian_poses
+from pyroboplan.planning.utils import discretize_joint_space_path
+from pyroboplan.visualization.meshcat_utils import visualize_frames
+
+
+def prepare_collision_scene(collision_model):
+    """Helper function to create a collision scene for this example."""
+
+    # Modify the collision model so all the Panda links are translucent
+    for cobj in collision_model.geometryObjects:
+        cobj.meshColor = np.array([0.7, 0.7, 0.7, 0.25])
+
+    # Add collision objects
+    obstacle_0 = pinocchio.GeometryObject(
+        "obstacle_0",
+        0,
+        hppfcl.Sphere(0.2),
+        pinocchio.SE3(np.eye(3), np.array([0.0, 0.1, 1.1])),
+    )
+    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.25, 0.25, 0.25),
+        pinocchio.SE3(np.eye(3), np.array([-0.5, 0.5, 0.7])),
+    )
+    obstacle_1.meshColor = np.array([0.0, 1.0, 0.0, 0.2])
+    collision_model.addGeometryObject(obstacle_1)
+
+
+if __name__ == "__main__":
+    # Create models and data
+    pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models")
+    package_dir = join(pinocchio_model_dir, "panda_description")
+    urdf_filename = join(package_dir, "urdf", "panda.urdf")
+
+    model, collision_model, visual_model = pinocchio.buildModelsFromUrdf(
+        urdf_filename, package_dirs=pinocchio_model_dir
+    )
+    data = model.createData()
+
+    prepare_collision_scene(collision_model)
+
+    # Initialize visualizer
+    viz = MeshcatVisualizer(model, collision_model, visual_model, data=data)
+    viz.initViewer(open=True)
+    viz.loadViewerModel()
+    viz.displayCollisions(True)
+    viz.displayVisuals(False)
+
+    # Define the active collision pairs
+    collision_names = [
+        cobj.name for cobj in collision_model.geometryObjects if "panda" in cobj.name
+    ]
+    obstacle_names = ["obstacle_0", "obstacle_1"]
+    for obstacle_name in obstacle_names:
+        for collision_name in collision_names:
+            collision_model.addCollisionPair(
+                pinocchio.CollisionPair(
+                    collision_model.getGeometryId(collision_name),
+                    collision_model.getGeometryId(obstacle_name),
+                )
+            )
+    collision_data = pinocchio.GeometryData(collision_model)
+
+    # Define a joint space path
+    q_start = pinocchio.randomConfiguration(model)
+    q_end = pinocchio.randomConfiguration(model)
+    max_angle_step = 0.05
+    q_path = discretize_joint_space_path(q_start, q_end, max_angle_step)
+
+    # Visualize the path
+    target_tforms = extract_cartesian_poses(model, "panda_hand", q_path)
+    visualize_frames(viz, "path", target_tforms, line_length=0.05, line_width=1)
+    viz.display(q_start)
+    time.sleep(1.0)
+
+    # Collision check along the path
+    for q in q_path:
+        pinocchio.computeCollisions(
+            model, data, collision_model, collision_data, q, False
+        )
+
+        contacts = []
+        for k in range(len(collision_model.collisionPairs)):
+            cr = collision_data.collisionResults[k]
+            cp = collision_model.collisionPairs[k]
+            if cr.isCollision():
+                print(
+                    "collision between:",
+                    collision_model.geometryObjects[cp.first].name,
+                    " and ",
+                    collision_model.geometryObjects[cp.second].name,
+                )
+                for contact in cr.getContacts():
+                    contacts.extend(
+                        [
+                            contact.pos,
+                            contact.pos - contact.normal * contact.penetration_depth,
+                        ]
+                    )
+        if len(contacts) == 0:
+            print("Found no collisions!")
+
+        viz.viewer["collision_display"].set_object(
+            mg.LineSegments(
+                mg.PointsGeometry(
+                    position=np.array(contacts).T,
+                    color=np.array([[1.0, 0.0, 0.0] for _ in contacts]).T,
+                ),
+                mg.LineBasicMaterial(
+                    linewidth=3,
+                    vertexColors=True,
+                ),
+            )
+        )
+
+        viz.display(q)
+        time.sleep(0.2)

examples/old/test_pinocchio_ur5.py

@@ -7,10 +7,10 @@
 import time
 
 # This path refers to Pinocchio source code but you can define your own directory here.
-pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "models")
+pinocchio_model_dir = join(dirname(str(abspath(__file__))), "..", "..", "models")
 
 # You should change here to set up your own URDF file or just pass it as an argument of this example.
-urdf_filename = join(pinocchio_model_dir, "ur_description", "urdf", "ur5_robot.urdf")
+urdf_filename = join(pinocchio_model_dir, "ur_description", "urdf", "ur5_gripper.urdf")
 
 # Load the urdf model
 model = pinocchio.buildModelFromUrdf(urdf_filename)
@@ -80,7 +80,7 @@ def sim_loop():
 
         pinocchio.framesForwardKinematics(model, data, qnext)
         tool_tform = data.oMf[model.getFrameId("tool0")]
-        # print(f"q: {q}\ntool_tform:\n{tool_tform}")
+        print(f"q: {q}\ntool_tform:\n{tool_tform}")
 
         # Compute collisions and distances
         pinocchio.computeCollisions(

src/pyroboplan/core/utils.py

@@ -1,5 +1,6 @@
 """ Core utilities for robot modeling. """
 
+import copy
 import numpy as np
 import pinocchio
 
@@ -69,3 +70,30 @@ def check_within_limits(model, q):
     return np.all(q >= model.lowerPositionLimit) and np.all(
         q <= model.upperPositionLimit
     )
+
+
+def extract_cartesian_poses(model, target_frame, q_vec):
+    """
+    Extracts the Cartesian poses of a specified model frame given a list of joint configurations.
+
+    Parameters
+    ----------
+        model : `pinocchio.Model`
+            The model from which to perform forward kinematics.
+        target_frame : str
+            The name of the target frame.
+        q_vec : array-like
+            A list of joint configuration values describing the path.
+
+    Returns
+    -------
+        list[`pinocchio.SE3`]
+            A list of transforms describing the Cartesian poses of the specified frame at the provided joint configurations.
+    """
+    data = model.createData()
+    target_frame_id = model.getFrameId(target_frame)
+    tforms = []
+    for q in q_vec:
+        pinocchio.framesForwardKinematics(model, data, q)
+        tforms.append(copy.deepcopy(data.oMf[target_frame_id]))
+    return tforms

src/pyroboplan/planning/utils.py

@@ -0,0 +1,30 @@
+""" Utilities for motion planning. """
+
+import numpy as np
+
+
+def discretize_joint_space_path(q_start, q_end, max_angle_distance):
+    """
+    Discretizes a joint space path from `q_start` to `q_end` given a maximum angle distance between samples.
+
+    This is used primarily for producing paths for collision checking.
+
+    Parameters
+    ----------
+        q_start : array-like
+            The starting joint configuration.
+        q_end : array-like
+            The final joint configuration.
+        max_angle_distance : float
+            The maximum angular displacement, in radians, between samples.
+
+    Returns
+    -------
+        list[array-like]
+            A list of joint configuration arrays between the start and end points, inclusive.
+    """
+    q_diff = q_end - q_start
+    max_delta_q = max(abs(q_diff))
+    num_steps = int(np.ceil(max_delta_q / max_angle_distance)) + 1
+    step_vec = np.linspace(0.0, 1.0, num_steps)
+    return [q_start + step * q_diff for step in step_vec]

src/pyroboplan/visualization/meshcat_utils.py

@@ -46,3 +46,30 @@ def visualize_frame(visualizer, name, tform, line_length=0.2, line_width=3):
         )
     )
     visualizer.viewer[name].set_transform(tform.homogeneous)
+
+
+def visualize_frames(visualizer, prefix_name, tforms, line_length=0.2, line_width=3):
+    """
+    Visualizes a set of coordinate frames as axis triads at specified poses.
+
+    Parameters
+    ----------
+        visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`
+            The visualizer instance.
+        prefix_name : str
+            The name of the MeshCat component to add.
+        tforms : list[`pinocchio.SE3`]
+            A list of transforms at which to display frames.
+        line_length : float, optional
+            The length of the axes in the triad.
+        line_width : float, optional
+            The width of the axes in the triad.
+    """
+    for idx, tform in enumerate(tforms):
+        visualize_frame(
+            visualizer,
+            f"{prefix_name}/frame{idx}",
+            tform,
+            line_length=line_length,
+            line_width=line_width,
+        )