Add path shortening utils and consolidate examples (#33)

Author: eholum

.docker/ros/Dockerfile

@@ -28,7 +28,7 @@ RUN source /opt/ros/${ROS_DISTRO}/setup.bash \
 # As/if this package matures, a possible solution is to apply patches for ROS release.
 WORKDIR /workspace/roboplan_ws/src/roboplan/bindings
 RUN source /opt/ros/${ROS_DISTRO}/setup.bash \
-    && pip3 install "numpy<2.0.0" typing_extensions==4.10.0 viser opencv-contrib-python-headless \
+    && pip3 install "numpy<2.0.0" typing_extensions==4.10.0 viser opencv-contrib-python-headless tyro \
     && source /workspace/roboplan_ws/install/setup.bash \
     && pip3 install --no-build-isolation -ve .
 

.docker/ubuntu/Dockerfile

@@ -10,7 +10,7 @@ SHELL ["/bin/bash", "-c"]
 RUN apt-get update -y \
     && apt install -y \
         cmake libeigen3-dev libgmock-dev liburdfdom-dev python3-pip
-RUN pip3 install nanobind pytest scikit-build-core
+RUN pip3 install nanobind pytest scikit-build-core tyro
 
 # Install pinocchio with pip as it supports additional CPU architectures.
 # Also install additional dependencies for examples.

bindings/examples/common.py

@@ -0,0 +1,29 @@
+from pathlib import Path
+from roboplan import get_package_share_dir
+
+
+ROBOPLAN_EXAMPLES_DIR = Path(get_package_share_dir())
+
+# Dictionary of supported example models and their relevant parameters.
+# Entries are a list of:
+#     - The URDF path.
+#     - The SRDF path.
+#     - The end-effector name.
+#     - The robot's base link.
+#     - The starting "pose" of the robot.
+MODELS = {
+    "ur5": [
+        ROBOPLAN_EXAMPLES_DIR / "ur_robot_model" / "ur5_gripper.urdf",
+        ROBOPLAN_EXAMPLES_DIR / "ur_robot_model" / "ur5_gripper.srdf",
+        "tool0",
+        "base",
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    ],
+    "franka": [
+        ROBOPLAN_EXAMPLES_DIR / "franka_robot_model" / "fr3.urdf",
+        ROBOPLAN_EXAMPLES_DIR / "franka_robot_model" / "fr3.srdf",
+        "fr3_hand",
+        "fr3_link0",
+        [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    ],
+}

bindings/examples/example_ik_franka.py bindings/examples/example_ik.pybindings/examples/example_ik_franka.py renamed to bindings/examples/example_ik.py

@@ -1,11 +1,12 @@
-from pathlib import Path
+import sys
 import time
+import tyro
 
 import numpy as np
 import pinocchio as pin
 
+from common import MODELS, ROBOPLAN_EXAMPLES_DIR
 from roboplan import (
-    get_package_share_dir,
     Scene,
     JointConfiguration,
     CartesianConfiguration,
@@ -15,12 +16,31 @@
 from roboplan.viser_visualizer import ViserVisualizer
 
 
-if __name__ == "__main__":
+def main(
+    model: str = "ur5",
+    max_iters: int = 100,
+    step_size: float = 0.25,
+    host: str = "localhost",
+    port: str = "8000",
+):
+    """
+    Run the IK example with the provided parameters.
+
+
+    Parameters:
+        model: The name of the model to user (ur5 or franka).
+        max_iters: Maximum number of iterations for the IK solver.
+        step_size: Integration step size for the IK solver.
+        host: The host for the ViserVisualizer.
+        port: The port for the ViserVisualizer.
+    """
+
+    if model not in MODELS:
+        print(f"Invalid model requested: {model}")
+        sys.exit(1)
 
-    roboplan_examples_dir = Path(get_package_share_dir())
-    urdf_path = roboplan_examples_dir / "franka_robot_model" / "fr3.urdf"
-    srdf_path = roboplan_examples_dir / "franka_robot_model" / "fr3.srdf"
-    package_paths = [roboplan_examples_dir]
+    urdf_path, srdf_path, ee_name, base_link, start_pose = MODELS[model]
+    package_paths = [ROBOPLAN_EXAMPLES_DIR]
 
     scene = Scene("test_scene", urdf_path, srdf_path, package_paths)
 
@@ -30,18 +50,18 @@
         urdf_path, package_dirs=package_paths
     )
     viz = ViserVisualizer(model, collision_model, visual_model)
-    viz.initViewer(open=True, loadModel=True)
+    viz.initViewer(open=True, loadModel=True, host=host, port=port)
 
     # Set up an IK solver
     options = SimpleIkOptions()
-    options.max_iters = 100
-    options.step_size = 0.25
+    options.max_iters = max_iters
+    options.step_size = step_size
     ik_solver = SimpleIk(scene, options)
 
     start = JointConfiguration()
     goal = CartesianConfiguration()
-    goal.base_frame = "fr3_link0"
-    goal.tip_frame = "fr3_hand"
+    goal.base_frame = base_link
+    goal.tip_frame = ee_name
     solution = JointConfiguration()
 
     # Create an interactive marker.
@@ -58,7 +78,7 @@ def solve_ik(_):
         goal.tform = pin.SE3(
             pin.Quaternion(controls.wxyz[[1, 2, 3, 0]]), controls.position
         ).homogeneous
-        start.positions = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        start.positions = np.array(start_pose)
         result = ik_solver.solveIk(goal, start, solution)
         if result:
             viz.display(solution.positions)
@@ -83,5 +103,12 @@ def randomize_position(_):
 
     # Display the arm and marker at the starting position, then sleep forever.
     randomize_position(None)
-    while True:
-        time.sleep(10.0)
+    try:
+        while True:
+            time.sleep(10.0)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    tyro.cli(main)

bindings/examples/example_ik_ur.py

@@ -0,0 +1,113 @@
+import sys
+import time
+import tyro
+
+import pinocchio as pin
+from common import MODELS, ROBOPLAN_EXAMPLES_DIR
+from roboplan import (
+    shortcutPath,
+    JointConfiguration,
+    Scene,
+    RRTOptions,
+    RRT,
+)
+from roboplan.viser_visualizer import ViserVisualizer, visualizePath, visualizeTree
+
+
+def main(
+    model: str = "ur5",
+    max_connection_distance: float = 1.0,
+    collision_check_step_size: float = 0.05,
+    goal_biasing_probability: float = 0.15,
+    max_nodes: int = 1000,
+    max_planning_time: float = 3.0,
+    rrt_connect: bool = False,
+    include_shortcutting: bool = False,
+    host: str = "localhost",
+    port: str = "8000",
+):
+    """
+    Run the RRT example with the provided parameters.
+
+
+    Parameters:
+        model: The name of the model to user (ur5 or franka).
+        max_connection_distance: Maximum connection distance between two search nodes.
+        collision_check_step_size: Configuration-space step size for collision checking along edges.
+        goal_biasing_probability: Weighting of the goal node during random sampling.
+        max_nodes: The maximum number of nodes to add to the search tree.
+        max_planning_time: The maximum time (in seconds) to search for a path.
+        rrt_connect: Whether or not to use RRT-Connect.
+        include_shortcutting: Whether or not to include path shortcutting for found paths.
+        host: The host for the ViserVisualizer.
+        port: The port for the ViserVisualizer.
+    """
+
+    if model not in MODELS:
+        print(f"Invalid model requested: {model}")
+        sys.exit(1)
+
+    urdf_path, srdf_path, ee_name, _, _ = MODELS[model]
+    package_paths = [ROBOPLAN_EXAMPLES_DIR]
+
+    scene = Scene("test_scene", urdf_path, srdf_path, package_paths)
+
+    # Create a redundant Pinocchio model just for visualization.
+    # When Pinocchio 4.x releases nanobind bindings, we should be able to directly grab the model from the scene instead.
+    model, collision_model, visual_model = pin.buildModelsFromUrdf(
+        urdf_path, package_dirs=package_paths
+    )
+    viz = ViserVisualizer(model, collision_model, visual_model)
+    viz.initViewer(open=True, loadModel=True, host=host, port=port)
+
+    # Optionally include path shortening
+    include_shortcutting = True
+
+    # Set up an RRT and perform path planning.
+    options = RRTOptions()
+    options.max_connection_distance = max_connection_distance
+    options.collision_check_step_size = collision_check_step_size
+    options.goal_biasing_probability = goal_biasing_probability
+    options.max_nodes = max_nodes
+    options.max_planning_time = max_planning_time
+    options.rrt_connect = rrt_connect
+    rrt = RRT(scene, options)
+
+    start = JointConfiguration()
+    start.positions = scene.randomCollisionFreePositions()
+    assert start.positions is not None
+
+    goal = JointConfiguration()
+    goal.positions = scene.randomCollisionFreePositions()
+    assert goal.positions is not None
+
+    path = rrt.plan(start, goal)
+    assert path is not None
+
+    if include_shortcutting:
+        shortcut_path = shortcutPath(
+            scene, path, options.collision_check_step_size, 1000
+        )
+
+    # Visualize the tree and path
+    print(path)
+    viz.display(start.positions)
+    visualizePath(viz, scene, path, ee_name, 0.05)
+    visualizeTree(viz, scene, rrt, ee_name, 0.05)
+
+    if include_shortcutting:
+        print("Shortcutted path:")
+        print(shortcut_path)
+        visualizePath(
+            viz, scene, shortcut_path, ee_name, 0.05, (0, 100, 0), "/rrt/shortcut_path"
+        )
+
+    try:
+        while True:
+            time.sleep(10.0)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    tyro.cli(main)