From 2eeefa0d7c95d698d23151a309a26342e48c1f22 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 2 Apr 2024 16:15:27 -0400 Subject: [PATCH] Add example for collision checking along a path --- examples/example_collision_along_path.py | 130 ++++++++++++++++++ examples/old/test_pinocchio_ur5.py | 6 +- src/pyroboplan/core/utils.py | 28 ++++ src/pyroboplan/planning/utils.py | 30 ++++ src/pyroboplan/visualization/meshcat_utils.py | 27 ++++ 5 files changed, 218 insertions(+), 3 deletions(-) create mode 100644 examples/example_collision_along_path.py create mode 100644 src/pyroboplan/planning/utils.py diff --git a/examples/example_collision_along_path.py b/examples/example_collision_along_path.py new file mode 100644 index 0000000..a032715 --- /dev/null +++ b/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) diff --git a/examples/old/test_pinocchio_ur5.py b/examples/old/test_pinocchio_ur5.py index fa42748..70db71a 100644 --- a/examples/old/test_pinocchio_ur5.py +++ b/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( diff --git a/src/pyroboplan/core/utils.py b/src/pyroboplan/core/utils.py index 8588408..b128ce0 100644 --- a/src/pyroboplan/core/utils.py +++ b/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 diff --git a/src/pyroboplan/planning/utils.py b/src/pyroboplan/planning/utils.py new file mode 100644 index 0000000..1bdc738 --- /dev/null +++ b/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] diff --git a/src/pyroboplan/visualization/meshcat_utils.py b/src/pyroboplan/visualization/meshcat_utils.py index 013dbf1..d2cfbba 100644 --- a/src/pyroboplan/visualization/meshcat_utils.py +++ b/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, + )