From 1db4b1769027bd272c852aab1477163b81749ca9 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 24 Apr 2026 08:37:58 +0200 Subject: [PATCH 1/8] [tutorial_7] Open gripper before approach; rewrite instructions Align structure with tutorial_6's README pattern and add a 'Sync the initial gripper state' section: prepend open_gripper to segments[0] pre-actions so the fingers are guaranteed open before the arm descends onto the box. --- tutorial_7/README.md | 162 +++++++++++++++++++++++++++---------------- 1 file changed, 101 insertions(+), 61 deletions(-) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index c86ba39..c86325a 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -6,15 +6,27 @@ Having completed [tutorial 6](../tutorial_6/README.md). ## Overview -This tutorial extends tutorial 6 to a full pick-and-place scenario. It loads the -FR3 robot with a gripper, a box, and a ground plane, builds a manipulation -constraint graph, solves the pick-and-place problem, and executes on Gazebo with -gripper open/close actions between trajectory segments. +Tutorial 6 executed a single arm trajectory. Pick-and-place adds a second +actuator — the gripper — whose state must change **in the middle** of the +path: open on approach, closed during transport, open again after release. + +This tutorial builds an HPP manipulation constraint graph, solves a +pick-and-place problem, splits the resulting path into **segments** delimited +by grasp/release events detected from the graph, and executes the segments on +Gazebo with gripper actions in between. ## Setting up the simulation -Use the same Docker image as tutorial 6 (`hpp-tutorial-ros2`). If you haven't -built it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). +Use the same Docker image as tutorial 6 (`hpp-ros2:tuto`). If you haven't built +it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). + +Inside the container, build the packages (first time only): + +``` +cd ~/devel/src +make hpp-exec.install +make hpp_tutorial.install +``` ## Terminal 1: Launching the simulation @@ -26,9 +38,9 @@ ros2 launch hpp_tutorial tutorial_7_launch.py Wait until you see `Configured and activated gripper_controller` in the output. -Note: One gripper finger may appear loose in Gazebo. This is a simulation artifact -with mimic joints (finger2 copies finger1 via physics constraints). It does not -affect the real robot. +Note: one gripper finger may appear loose in Gazebo. This is a simulation +artefact with mimic joints (finger2 copies finger1 via physics constraints). +It does not affect the real robot. ## Terminal 2: Planning and executing @@ -38,103 +50,124 @@ Open a second terminal: docker exec -it hpp bash ``` -Source the environments and run the tutorial script: +Run the tutorial script: ``` cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` -The script loads the FR3 robot with a box on a ground plane, builds a -manipulation constraint graph, and solves a pick-and-place problem (moving the -box from one position to another). The result is a list of `configs` and `times` -with grasp/release transitions embedded in the path. +The script loads the FR3 with a box on a ground plane, builds a manipulation +constraint graph, and solves a pick-and-place problem (move the box from +`(0.4, -0.2)` to `(0.4, 0.2)`). At the end you get two lists, `configs` and +`times`, describing the full planned motion — including the waypoints where a +grasp is acquired or released. -You can visualize the path in the viewer: +You can visualize the path in the browser viewer: ```python v = display() v.loadPath(p_timed) ``` -## Understanding segments and actions +## Understanding the phases + +HPP returns a single continuous path, but the robot cannot execute it as one +trajectory: the gripper has to close somewhere in the middle and open again +later. We need to break the path at those events. + +For this pick-and-place there are exactly two such events — one pick, one +place — which split the path into three phases: -The key difference with tutorial 6 is that the trajectory has multiple phases: -approach, grasp, transport, and release. The `hpp_exec` package detects these -transitions from the constraint graph and splits the trajectory into **segments** -with gripper actions between them. +| # | Phase | What the arm does | Gripper action before | +|---|------------|-----------------------------|-----------------------| +| 0 | approach | move above the box, descend | open (see below) | +| 1 | transport | carry the box to the goal | close | +| 2 | retreat | lift and return | open | -### How it works +## Detecting grasp/release events -HPP's constraint graph encodes the grasp state at each configuration. The -`segments_from_graph()` function queries the graph for each waypoint to detect -when a grasp is acquired or released: +HPP's constraint graph already knows where those events are. Each configuration +belongs to a graph **state** whose name lists the active grasps (e.g. `"free"` +or `"fr3/gripper grasps box/handle"`). `hpp_exec` walks the configs, calls +`graph.getStateFromConfiguration(q)` at each one, and flags every index where +the active-grasp set changes: ```python from hpp_exec import extract_grasp_transitions -transitions = extract_grasp_transitions(configs, times, graph) -for t in transitions: - print(f"Config {t.config_index} at t={t.time:.2f}s:") - print(f" Acquired: {t.acquired}") # New grasps (close gripper) - print(f" Released: {t.released}") # Lost grasps (open gripper) +for t in extract_grasp_transitions(configs, times, graph): + print(f"t={t.time:.2f}s acquired={t.acquired} released={t.released}") ``` -### Segments and pre_actions +## Building segments from the constraint graph -A `Segment` represents a slice of the trajectory with optional actions to run -before or after sending that segment: +`segments_from_graph()` uses those transitions to build one `Segment` per +phase, wiring `on_grasp` and `on_release` as *pre-actions* on the appropriate +segments: ```python -from hpp_exec import Segment - -# Example: 3 segments for approach → grasp → retreat -segments = [ - Segment(0, 50), # Approach (no action) - Segment(50, 150, pre_actions=[close_gripper]), # Transport (close first) - Segment(150, 200, pre_actions=[open_gripper]), # Retreat (open first) -] +from hpp_exec import segments_from_graph + +segments = segments_from_graph( + configs, times, graph, + on_grasp=close_gripper, + on_release=open_gripper, +) ``` -The `execute_segments()` function iterates through segments: -1. Run all `pre_actions` (stop if any returns False) -2. Send the arm trajectory for this segment -3. Run all `post_actions` (stop if any returns False) +For this problem you get three segments matching the table above: segment 0 +has no pre-action, segment 1 closes the gripper first, segment 2 opens it. + +## Sync the initial gripper state + +`segments_from_graph()` only sets pre-actions on segments *after* a transition, +so segment 0 has none. But segment 0 is precisely where the arm descends +toward the box, and HPP planned that descent assuming the fingers are already +open (see `q_init` in `init.py`). If the real gripper is closed — for instance +because the previous run ended mid-transport, or Gazebo spawned with fingers +at some other pose — the arm descends into the box. + +The fix is one line: prepend the open action to segment 0's pre-actions. Since +`send_trajectory` blocks until the gripper controller reports completion, the +arm will not move until the fingers are actually open: + +```python +segments[0].pre_actions.insert(0, open_gripper) +``` ## Executing with gripper actions -Define gripper actions as functions returning `True` on success: +Gripper actions are just callables returning `True` on success — here we send +a small two-point trajectory to the gripper controller. Only +`fr3_finger_joint1` is commanded; `fr3_finger_joint2` follows as a mimic joint. ```python from hpp_exec import segments_from_graph, execute_segments, send_trajectory import numpy as np -# Only fr3_finger_joint1 is controllable (finger2 is a mimic joint) -def close_gripper(): - send_trajectory( - [np.array([0.035]), np.array([0.0])], +def open_gripper(): + return send_trajectory( + [np.array([0.0]), np.array([0.035])], [0.0, 0.5], joint_names=['fr3_finger_joint1'], controller_topic='/gripper_controller/follow_joint_trajectory', ) - return True -def open_gripper(): - send_trajectory( - [np.array([0.0]), np.array([0.035])], +def close_gripper(): + return send_trajectory( + [np.array([0.035]), np.array([0.0])], [0.0, 0.5], joint_names=['fr3_finger_joint1'], controller_topic='/gripper_controller/follow_joint_trajectory', ) - return True -# Build segments from the constraint graph (auto-detects grasp/release) segments = segments_from_graph( configs, times, graph, on_grasp=close_gripper, on_release=open_gripper, ) +segments[0].pre_actions.insert(0, open_gripper) -# Execute all segments with gripper actions execute_segments( segments, configs, times, joint_names=[f'fr3_joint{i}' for i in range(1, 8)], @@ -142,13 +175,20 @@ execute_segments( ) ``` -You should see the robot approach the box, close its gripper, move the box to -the goal position, open the gripper, and retreat. +`execute_segments()` iterates over the segments: for each one it runs every +`pre_action` (aborting on the first `False`), sends the arm trajectory for +`configs[start:end]` and waits for it to complete, then runs every +`post_action`. + +Expected behaviour in Gazebo: the fingers open, the arm descends, the fingers +close on the box, the arm carries it to the goal, the fingers open, the arm +retreats. ## Reference: Franka native gripper (real robot) -On a real Franka robot, you can use the native gripper actions which provide -force-controlled grasping via `franka_msgs`: +On a real Franka robot you can use the native gripper actions, which provide +force-controlled grasping via `franka_msgs`. Drop-in replacement: swap the +`open_gripper`/`close_gripper` callables for `gripper.open`/`gripper.close`. ```python import rclpy @@ -207,13 +247,13 @@ class FrankaGripper: def destroy(self): self._node.destroy_node() -# Usage with segments gripper = FrankaGripper("fr3") segments = segments_from_graph( configs, times, graph, on_grasp=gripper.close, on_release=gripper.open, ) +segments[0].pre_actions.insert(0, gripper.open) # sync initial state execute_segments(segments, configs, times, ...) gripper.destroy() ``` From f33c3246a0a17b3249e2109346ee6a6889563357 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 27 Apr 2026 09:34:11 +0200 Subject: [PATCH 2/8] [pre-commit] Tell ruff isort that launch/launch_ros are third-party --- pyproject.toml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pyproject.toml b/pyproject.toml index e14e5b7..2c41eb6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,5 +4,8 @@ extend-exclude = ["cmake"] [tool.ruff.lint] extend-select = ["I", "NPY", "RUF", "UP", "W"] +[tool.ruff.lint.isort] +known-third-party = ["launch", "launch_ros"] + [tool.tomlsort] all = true From 5228c19ca541cdde25f3a4ee078d059caa914aac Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 30 Apr 2026 09:37:04 +0200 Subject: [PATCH 3/8] Rewrite tutorial_7 --- tutorial_7/README.md | 228 +++++++++++-------------------------------- tutorial_7/init.py | 73 +++++++++++++- 2 files changed, 130 insertions(+), 171 deletions(-) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index c86325a..e946fb0 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -6,19 +6,21 @@ Having completed [tutorial 6](../tutorial_6/README.md). ## Overview -Tutorial 6 executed a single arm trajectory. Pick-and-place adds a second -actuator — the gripper — whose state must change **in the middle** of the -path: open on approach, closed during transport, open again after release. +Tutorial 6 executed one arm trajectory with `send_trajectory`. This tutorial +adds the gripper: the robot must open the fingers before approaching the box, +close them before transport, then open them again at the goal. -This tutorial builds an HPP manipulation constraint graph, solves a -pick-and-place problem, splits the resulting path into **segments** delimited -by grasp/release events detected from the graph, and executes the segments on -Gazebo with gripper actions in between. +The script plans a pick-and-place path with an HPP manipulation constraint +graph. We then use `hpp_exec` to split the path at grasp and release +transitions and execute each segment on Gazebo. + +For the full `hpp_exec` API, see the +[hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html). ## Setting up the simulation -Use the same Docker image as tutorial 6 (`hpp-ros2:tuto`). If you haven't built -it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). +Use the same Docker image as tutorial 6 (`hpp-ros2:tuto`). If you have not +built it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). Inside the container, build the packages (first time only): @@ -30,7 +32,7 @@ make hpp_tutorial.install ## Terminal 1: Launching the simulation -Launch Gazebo with the FR3 including its gripper: +Launch Gazebo with the FR3 and its gripper: ``` ros2 launch hpp_tutorial tutorial_7_launch.py @@ -39,10 +41,9 @@ ros2 launch hpp_tutorial tutorial_7_launch.py Wait until you see `Configured and activated gripper_controller` in the output. Note: one gripper finger may appear loose in Gazebo. This is a simulation -artefact with mimic joints (finger2 copies finger1 via physics constraints). -It does not affect the real robot. +artefact with mimic joints. It does not affect the tutorial. -## Terminal 2: Planning and executing +## Terminal 2: Planning Open a second terminal: @@ -57,53 +58,28 @@ cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` -The script loads the FR3 with a box on a ground plane, builds a manipulation -constraint graph, and solves a pick-and-place problem (move the box from -`(0.4, -0.2)` to `(0.4, 0.2)`). At the end you get two lists, `configs` and -`times`, describing the full planned motion — including the waypoints where a -grasp is acquired or released. +The script loads the FR3, the ground, and a box. It solves a pick-and-place +problem that moves the box from `(0.4, -0.2)` to `(0.4, 0.2)`, optimizes the +path, and time-parameterizes it with `SimpleTimeParameterization`. + +You can visualize the planned path in the browser viewer: -You can visualize the path in the browser viewer: ```python v = display() v.loadPath(p_timed) ``` -## Understanding the phases - -HPP returns a single continuous path, but the robot cannot execute it as one -trajectory: the gripper has to close somewhere in the middle and open again -later. We need to break the path at those events. - -For this pick-and-place there are exactly two such events — one pick, one -place — which split the path into three phases: +At this point the useful objects are: -| # | Phase | What the arm does | Gripper action before | -|---|------------|-----------------------------|-----------------------| -| 0 | approach | move above the box, descend | open (see below) | -| 1 | transport | carry the box to the goal | close | -| 2 | retreat | lift and return | open | +- `configs`: sampled HPP configurations along the timed path. +- `times`: timestamps in seconds. +- `graph`: the HPP manipulation constraint graph. +- `open_gripper` and `close_gripper`: small Gazebo gripper actions. -## Detecting grasp/release events +## Building execution segments -HPP's constraint graph already knows where those events are. Each configuration -belongs to a graph **state** whose name lists the active grasps (e.g. `"free"` -or `"fr3/gripper grasps box/handle"`). `hpp_exec` walks the configs, calls -`graph.getStateFromConfiguration(q)` at each one, and flags every index where -the active-grasp set changes: - -```python -from hpp_exec import extract_grasp_transitions - -for t in extract_grasp_transitions(configs, times, graph): - print(f"t={t.time:.2f}s acquired={t.acquired} released={t.released}") -``` - -## Building segments from the constraint graph - -`segments_from_graph()` uses those transitions to build one `Segment` per -phase, wiring `on_grasp` and `on_release` as *pre-actions* on the appropriate -segments: +The planned path contains the approach, transport, and retreat motion in one +path. To execute it, split it where the manipulation graph changes state: ```python from hpp_exec import segments_from_graph @@ -115,145 +91,57 @@ segments = segments_from_graph( ) ``` -For this problem you get three segments matching the table above: segment 0 -has no pre-action, segment 1 closes the gripper first, segment 2 opens it. +For this problem, `segments` contains three phases: -## Sync the initial gripper state +| # | Phase | What the arm does | Pre-action | +|---|-----------|-----------------------------|------------| +| 0 | approach | move above the box, descend | none | +| 1 | transport | carry the box to the goal | close | +| 2 | retreat | lift and return | open | -`segments_from_graph()` only sets pre-actions on segments *after* a transition, -so segment 0 has none. But segment 0 is precisely where the arm descends -toward the box, and HPP planned that descent assuming the fingers are already -open (see `q_init` in `init.py`). If the real gripper is closed — for instance -because the previous run ended mid-transport, or Gazebo spawned with fingers -at some other pose — the arm descends into the box. - -The fix is one line: prepend the open action to segment 0's pre-actions. Since -`send_trajectory` blocks until the gripper controller reports completion, the -arm will not move until the fingers are actually open: +Make the initial gripper state explicit before the approach: ```python segments[0].pre_actions.insert(0, open_gripper) ``` -## Executing with gripper actions +This matters if a previous run left the simulated gripper closed. -Gripper actions are just callables returning `True` on success — here we send -a small two-point trajectory to the gripper controller. Only -`fr3_finger_joint1` is commanded; `fr3_finger_joint2` follows as a mimic joint. +## Executing the segments -```python -from hpp_exec import segments_from_graph, execute_segments, send_trajectory -import numpy as np - -def open_gripper(): - return send_trajectory( - [np.array([0.0]), np.array([0.035])], - [0.0, 0.5], - joint_names=['fr3_finger_joint1'], - controller_topic='/gripper_controller/follow_joint_trajectory', - ) - -def close_gripper(): - return send_trajectory( - [np.array([0.035]), np.array([0.0])], - [0.0, 0.5], - joint_names=['fr3_finger_joint1'], - controller_topic='/gripper_controller/follow_joint_trajectory', - ) +Send the arm segments to the arm controller and let the pre-actions command +the gripper controller: -segments = segments_from_graph( - configs, times, graph, - on_grasp=close_gripper, - on_release=open_gripper, -) -segments[0].pre_actions.insert(0, open_gripper) +```python +from hpp_exec import execute_segments execute_segments( segments, configs, times, - joint_names=[f'fr3_joint{i}' for i in range(1, 8)], + joint_names=[f"fr3_joint{i}" for i in range(1, 8)], joint_indices=list(range(7)), ) ``` -`execute_segments()` iterates over the segments: for each one it runs every -`pre_action` (aborting on the first `False`), sends the arm trajectory for -`configs[start:end]` and waits for it to complete, then runs every -`post_action`. +You should see the fingers open, the arm descend, the fingers close on the +box, the arm carry the box to the goal, the fingers open, and the arm retreat. -Expected behaviour in Gazebo: the fingers open, the arm descends, the fingers -close on the box, the arm carries it to the goal, the fingers open, the arm -retreats. +`init.py` also defines this convenience wrapper: -## Reference: Franka native gripper (real robot) +```python +execute_on_gazebo() +``` + +## Experiment -On a real Franka robot you can use the native gripper actions, which provide -force-controlled grasping via `franka_msgs`. Drop-in replacement: swap the -`open_gripper`/`close_gripper` callables for `gripper.open`/`gripper.close`. +Inspect the segments before executing: ```python -import rclpy -from rclpy.node import Node -from rclpy.action import ActionClient -from franka_msgs.action import Grasp, Move - -class FrankaGripper: - def __init__(self, arm_id="fr3"): - self.arm_id = arm_id - if not rclpy.ok(): - rclpy.init() - self._node = Node("franka_gripper") - self._grasp_client = ActionClient( - self._node, Grasp, f"/{arm_id}_gripper/grasp" - ) - self._move_client = ActionClient( - self._node, Move, f"/{arm_id}_gripper/move" - ) - - def open(self) -> bool: - """Open gripper using Move action (position control).""" - if not self._move_client.wait_for_server(timeout_sec=10.0): - return False - goal = Move.Goal() - goal.width = 0.08 # Fully open (meters) - goal.speed = 0.05 # m/s - future = self._move_client.send_goal_async(goal) - rclpy.spin_until_future_complete(self._node, future, timeout_sec=10.0) - goal_handle = future.result() - if not goal_handle or not goal_handle.accepted: - return False - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(self._node, result_future, timeout_sec=30.0) - return True - - def close(self) -> bool: - """Close gripper using Grasp action (force-controlled).""" - if not self._grasp_client.wait_for_server(timeout_sec=10.0): - return False - goal = Grasp.Goal() - goal.width = 0.02 # Target width (meters) - goal.speed = 0.05 # Closing speed (m/s) - goal.force = 50.0 # Grasping force (Newtons) - goal.epsilon.inner = 0.01 # Tolerance for grasp success - goal.epsilon.outer = 0.01 - future = self._grasp_client.send_goal_async(goal) - rclpy.spin_until_future_complete(self._node, future, timeout_sec=10.0) - goal_handle = future.result() - if not goal_handle or not goal_handle.accepted: - return False - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(self._node, result_future, timeout_sec=30.0) - return True - - def destroy(self): - self._node.destroy_node() - -gripper = FrankaGripper("fr3") -segments = segments_from_graph( - configs, times, graph, - on_grasp=gripper.close, - on_release=gripper.open, -) -segments[0].pre_actions.insert(0, gripper.open) # sync initial state -execute_segments(segments, configs, times, ...) -gripper.destroy() +for i, s in enumerate(segments): + pre = [a.__name__ for a in s.pre_actions] + print(f"segment {i}: configs[{s.start_index}:{s.end_index}] pre={pre}") ``` + +Try running the execution twice without resetting the box pose. On the second +run, the planned start pose and the simulated world no longer match, so the +robot will descend where the box used to be. `hpp_exec` executes the path that +HPP planned, but it does not re-plan against the current world state. diff --git a/tutorial_7/init.py b/tutorial_7/init.py index 42b2379..fbc448c 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -6,6 +6,15 @@ from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp_viser import Viewer +try: + from hpp_exec import execute_segments, segments_from_graph, send_trajectory + + ROS_EXECUTION_AVAILABLE = True + ROS_EXECUTION_IMPORT_ERROR = None +except ImportError as exc: + ROS_EXECUTION_AVAILABLE = False + ROS_EXECUTION_IMPORT_ERROR = exc + def display(): v = Viewer(robot) @@ -15,6 +24,10 @@ def display(): return v +ARM_JOINT_NAMES = [f"fr3_joint{i}" for i in range(1, 8)] +GRIPPER_JOINT_NAMES = ["fr3_finger_joint1"] + + # Load FR3 robot with gripper robot = Device("tuto") @@ -178,4 +191,62 @@ def display(): print(" v = display()") print(" v.loadPath(p_timed)") print() -print("To execute on Gazebo, see README.md for gripper setup and execution code.") + + +if ROS_EXECUTION_AVAILABLE: + + def open_gripper(): + return send_trajectory( + [np.array([0.0]), np.array([0.035])], + [0.0, 0.5], + joint_names=GRIPPER_JOINT_NAMES, + controller_topic="/gripper_controller/follow_joint_trajectory", + ) + + def close_gripper(): + return send_trajectory( + [np.array([0.035]), np.array([0.0])], + [0.0, 0.5], + joint_names=GRIPPER_JOINT_NAMES, + controller_topic="/gripper_controller/follow_joint_trajectory", + ) + + segments = segments_from_graph( + configs, + times, + graph, + on_grasp=close_gripper, + on_release=open_gripper, + ) + if segments: + segments[0].pre_actions.insert(0, open_gripper) + + def execute_on_gazebo(): + return execute_segments( + segments, + configs, + times, + joint_names=ARM_JOINT_NAMES, + joint_indices=list(range(7)), + ) + + print("To execute on Gazebo:") + print(" execute_on_gazebo()") + print() + print("Useful helpers:") + print(" open_gripper()") + print(" close_gripper()") +else: + segments = None + + def _unavailable(*_args, **_kwargs): + raise RuntimeError( + "ROS2 execution helpers are unavailable in this Python environment." + ) from ROS_EXECUTION_IMPORT_ERROR + + open_gripper = _unavailable + close_gripper = _unavailable + execute_on_gazebo = _unavailable + + print("ROS2 execution helpers are unavailable in this Python env.") + print(f"Import error: {ROS_EXECUTION_IMPORT_ERROR}") From 514536ff56038f4e3fcd758e51fc2b09bd77c05a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 4 May 2026 09:01:24 +0200 Subject: [PATCH 4/8] [tutorial_7] Move execution details to hpp-exec docs --- launch/tutorial_7_launch.py | 65 ++++++++++++++- tutorial_7/README.md | 122 ++++++++++------------------ tutorial_7/init.py | 158 +++++++++++++++++++++++++++++++++--- 3 files changed, 252 insertions(+), 93 deletions(-) diff --git a/launch/tutorial_7_launch.py b/launch/tutorial_7_launch.py index cbd1453..c1ba86b 100644 --- a/launch/tutorial_7_launch.py +++ b/launch/tutorial_7_launch.py @@ -45,6 +45,23 @@ def get_robot_description_and_controllers(context: LaunchContext, robot_type): }, ).toxml() + detachable_joint_plugin = f""" + + + {robot_type_str}_hand + box + base_link + /box/attach + /box/detach + true + + + +""" + robot_description_xml = robot_description_xml.replace( + "", detachable_joint_plugin + ) + # Replace the Franka default controllers YAML with our tutorial controllers franka_controllers = os.path.join( get_package_share_directory("franka_gazebo_bringup"), @@ -88,6 +105,9 @@ def get_robot_description_and_controllers(context: LaunchContext, robot_type): def generate_launch_description(): robot_type = LaunchConfiguration("robot_type") + tutorial_share = get_package_share_directory("hpp_tutorial") + box_urdf = os.path.join(tutorial_share, "urdf", "box.urdf") + ground_urdf = os.path.join(tutorial_share, "urdf", "ground.urdf") # Gazebo Sim os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname( @@ -111,10 +131,51 @@ def generate_launch_description(): output="screen", ) + spawn_box = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "box", + "-file", + box_urdf, + "-x", + "0.4", + "-y", + "-0.2", + "-z", + "0.0251", + ], + output="screen", + ) + + spawn_ground = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "ground", + "-file", + ground_urdf, + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.0", + ], + output="screen", + ) + bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", + "/box/attach@std_msgs/msg/Empty]gz.msgs.Empty", + "/box/detach@std_msgs/msg/Empty]gz.msgs.Empty", + "/world/empty/set_pose@ros_gz_interfaces/srv/SetEntityPose", + ], output="screen", ) @@ -126,6 +187,8 @@ def generate_launch_description(): function=get_robot_description_and_controllers, args=[robot_type] ), spawn, + spawn_ground, + spawn_box, bridge, ] ) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index e946fb0..1fe3500 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -6,44 +6,36 @@ Having completed [tutorial 6](../tutorial_6/README.md). ## Overview -Tutorial 6 executed one arm trajectory with `send_trajectory`. This tutorial -adds the gripper: the robot must open the fingers before approaching the box, -close them before transport, then open them again at the goal. +Tutorial 6 executed a single arm trajectory. Pick-and-place adds a second +actuator - the gripper - whose state must change in the middle of the path: +open on approach, closed during transport, open again after release. -The script plans a pick-and-place path with an HPP manipulation constraint -graph. We then use `hpp_exec` to split the path at grasp and release -transitions and execute each segment on Gazebo. - -For the full `hpp_exec` API, see the -[hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html). +This tutorial builds an HPP manipulation constraint graph, solves a +pick-and-place problem, then executes it in Gazebo with the arm trajectory, +gripper motion, and box attachment coordinated automatically. ## Setting up the simulation Use the same Docker image as tutorial 6 (`hpp-ros2:tuto`). If you have not built it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). -Inside the container, build the packages (first time only): - -``` -cd ~/devel/src -make hpp-exec.install -make hpp_tutorial.install -``` - ## Terminal 1: Launching the simulation -Launch Gazebo with the FR3 and its gripper: +Launch Gazebo with the FR3 including its gripper: ``` ros2 launch hpp_tutorial tutorial_7_launch.py ``` Wait until you see `Configured and activated gripper_controller` in the output. +The launch file also spawns the ground plane and the box, then bridges the +Gazebo topics used to attach/detach the box. Note: one gripper finger may appear loose in Gazebo. This is a simulation -artefact with mimic joints. It does not affect the tutorial. +artefact with mimic joints (finger2 copies finger1 via physics constraints). +It does not affect the real robot. -## Terminal 2: Planning +## Terminal 2: Planning and executing Open a second terminal: @@ -58,90 +50,60 @@ cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` -The script loads the FR3, the ground, and a box. It solves a pick-and-place -problem that moves the box from `(0.4, -0.2)` to `(0.4, 0.2)`, optimizes the -path, and time-parameterizes it with `SimpleTimeParameterization`. +The script loads the FR3 with a box on a ground plane, builds a manipulation +constraint graph, and solves a pick-and-place problem (move the box from +`(0.4, -0.2)` to `(0.4, 0.2)`). At the end you get two lists, `configs` and +`times`, describing the full planned motion, including the waypoints where a +grasp is acquired or released. -You can visualize the planned path in the browser viewer: +You can visualize the path in the browser viewer: ```python v = display() v.loadPath(p_timed) ``` -At this point the useful objects are: - -- `configs`: sampled HPP configurations along the timed path. -- `times`: timestamps in seconds. -- `graph`: the HPP manipulation constraint graph. -- `open_gripper` and `close_gripper`: small Gazebo gripper actions. +## Executing -## Building execution segments - -The planned path contains the approach, transport, and retreat motion in one -path. To execute it, split it where the manipulation graph changes state: +Before each run, reset the simulated box and open the gripper: ```python -from hpp_exec import segments_from_graph - -segments = segments_from_graph( - configs, times, graph, - on_grasp=close_gripper, - on_release=open_gripper, -) +prepare_sim_for_run() ``` -For this problem, `segments` contains three phases: - -| # | Phase | What the arm does | Pre-action | -|---|-----------|-----------------------------|------------| -| 0 | approach | move above the box, descend | none | -| 1 | transport | carry the box to the goal | close | -| 2 | retreat | lift and return | open | - -Make the initial gripper state explicit before the approach: +Then execute the planned motion: ```python -segments[0].pre_actions.insert(0, open_gripper) +execute_on_gazebo() ``` -This matters if a previous run left the simulated gripper closed. +Expected behaviour in Gazebo: the gripper opens, descends onto the box, the +box is attached to the hand, the fingers close, the arm carries it to the goal, +the fingers open, the box is detached, and the arm retreats. -## Executing the segments +## Understanding the execution -Send the arm segments to the arm controller and let the pre-actions command -the gripper controller: +The script builds the execution segments for you. For this problem there are +three phases: -```python -from hpp_exec import execute_segments - -execute_segments( - segments, configs, times, - joint_names=[f"fr3_joint{i}" for i in range(1, 8)], - joint_indices=list(range(7)), -) -``` - -You should see the fingers open, the arm descend, the fingers close on the -box, the arm carry the box to the goal, the fingers open, and the arm retreat. - -`init.py` also defines this convenience wrapper: +1. approach the box with the gripper open, +2. grasp the box and transport it, +3. release the box and retreat. -```python -execute_on_gazebo() -``` +The details of how `hpp-exec` detects grasp/release transitions from the HPP +constraint graph, builds segments, and calls gripper actions are documented in +the [hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html#hpp_exec_grasps). ## Experiment -Inspect the segments before executing: +Run the same planned path twice: ```python -for i, s in enumerate(segments): - pre = [a.__name__ for a in s.pre_actions] - print(f"segment {i}: configs[{s.start_index}:{s.end_index}] pre={pre}") +prepare_sim_for_run() +execute_on_gazebo() +prepare_sim_for_run() +execute_on_gazebo() ``` -Try running the execution twice without resetting the box pose. On the second -run, the planned start pose and the simulated world no longer match, so the -robot will descend where the box used to be. `hpp_exec` executes the path that -HPP planned, but it does not re-plan against the current world state. +The second `prepare_sim_for_run()` call should put the box back at the initial +pose before replaying the motion. diff --git a/tutorial_7/init.py b/tutorial_7/init.py index fbc448c..00dbdcc 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -1,4 +1,5 @@ import numpy as np +import time from pinocchio import SE3 from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint from pyhpp.core import RandomShortcut, SimpleTimeParameterization @@ -7,6 +8,13 @@ from pyhpp_viser import Viewer try: + import rclpy + from geometry_msgs.msg import Pose + from rclpy.node import Node + from ros_gz_interfaces.msg import Entity + from ros_gz_interfaces.srv import SetEntityPose + from std_msgs.msg import Empty + from hpp_exec import execute_segments, segments_from_graph, send_trajectory ROS_EXECUTION_AVAILABLE = True @@ -26,6 +34,8 @@ def display(): ARM_JOINT_NAMES = [f"fr3_joint{i}" for i in range(1, 8)] GRIPPER_JOINT_NAMES = ["fr3_finger_joint1"] +BOX_INITIAL_POSITION = (0.4, -0.2, 0.0251) +BOX_GOAL_POSITION = (0.4, 0.2, 0.0251) # Load FR3 robot with gripper @@ -111,9 +121,7 @@ def display(): 0.785, # arm (ready) 0.035, 0.035, # fingers (open) - 0.4, - -0.2, - 0.0251, + *BOX_INITIAL_POSITION, 0, 0, 0, @@ -133,9 +141,7 @@ def display(): 0.785, 0.035, 0.035, - 0.4, - 0.2, - 0.0251, + *BOX_GOAL_POSITION, 0, 0, 0, @@ -193,8 +199,121 @@ def display(): print() +class _UnavailableGazeboExecution: + def __init__(self, import_error): + self._import_error = import_error + + def raise_unavailable(self): + raise RuntimeError( + "ROS2 / Gazebo execution helpers are unavailable in this Python " + "environment." + ) from self._import_error + + def attach(self): + self.raise_unavailable() + + def detach(self): + self.raise_unavailable() + + def reset_pose(self, *_args, **_kwargs): + self.raise_unavailable() + + if ROS_EXECUTION_AVAILABLE: + class GazeboBoxController: + def __init__( + self, + world_name="empty", + attach_topic="/box/attach", + detach_topic="/box/detach", + pose_service=None, + settle_time=0.2, + ): + self.attach_topic = attach_topic + self.detach_topic = detach_topic + self.pose_service = pose_service or f"/world/{world_name}/set_pose" + self.settle_time = settle_time + self._node = None + self._attach_pub = None + self._detach_pub = None + self._pose_client = None + self._attached = False + + def _ensure_node(self): + if self._node is not None: + return self._node + + if not rclpy.ok(): + rclpy.init() + + self._node = Node("tutorial_7_box_controller") + self._attach_pub = self._node.create_publisher(Empty, self.attach_topic, 1) + self._detach_pub = self._node.create_publisher(Empty, self.detach_topic, 1) + self._pose_client = self._node.create_client( + SetEntityPose, self.pose_service + ) + return self._node + + def _publish(self, publisher_attr, action_name): + node = self._ensure_node() + publisher = getattr(self, publisher_attr) + publisher.publish(Empty()) + rclpy.spin_once(node, timeout_sec=0.05) + time.sleep(self.settle_time) + node.get_logger().info(f"Published '{action_name}' on Gazebo topic") + return True + + def attach(self): + if self._attached: + return True + ok = self._publish("_attach_pub", self.attach_topic) + if ok: + self._attached = True + return ok + + def detach(self): + ok = self._publish("_detach_pub", self.detach_topic) + if ok: + self._attached = False + return ok + + def reset_pose(self, xyz=BOX_INITIAL_POSITION): + node = self._ensure_node() + + if not self.detach(): + return False + + if not self._pose_client.wait_for_service(timeout_sec=3.0): + node.get_logger().error( + f"Gazebo pose service '{self.pose_service}' is unavailable" + ) + return False + + req = SetEntityPose.Request() + req.entity.name = "box" + req.entity.type = Entity.MODEL + req.pose = Pose() + req.pose.position.x = float(xyz[0]) + req.pose.position.y = float(xyz[1]) + req.pose.position.z = float(xyz[2]) + req.pose.orientation.w = 1.0 + + future = self._pose_client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=3.0) + result = future.result() + if result is None or not result.success: + node.get_logger().error("Failed to reset the Gazebo box pose") + return False + + time.sleep(self.settle_time) + node.get_logger().info( + f"Reset box pose to x={xyz[0]:.3f}, y={xyz[1]:.3f}, z={xyz[2]:.3f}" + ) + return True + + box_controller = GazeboBoxController() + def open_gripper(): return send_trajectory( [np.array([0.0]), np.array([0.035])], @@ -211,12 +330,21 @@ def close_gripper(): controller_topic="/gripper_controller/follow_joint_trajectory", ) + def grasp_box(): + return box_controller.attach() and close_gripper() + + def release_box(): + return open_gripper() and box_controller.detach() + + def prepare_sim_for_run(): + return box_controller.reset_pose(BOX_INITIAL_POSITION) and open_gripper() + segments = segments_from_graph( configs, times, graph, - on_grasp=close_gripper, - on_release=open_gripper, + on_grasp=grasp_box, + on_release=release_box, ) if segments: segments[0].pre_actions.insert(0, open_gripper) @@ -231,22 +359,28 @@ def execute_on_gazebo(): ) print("To execute on Gazebo:") + print(" prepare_sim_for_run()") print(" execute_on_gazebo()") print() print("Useful helpers:") print(" open_gripper()") print(" close_gripper()") + print(" box_controller.attach()") + print(" box_controller.detach()") + print(" box_controller.reset_pose()") else: + box_controller = _UnavailableGazeboExecution(ROS_EXECUTION_IMPORT_ERROR) segments = None def _unavailable(*_args, **_kwargs): - raise RuntimeError( - "ROS2 execution helpers are unavailable in this Python environment." - ) from ROS_EXECUTION_IMPORT_ERROR + box_controller.raise_unavailable() open_gripper = _unavailable close_gripper = _unavailable + grasp_box = _unavailable + release_box = _unavailable + prepare_sim_for_run = _unavailable execute_on_gazebo = _unavailable - print("ROS2 execution helpers are unavailable in this Python env.") + print("ROS2 / Gazebo execution helpers are unavailable in this Python env.") print(f"Import error: {ROS_EXECUTION_IMPORT_ERROR}") From 72e83974054ee49f736d8fff7f50be79aeeedb41 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 4 May 2026 09:24:17 +0200 Subject: [PATCH 5/8] [pre-commit] Sort tutorial imports --- launch/tutorial_6_launch.py | 3 +-- launch/tutorial_7_launch.py | 3 +-- tutorial_7/init.py | 6 +++--- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/launch/tutorial_6_launch.py b/launch/tutorial_6_launch.py index 30e81f5..ad1adf6 100644 --- a/launch/tutorial_6_launch.py +++ b/launch/tutorial_6_launch.py @@ -8,6 +8,7 @@ import xacro from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -17,8 +18,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch import LaunchContext, LaunchDescription - def get_robot_description_and_controllers(context: LaunchContext, robot_type): robot_type_str = context.perform_substitution(robot_type) diff --git a/launch/tutorial_7_launch.py b/launch/tutorial_7_launch.py index c1ba86b..da60d40 100644 --- a/launch/tutorial_7_launch.py +++ b/launch/tutorial_7_launch.py @@ -8,6 +8,7 @@ import xacro from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -17,8 +18,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch import LaunchContext, LaunchDescription - def get_robot_description_and_controllers(context: LaunchContext, robot_type): robot_type_str = context.perform_substitution(robot_type) diff --git a/tutorial_7/init.py b/tutorial_7/init.py index 00dbdcc..cb3e229 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -1,5 +1,6 @@ -import numpy as np import time + +import numpy as np from pinocchio import SE3 from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint from pyhpp.core import RandomShortcut, SimpleTimeParameterization @@ -10,13 +11,12 @@ try: import rclpy from geometry_msgs.msg import Pose + from hpp_exec import execute_segments, segments_from_graph, send_trajectory from rclpy.node import Node from ros_gz_interfaces.msg import Entity from ros_gz_interfaces.srv import SetEntityPose from std_msgs.msg import Empty - from hpp_exec import execute_segments, segments_from_graph, send_trajectory - ROS_EXECUTION_AVAILABLE = True ROS_EXECUTION_IMPORT_ERROR = None except ImportError as exc: From 92ee6304152fbd458be2ee90e2c3afe4148b4fa8 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 11 May 2026 13:51:44 +0200 Subject: [PATCH 6/8] [tutorial_7] Use restored hpp-exec graph API --- tutorial_7/README.md | 106 +++++++++++++++++++++++++------------------ tutorial_7/init.py | 19 ++------ 2 files changed, 67 insertions(+), 58 deletions(-) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index 1fe3500..88955a7 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -6,13 +6,16 @@ Having completed [tutorial 6](../tutorial_6/README.md). ## Overview -Tutorial 6 executed a single arm trajectory. Pick-and-place adds a second -actuator - the gripper - whose state must change in the middle of the path: -open on approach, closed during transport, open again after release. +Tutorial 6 executed one arm trajectory with `send_trajectory`. This tutorial +adds the gripper: the robot must open the fingers before approaching the box, +close them before transport, then open them again at the goal. -This tutorial builds an HPP manipulation constraint graph, solves a -pick-and-place problem, then executes it in Gazebo with the arm trajectory, -gripper motion, and box attachment coordinated automatically. +The script plans a pick-and-place path with an HPP manipulation constraint +graph. We then use `hpp_exec` to split the path at grasp and release +transitions and execute each segment on Gazebo. + +For the full `hpp_exec` API, see the +[hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html). ## Setting up the simulation @@ -21,21 +24,18 @@ built it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). ## Terminal 1: Launching the simulation -Launch Gazebo with the FR3 including its gripper: +Launch Gazebo with the FR3 and its gripper: ``` ros2 launch hpp_tutorial tutorial_7_launch.py ``` Wait until you see `Configured and activated gripper_controller` in the output. -The launch file also spawns the ground plane and the box, then bridges the -Gazebo topics used to attach/detach the box. Note: one gripper finger may appear loose in Gazebo. This is a simulation -artefact with mimic joints (finger2 copies finger1 via physics constraints). -It does not affect the real robot. +artefact with mimic joints. It does not affect the tutorial. -## Terminal 2: Planning and executing +## Terminal 2: Planning Open a second terminal: @@ -50,60 +50,80 @@ cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` -The script loads the FR3 with a box on a ground plane, builds a manipulation -constraint graph, and solves a pick-and-place problem (move the box from -`(0.4, -0.2)` to `(0.4, 0.2)`). At the end you get two lists, `configs` and -`times`, describing the full planned motion, including the waypoints where a -grasp is acquired or released. +The script loads the FR3, the ground, and a box. It solves a pick-and-place +problem that moves the box from `(0.4, -0.2)` to `(0.4, 0.2)`, optimizes the +path, and time-parameterizes it with `SimpleTimeParameterization`. -You can visualize the path in the browser viewer: +You can visualize the planned path in the browser viewer: ```python v = display() v.loadPath(p_timed) ``` -## Executing +At this point the useful objects are: + +- `p_timed`: the time-parameterized HPP path. +- `configs`: sampled HPP configurations along the timed path. +- `times`: timestamps in seconds, returned by `segments_from_graph`. +- `segments`: executable arm trajectory slices with gripper pre-actions. +- `graph`: the HPP manipulation constraint graph. +- `open_gripper`, `close_gripper`, `grasp_box`, and `release_box`: small + Gazebo actions for the gripper and simulated box attachment. + +## Building execution segments -Before each run, reset the simulated box and open the gripper: +The planned path contains the approach, transport, and retreat motion in one +path. To execute it, ask `hpp_exec` to sample the timed path, insert graph +transition boundaries, and split the arm motion where a gripper action is +needed: ```python -prepare_sim_for_run() +from hpp_exec import segments_from_graph + +configs, times, segments = segments_from_graph( + p_timed, graph, + on_grasp=grasp_box, + on_release=release_box, +) ``` -Then execute the planned motion: +For this problem, `segments` contains three phases: + +| # | Phase | What the arm does | Pre-action | +|---|-----------|-----------------------------|------------| +| 0 | approach | move above the box, descend | none | +| 1 | transport | carry the box to the goal | attach and close | +| 2 | retreat | lift and return | open and detach | + +Make the initial gripper state explicit before the approach: ```python -execute_on_gazebo() +segments[0].pre_actions.insert(0, open_gripper) ``` -Expected behaviour in Gazebo: the gripper opens, descends onto the box, the -box is attached to the hand, the fingers close, the arm carries it to the goal, -the fingers open, the box is detached, and the arm retreats. +This matters if a previous run left the simulated gripper closed. -## Understanding the execution +## Executing the segments -The script builds the execution segments for you. For this problem there are -three phases: +Send the arm segments to the arm controller and let the pre-actions command +the gripper controller: -1. approach the box with the gripper open, -2. grasp the box and transport it, -3. release the box and retreat. +```python +from hpp_exec import execute_segments -The details of how `hpp-exec` detects grasp/release transitions from the HPP -constraint graph, builds segments, and calls gripper actions are documented in -the [hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html#hpp_exec_grasps). +execute_segments( + segments, configs, times, + joint_names=[f"fr3_joint{i}" for i in range(1, 8)], + joint_indices=list(range(7)), +) +``` -## Experiment +You should see the fingers open, the arm descend, the fingers close on the +box, the arm carry the box to the goal, the fingers open, and the arm retreat. -Run the same planned path twice: +`init.py` also defines this convenience wrapper: ```python -prepare_sim_for_run() -execute_on_gazebo() -prepare_sim_for_run() execute_on_gazebo() ``` - -The second `prepare_sim_for_run()` call should put the box back at the initial -pose before replaying the motion. diff --git a/tutorial_7/init.py b/tutorial_7/init.py index cb3e229..7d3b31b 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -180,18 +180,7 @@ def display(): p_timed = stp.optimize(p_opt) print(f"Trajectory duration: {p_timed.length():.3f} s") -# Extract waypoints -n_samples = 200 -configs = [] -times = [] -for i in range(n_samples + 1): - t = (i / n_samples) * p_timed.length() - q, success = p_timed(t) - if success: - configs.append(np.array(q)) - times.append(t) - -print(f"Extracted {len(configs)} waypoints, ready to execute.") +print("Timed path ready for visualization and execution.") print() print("To visualize:") print(" v = display()") @@ -339,13 +328,13 @@ def release_box(): def prepare_sim_for_run(): return box_controller.reset_pose(BOX_INITIAL_POSITION) and open_gripper() - segments = segments_from_graph( - configs, - times, + configs, times, segments = segments_from_graph( + p_timed, graph, on_grasp=grasp_box, on_release=release_box, ) + print(f"Extracted {len(configs)} waypoints, ready to execute.") if segments: segments[0].pre_actions.insert(0, open_gripper) From 5b5034940bb04da2b2356eaa95ab2b72b840ad68 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 12 May 2026 11:00:26 +0200 Subject: [PATCH 7/8] [tutorial_6] Pin Franka Docker sources --- tutorial_6/Dockerfile | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 05ab7c1..617bfaa 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -1,11 +1,14 @@ # Extends the base tutorial image with ROS2 control + Gazebo + Franka packages # for executing planned trajectories on a simulated Panda/FR3 robot. # -# Build: docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp-tutorial-ros2 . +# Build: docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp-ros2:tuto . # Run: See tutorial_6/README.md for docker run instructions. FROM hpp:tuto +ARG FRANKA_DESCRIPTION_VERSION=2.6.0 +ARG FRANKA_ROS2_VERSION=v3.2.2 + USER root # ROS2 control packages @@ -28,29 +31,25 @@ RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ ros-jazzy-xacro \ && apt-get clean && rm -rf /var/lib/apt/lists/* -# Franka Panda packages (description + Gazebo bringup) +# Franka packages used by the tutorial. Pin the source versions because the +# upstream jazzy branch can add runtime dependencies for hardware controllers +# that are not needed for this Gazebo-only example. WORKDIR /opt/franka_ws RUN mkdir -p src \ && cd src \ - && git clone --depth 1 https://github.com/frankarobotics/franka_description.git \ - && git clone --branch jazzy --depth 1 https://github.com/frankarobotics/franka_ros2.git - -RUN apt-get update && apt-get install -y python3-rosdep \ - && (rosdep init 2>/dev/null || true) \ - && rosdep update \ - && . /opt/ros/jazzy/setup.sh \ - && rosdep install --from-paths src --ignore-src -r -y \ - && apt-get clean && rm -rf /var/lib/apt/lists/* + && git clone --branch ${FRANKA_DESCRIPTION_VERSION} --depth 1 https://github.com/frankarobotics/franka_description.git \ + && git clone --branch ${FRANKA_ROS2_VERSION} --depth 1 https://github.com/frankarobotics/franka_ros2.git RUN chown -R user:user /opt/franka_ws USER user RUN . /opt/ros/jazzy/setup.sh \ - && colcon build --packages-up-to \ + && colcon build --packages-select \ franka_description \ + franka_mobile_sensors \ franka_gazebo_bringup \ - --cmake-args -DCMAKE_BUILD_TYPE=Release + --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF RUN echo 'source /opt/franka_ws/install/setup.bash 2>/dev/null' >> /home/user/.bashrc From 62c91bf431760a517115ba5edcf9ef7530e694f8 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 12 May 2026 13:29:59 +0200 Subject: [PATCH 8/8] [tutorial_6] Install ROS-Gazebo bridge --- tutorial_6/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 617bfaa..4b315cf 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -27,6 +27,7 @@ RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ ros-jazzy-sdformat-urdf \ ros-jazzy-gz-ros2-control \ ros-jazzy-ros-gz-sim \ + ros-jazzy-ros-gz-bridge \ ros-jazzy-joint-state-publisher-gui \ ros-jazzy-xacro \ && apt-get clean && rm -rf /var/lib/apt/lists/*