From 9ff2a9f4e20a460a023524fb29fa1d832f8f7082 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 18 May 2026 11:05:34 +0200 Subject: [PATCH 1/5] [tutorial_6] Keep ROS packages consistent in Docker image --- tutorial_6/Dockerfile | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 4b315cf..6607cb8 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -12,9 +12,13 @@ ARG FRANKA_ROS2_VERSION=v3.2.2 USER root # ROS2 control packages -RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ +RUN apt-get update -y \ + && DEBIAN_FRONTEND=noninteractive apt-get dist-upgrade -qqy \ + && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ + ros-jazzy-fastcdr \ ros-jazzy-control-msgs \ ros-jazzy-trajectory-msgs \ + ros-jazzy-joint-state-broadcaster \ ros-jazzy-joint-trajectory-controller \ ros-jazzy-controller-manager \ ros-jazzy-ros2controlcli \ From d57e14a00e7b01febd9565210b93176e6c6e5c62 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 20 May 2026 11:58:22 +0200 Subject: [PATCH 2/5] [tutorial_7] Use hpp-exec graph segments --- tutorial_7/README.md | 71 +++++----- tutorial_7/init.py | 315 ++++++++++++++++--------------------------- 2 files changed, 155 insertions(+), 231 deletions(-) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index 88955a7..faf32a8 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -11,8 +11,8 @@ adds the gripper: the robot must open the fingers before approaching the box, close them before transport, then open them again at the goal. 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. +graph. We then use `hpp_exec` to expose the graph segments, attach the Gazebo +actions for this problem, and execute the segments. For the full `hpp_exec` API, see the [hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html). @@ -52,7 +52,8 @@ 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`. +path, enforces transition semantics, and time-parameterizes it with +`SimpleTimeParameterization`. You can visualize the planned path in the browser viewer: @@ -66,52 +67,63 @@ 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. +- `segments`: graph segments where you can add pre/post 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. +- `open_gripper`, `close_gripper`, `grasp_box`, and `release_box`: Gazebo + actions for the gripper and simulated box attachment. ## Building execution segments 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: +path. Ask `hpp_exec` to sample the timed path and expose the HPP graph +segments: ```python -from hpp_exec import segments_from_graph +from hpp_exec import print_segments, segments_from_graph -configs, times, segments = segments_from_graph( - p_timed, graph, - on_grasp=grasp_box, - on_release=release_box, -) +configs, times, segments = segments_from_graph(p_timed, graph) +print_segments(segments) ``` -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 | +The table shows the segment times, graph transition names, nominal states, +observed states, and how many pre/post actions are attached. +`EnforceTransitionSemantic` keeps the graph transition stored on each optimized +subpath consistent with the states observed along that subpath. -Make the initial gripper state explicit before the approach: +For this tutorial, use the rows named +`fr3/gripper > box/handle | f_23` and +`fr3/gripper < box/handle | 0-0_32`. With the current path they are segments +2 and 4: ```python -segments[0].pre_actions.insert(0, open_gripper) +segments[0].pre_actions.append(open_gripper) + +# 2: fr3/gripper > box/handle | f_23 +segments[2].pre_actions.append(grasp_box) + +# 4: fr3/gripper < box/handle | 0-0_32 +segments[4].pre_actions.append(release_box) + +print_segments(segments) ``` -This matters if a previous run left the simulated gripper closed. +Conceptually, execution has three phases: + +| # | Phase | What the arm does | Action before phase | +|---|-----------|-----------------------------|---------------------| +| 0 | approach | move above the box, descend | open | +| 1 | transport | carry the box to the goal | attach and close | +| 2 | retreat | lift and return | open and detach | ## Executing the segments -Send the arm segments to the arm controller and let the pre-actions command +Send the arm segments to the arm controller and let the segment actions command the gripper controller: ```python from hpp_exec import execute_segments +reset_box_pose() execute_segments( segments, configs, times, joint_names=[f"fr3_joint{i}" for i in range(1, 8)], @@ -122,8 +134,5 @@ execute_segments( 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: - -```python -execute_on_gazebo() -``` +`reset_box_pose()` detaches the simulated box if needed and places it back at +the planned start pose before execution. diff --git a/tutorial_7/init.py b/tutorial_7/init.py index 7d3b31b..011d2e9 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -1,27 +1,30 @@ import time import numpy as np +import rclpy +from geometry_msgs.msg import Pose +from hpp_exec import ( + print_segments, + segments_from_graph, + send_trajectory, +) from pinocchio import SE3 from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint from pyhpp.core import RandomShortcut, SimpleTimeParameterization -from pyhpp.manipulation import Device, Graph, ManipulationPlanner, Problem, urdf +from pyhpp.manipulation import ( + Device, + EnforceTransitionSemantic, + Graph, + ManipulationPlanner, + Problem, + urdf, +) from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp_viser import Viewer - -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 - - ROS_EXECUTION_AVAILABLE = True - ROS_EXECUTION_IMPORT_ERROR = None -except ImportError as exc: - ROS_EXECUTION_AVAILABLE = False - ROS_EXECUTION_IMPORT_ERROR = exc +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 def display(): @@ -172,6 +175,9 @@ def display(): p_opt = optimizer.optimize(p) print(f"Optimized path length: {p_opt.length():.3f}") +semantic = EnforceTransitionSemantic(problem) +p_opt = semantic.optimize(p_opt) + # Time-parameterize for execution stp = SimpleTimeParameterization(problem) stp.order = 2 @@ -187,189 +193,98 @@ def display(): print(" v.loadPath(p_timed)") print() +configs, times, segments = segments_from_graph(p_timed, graph) + +if not rclpy.ok(): + rclpy.init() + +gazebo_node = Node("tutorial_7_box_controller") +attach_pub = gazebo_node.create_publisher(Empty, "/box/attach", 1) +detach_pub = gazebo_node.create_publisher(Empty, "/box/detach", 1) +pose_client = gazebo_node.create_client(SetEntityPose, "/world/empty/set_pose") + + +def attach_box(): + attach_pub.publish(Empty()) + rclpy.spin_once(gazebo_node, timeout_sec=0.05) + time.sleep(0.2) + gazebo_node.get_logger().info("Published '/box/attach' on Gazebo topic") + return True + + +def detach_box(): + detach_pub.publish(Empty()) + rclpy.spin_once(gazebo_node, timeout_sec=0.05) + time.sleep(0.2) + gazebo_node.get_logger().info("Published '/box/detach' on Gazebo topic") + return True + + +def reset_box_pose(xyz=BOX_INITIAL_POSITION): + if not detach_box(): + return False + + if not pose_client.wait_for_service(timeout_sec=3.0): + gazebo_node.get_logger().error("Gazebo pose service is unavailable") + return False -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])], - [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", - ) - - 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() - - configs, times, segments = segments_from_graph( - p_timed, - graph, - on_grasp=grasp_box, - on_release=release_box, + 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 = pose_client.call_async(req) + rclpy.spin_until_future_complete(gazebo_node, future, timeout_sec=3.0) + result = future.result() + if result is None or not result.success: + gazebo_node.get_logger().error("Failed to reset the Gazebo box pose") + return False + + time.sleep(0.2) + gazebo_node.get_logger().info( + f"Reset box pose to x={xyz[0]:.3f}, y={xyz[1]:.3f}, z={xyz[2]:.3f}" + ) + return True + + +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", ) - print(f"Extracted {len(configs)} waypoints, ready to execute.") - 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(" 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): - 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 / Gazebo execution helpers are unavailable in this Python env.") - print(f"Import error: {ROS_EXECUTION_IMPORT_ERROR}") + + +def grasp_box(): + return attach_box() and close_gripper() + + +def release_box(): + return open_gripper() and detach_box() + + +print(f"Extracted {len(configs)} waypoints.") +print_segments(segments) + +print("Useful helpers:") +print(" open_gripper()") +print(" close_gripper()") +print(" grasp_box()") +print(" release_box()") +print(" attach_box()") +print(" detach_box()") +print(" reset_box_pose()") From 26532c78c4c412609458d51810648c04a73d1ba6 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 20 May 2026 13:45:25 +0200 Subject: [PATCH 3/5] Fix typos. --- tutorial_3/README.md | 2 +- tutorial_5/README.md | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tutorial_3/README.md b/tutorial_3/README.md index 1fc763a..6488a8f 100644 --- a/tutorial_3/README.md +++ b/tutorial_3/README.md @@ -229,6 +229,6 @@ v.loadPath(p1) v.loadPath(p2) v.loadPath(p3) ``` -The drilling path is a little bit naive since the tool trajectory is a straight line but the result of an interpolation in joint space and the velocities of the robot and tool are not controlled. +The drilling path is a little bit naive since the tool trajectory is not a straight line but the result of an interpolation in joint space and the velocities of the robot and tool are not controlled. [Tutorial 4](../tutorial_4/README.md) shows how to better control a tool trajectory. diff --git a/tutorial_5/README.md b/tutorial_5/README.md index e7ac654..054876b 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -48,7 +48,7 @@ p2 = bezier.optimize(p1) print(f"Smoothed path length: {p2.length():.3f} (was {p1.length():.3f})") ``` -The resulting path is C²-continuous — a prerequisite for producing a smooth velocity profile in +The resulting path is continuously differentiable (C1), a prerequisite for producing a smooth velocity profile in the next step. ## Time parameterization @@ -91,7 +91,7 @@ Parameters: Compare the profiles with and without Bézier smoothing: ```python -fig = plotTraj(toppra.optimize(p1), 0, 7, order=2) +fig = plotTraj(p3, 0, 7, order=2) fig.show() fig = plotTraj(p3, 0, 7, order=2) fig.show() From 76e3a17f7b162f8aed88008c1eb1dd565959d861 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 20 May 2026 15:01:49 +0200 Subject: [PATCH 4/5] [tutorial_7] Move explanation about EnforceTransitionSemantic. --- tutorial_7/README.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index faf32a8..aa747f3 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -52,8 +52,9 @@ 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, enforces transition semantics, and time-parameterizes it with -`SimpleTimeParameterization`. +path, time-parameterizes it with `SimpleTimeParameterization`. + +Note that between optimization and time parameterization, an object called `EnforceTransitionSemantic` is called. This steps labels each sub-path with the transition of the graph the sub-path belongs to. This step is necessary before executing the path in order to place pre-actions and post-actions at the right times. You can visualize the planned path in the browser viewer: @@ -87,8 +88,6 @@ print_segments(segments) The table shows the segment times, graph transition names, nominal states, observed states, and how many pre/post actions are attached. -`EnforceTransitionSemantic` keeps the graph transition stored on each optimized -subpath consistent with the states observed along that subpath. For this tutorial, use the rows named `fr3/gripper > box/handle | f_23` and From 49a52c9427627b3563c4fe8b039eb22b34f70788 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 20 May 2026 15:17:45 +0200 Subject: [PATCH 5/5] [tutorial_7] Add explanations about pre and post actions. --- tutorial_7/README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/tutorial_7/README.md b/tutorial_7/README.md index aa747f3..2487f23 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -135,3 +135,14 @@ box, the arm carry the box to the goal, the fingers open, and the arm retreat. `reset_box_pose()` detaches the simulated box if needed and places it back at the planned start pose before execution. + +## More details about the pre and post actions + +In script `init.py` several functions are defined. + + - `open_gripper` and `close_gripper` sends a reference value for "fr3_finger_joint1" in order + to open or close the gripper. This function uses `send_trajectory` to do so, as in + `tutorial_6`. On the real robot, this is performed by a ROS action instead. + - `grasp_box` and `release_box` call `attach_box` and `detach_box` respectively. These functions + tell gazebo that the box is attached to or detached from the end effector. They are useless on + the real robot.