Merge branch 'main' into tl-expected-wrapping

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.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 / "ur_robot_model" / "ur5_gripper.urdf"
-    srdf_path = roboplan_examples_dir / "ur_robot_model" / "ur5_gripper.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 = "base"
-    goal.tip_frame = "tool0"
+    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])
+        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_rrt.py

@@ -1,93 +1,54 @@
-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 (
-    computeFramePath,
-    get_package_share_dir,
+    shortcutPath,
     JointConfiguration,
-    JointPath,
     Scene,
     RRTOptions,
     RRT,
 )
-from roboplan.viser_visualizer import ViserVisualizer
-
-
-def visualizePath(
-    viz: ViserVisualizer,
-    scene: Scene,
-    rrt: RRT,
-    path: JointPath | None,
-    frame_name: str,
-    max_step_size: float,
-) -> None:
+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",
+):
     """
-    Helper function to visualize the RRT path.
-    TODO: Move this to the actual Python package itself.
+    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.
     """
-    start_nodes, goal_nodes = rrt.getNodes()
-
-    start_segments = []
-    for node in start_nodes[1:]:
-        q_start = start_nodes[node.parent_id].config
-        q_end = node.config
-        frame_path = computeFramePath(scene, q_start, q_end, frame_name, max_step_size)
-        for idx in range(len(frame_path) - 1):
-            start_segments.append([frame_path[idx][:3, 3], frame_path[idx + 1][:3, 3]])
-
-    goal_segments = []
-    for node in goal_nodes[1:]:
-        q_start = goal_nodes[node.parent_id].config
-        q_end = node.config
-        frame_path = computeFramePath(scene, q_start, q_end, frame_name, max_step_size)
-        for idx in range(len(frame_path) - 1):
-            goal_segments.append([frame_path[idx][:3, 3], frame_path[idx + 1][:3, 3]])
-
-    path_segments = []
-    if path is not None:
-        for idx in range(len(path.positions) - 1):
-            q_start = path.positions[idx]
-            q_end = path.positions[idx + 1]
-            frame_path = computeFramePath(
-                scene, q_start, q_end, frame_name, max_step_size
-            )
-            for idx in range(len(frame_path) - 1):
-                path_segments.append(
-                    [frame_path[idx][:3, 3], frame_path[idx + 1][:3, 3]]
-                )
-
-    if start_segments:
-        viz.viewer.scene.add_line_segments(
-            "/rrt/start_tree",
-            points=np.array(start_segments),
-            colors=(0, 100, 100),
-            line_width=1.0,
-        )
-    if goal_segments:
-        viz.viewer.scene.add_line_segments(
-            "/rrt/goal_tree",
-            points=np.array(goal_segments),
-            colors=(100, 0, 100),
-            line_width=1.0,
-        )
-
-    if path_segments:
-        viz.viewer.scene.add_line_segments(
-            "/rrt/path",
-            points=np.array(path_segments),
-            colors=(100, 100, 0),
-            line_width=3.0,
-        )
-
 
-if __name__ == "__main__":
+    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 / "ur_robot_model" / "ur5_gripper.urdf"
-    srdf_path = roboplan_examples_dir / "ur_robot_model" / "ur5_gripper.srdf"
-    package_paths = [roboplan_examples_dir]
+    urdf_path, srdf_path, ee_name, _, _ = MODELS[model]
+    package_paths = [ROBOPLAN_EXAMPLES_DIR]
 
     scene = Scene("test_scene", urdf_path, srdf_path, package_paths)
 
@@ -97,14 +58,19 @@ def visualizePath(
         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)
+
+    # Optionally include path shortening
+    include_shortcutting = True
 
     # Set up an RRT and perform path planning.
     options = RRTOptions()
-    options.max_connection_distance = 1.0
-    options.collision_check_step_size = 0.05
-    options.max_planning_time = 3.0
-    options.rrt_connect = True
+    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()
@@ -118,14 +84,30 @@ def visualizePath(
     path = rrt.plan(start, goal)
     assert path is not None
 
-    # WHOOPSIE DAISEY
-    goal.positions[0] = -6
-    path = rrt.plan_expected(start, goal)
+    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, rrt, path, "tool0", 0.05)
+    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"
+        )
 
-    while True:
-        time.sleep(10.0)
+    try:
+        while True:
+            time.sleep(10.0)
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    tyro.cli(main)

bindings/src/roboplan/__init__.py

@@ -1,6 +1,10 @@
 from .roboplan.core import (
     computeFramePath,
     hasCollisionsAlongPath,
+    shortcutPath,
+    getPathLengths,
+    getNormalizedPathScaling,
+    getConfigurationFromNormalizedPathScaling,
     CartesianConfiguration,
     JointConfiguration,
     JointInfo,