From 2e32e6c56c36ed3c55d3d7f894ec8772a553e525 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 30 Mar 2026 08:41:49 +0200 Subject: [PATCH 01/36] Add tutorial 5: path optimization and time parameterization --- tutorial_4/README.md | 2 + tutorial_5/README.md | 87 +++++++++++++++++++++++++++- tutorial_5/init.py | 135 +++++++++++++++++++++++++++++++++++++++++++ urdf/obstacle.urdf | 18 ++++++ 4 files changed, 241 insertions(+), 1 deletion(-) create mode 100644 tutorial_5/init.py create mode 100644 urdf/obstacle.urdf diff --git a/tutorial_4/README.md b/tutorial_4/README.md index c5e3758..55d14c6 100644 --- a/tutorial_4/README.md +++ b/tutorial_4/README.md @@ -69,3 +69,5 @@ pv = transition.pathValidation() res, p3, report = pv.validate(p2, False) ``` The path is then tested for collision after setting the relevant security margins. + +[Tutorial 5](../tutorial_5/README.md) shows how to optimize and time-parameterize paths. diff --git a/tutorial_5/README.md b/tutorial_5/README.md index 5dcc1e4..f5b8b5e 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -1,3 +1,88 @@ # How to optimize and time-parameterize paths. -Work in progress, please be patient. +## Prerequisite + +Having completed [tutorial 4](../tutorial_4/README.md) + +## Initializing the problem + +In the docker container, cd into `tutorial_5` directory. In a bash terminal, run + +``` +python -i init.py +``` + +The script contains the code of [tutorial 4](../tutorial_4/README.md) with an additional obstacle +placed between the robot and the plate. This forces the planner to find a non-straight path for `p1` +(the motion from `q_init` to the pregrasp configuration `qpg`). + +The path `p1` uses a dimensionless path parameter `s` ranging from `0` to `p1.length()`. This +parameter does not correspond to real time: the robot has no notion of velocity or acceleration +along this path. + +## SimpleTimeParameterization + +`SimpleTimeParameterization` computes a polynomial time parameterization that maps real time `t` to +the path parameter `s`, while respecting joint velocity and acceleration limits. + +``` +from pyhpp.core import SimpleTimeParameterization + +stp = SimpleTimeParameterization(problem) +stp.order = 2 +stp.safety = 0.95 +stp.maxAcceleration = 1.0 +p1_stp = stp.optimize(p1) +``` + +After optimization, `p1_stp.length()` returns the total execution time in seconds: +``` +print(f"Original path parameter range: {p1.length():.3f}") +print(f"STP duration: {p1_stp.length():.3f} s") +``` + +The parameters control the time parameterization: +- `order`: polynomial continuity order. `0` = linear (C0), `1` = cubic (C1, zero velocity at + endpoints), `2` = quintic (C2, zero velocity and acceleration at endpoints). +- `safety`: scaling factor for velocity limits (0.95 = use 95% of max velocity). +- `maxAcceleration`: maximum acceleration per DOF. Only used when `order >= 2`. Set to a negative + value to disable. + +## TOPPRA + +TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) computes the +time-optimal parameterization subject to velocity and torque constraints. Unlike +`SimpleTimeParameterization`, it accounts for the robot's dynamics. + +``` +from pyhpp_toppra import Toppra + +toppra = Toppra(problem) +toppra.velocityScale = 1.0 +toppra.N = 100 +p1_toppra = toppra.optimize(p1) +``` + +Compare the results: +``` +print(f"TOPPRA duration: {p1_toppra.length():.3f} s") +``` + +TOPPRA typically produces shorter execution times than `SimpleTimeParameterization` because it +computes the time-optimal solution rather than a conservative polynomial approximation. + +The parameters are: +- `velocityScale`: scaling factor for velocity limits (1.0 = full velocity). +- `effortScale`: scaling factor for torque limits. Set to a negative value to disable torque + constraints. +- `N`: minimal number of discretization points along the path. +- `interpolationMethod`: `"constant_acceleration"` or `"hermite"`. +- `gridpointMethod`: `"param_space"` or `"time_space"`. + +## Visualization + +You can visualize the path in the viewer: +``` +v = display() +v.loadPath(p1_toppra) +``` diff --git a/tutorial_5/init.py b/tutorial_5/init.py new file mode 100644 index 0000000..dbfb286 --- /dev/null +++ b/tutorial_5/init.py @@ -0,0 +1,135 @@ +import numpy as np +from pinocchio import SE3, neutral +from pyhpp.manipulation import ( + Device, + Graph, + Problem, + TransitionPlanner, + urdf, +) +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory +from pyhpp.manipulation.security_margins import SecurityMargins +from pyhpp_viser import Viewer + + +def display(): + v = Viewer(robot) + v.initViewer(open=False, loadModel=True) + v.setProblem(problem) + v.setGraph(graph) + return v + + +# use v = display() to create a Viewer instance. + +robot = Device("tuto") + +urdf_filename = "package://hpp_tutorial/urdf/staubli-drill.urdf" +srdf_filename = "package://hpp_tutorial/srdf/staubli-drill.srdf" + +urdf.loadModel( + robot, 0, "staubli", "anchor", urdf_filename, srdf_filename, SE3.Identity() +) + +urdf_filename = "package://hpp_tutorial/urdf/square-plate.urdf" +srdf_filename = "" + +urdf.loadModel( + robot, 0, "plate", "freeflyer", urdf_filename, srdf_filename, SE3.Identity() +) +robot.setJointBounds( + "plate/root_joint", + [ + 0, + 2, + -1, + 1, + 0, + 2, + -float("Inf"), + float("Inf"), + -float("Inf"), + float("Inf"), + -float("Inf"), + float("Inf"), + -float("Inf"), + float("Inf"), + ], +) + +# Load an obstacle between robot and plate to force a non-straight path +obstacle_pose = SE3(rotation=np.identity(3), translation=np.array([0.5, -0.25, 1.2])) +urdf.loadModel( + robot, 0, "obstacle", "anchor", + "package://hpp_tutorial/urdf/obstacle.urdf", "", obstacle_pose, +) + +# Position the plate in the environment +q_init = neutral(robot.model()) +r = robot.rankInConfiguration["plate/root_joint"] +q_init[r : r + 3] = [0.8, 0, 1] + +# Add a handle on the plate +R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) +T = np.array([0.02, 0, 0]) +pose = SE3(rotation=R, translation=T) +robot.addHandle("plate/base_link", "plate/hole", pose, 0.1, 6 * [True]) +handle = robot.handles()["plate/hole"] +handle.approachingDirection = np.array([0, 0, 1]) + +# Build the constraint graph +problem = Problem(robot) +graph = Graph("robot", robot, problem) +factory = ConstraintGraphFactory(graph) +factory.setGrippers(["staubli/tooltip"]) +objects = ["plate"] +handlesPerObject = [["plate/hole"]] +contactsPerObject = [[]] +factory.setObjects(objects, handlesPerObject, contactsPerObject) +factory.generate() +# Set security margins +sm = SecurityMargins(problem, factory, ["staubli", "plate"], robot) +sm.setSecurityMarginBetween("staubli", "plate", 0.05) +sm.apply() +# Deactivate collision checking between robot last joint and plate for the last part of the +# motion +transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") +graph.setSecurityMarginForTransition( + transition, "staubli/joint_6", "plate/root_joint", float("-inf") +) +graph.initialize() +graph.initialize() + +shooter = problem.configurationShooter() + +# Plan motions between waypoint configurations +planner = TransitionPlanner(problem) +planner.maxIterations(1000) +p1 = None +for i in range(50): + # Project random configuration on pregrasp waypoint state + transition = graph.getTransition("staubli/tooltip > plate/hole | f_01") + q = shooter.shoot() + res, qpg, err = graph.generateTargetConfig(transition, q_init, q) + if not res: + continue + # Check the configuration for collision + pv = transition.pathValidation() + res, report = pv.validateConfiguration(qpg) + if not res: + continue + # plan motion between q_init and qpg + planner.setTransition(transition) + try: + q_goal = np.zeros((1, robot.configSize()), order="F") + q_goal[0, :] = qpg + p1 = planner.planPath(q_init, q_goal, True) + except Exception as exc: + print(f"path planning failed between q_init and qpg: {exc}") + continue + break + +if p1 is None: + print("Failed to plan p1 after 50 attempts. Try running the script again.") +else: + print(f"Path p1 planned (q_init -> qpg), length: {p1.length():.3f}") diff --git a/urdf/obstacle.urdf b/urdf/obstacle.urdf new file mode 100644 index 0000000..857fe5c --- /dev/null +++ b/urdf/obstacle.urdf @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + From 54e96a9952f8f8a7891ab0a8463fe7fb2cb3e031 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 30 Mar 2026 13:52:25 +0200 Subject: [PATCH 02/36] [tutorial_5] Fix obstacle position, add path optimization and slow acceleration - Move obstacle to [0.5, -0.2, 1.2] so it blocks the straight-line path - Add RandomShortcut path optimization before time parameterization - Use maxAcceleration=0.5 for slower, more visible motions - Document that effortScale requires mass/inertia data (disabled for Staubli) --- tutorial_5/README.md | 59 +++++++++++++++++++++++++++++++++----------- tutorial_5/init.py | 16 +++++++++++- 2 files changed, 59 insertions(+), 16 deletions(-) diff --git a/tutorial_5/README.md b/tutorial_5/README.md index f5b8b5e..2abe568 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -20,24 +20,40 @@ The path `p1` uses a dimensionless path parameter `s` ranging from `0` to `p1.le parameter does not correspond to real time: the robot has no notion of velocity or acceleration along this path. +## Path optimization + +The path returned by the planner is collision-free but typically jagged, with unnecessary detours. +Before time-parameterizing, we should optimize the geometric path using shortcutting algorithms: + +```python +from pyhpp.core import RandomShortcut + +shortcut = RandomShortcut(problem) +p1_opt = shortcut.optimize(p1) +print(f"Optimized path length: {p1_opt.length():.3f} (was {p1.length():.3f})") +``` + +`RandomShortcut` repeatedly picks two random points along the path and tries to connect them with a +straight segment. If the shortcut is collision-free and shorter, it replaces the original sub-path. + ## SimpleTimeParameterization `SimpleTimeParameterization` computes a polynomial time parameterization that maps real time `t` to the path parameter `s`, while respecting joint velocity and acceleration limits. -``` +```python from pyhpp.core import SimpleTimeParameterization stp = SimpleTimeParameterization(problem) stp.order = 2 stp.safety = 0.95 -stp.maxAcceleration = 1.0 -p1_stp = stp.optimize(p1) +stp.maxAcceleration = 0.5 +p1_stp = stp.optimize(p1_opt) ``` After optimization, `p1_stp.length()` returns the total execution time in seconds: -``` -print(f"Original path parameter range: {p1.length():.3f}") +```python +print(f"Optimized path length: {p1_opt.length():.3f}") print(f"STP duration: {p1_stp.length():.3f} s") ``` @@ -45,8 +61,18 @@ The parameters control the time parameterization: - `order`: polynomial continuity order. `0` = linear (C0), `1` = cubic (C1, zero velocity at endpoints), `2` = quintic (C2, zero velocity and acceleration at endpoints). - `safety`: scaling factor for velocity limits (0.95 = use 95% of max velocity). -- `maxAcceleration`: maximum acceleration per DOF. Only used when `order >= 2`. Set to a negative - value to disable. +- `maxAcceleration`: maximum acceleration per DOF in rad/s². Only used when `order >= 2`. Set to a + negative value to disable. **Use a small value** (e.g. 0.5) to produce slower, more visible + motions — this makes the difference between the raw path `p1` and the parameterized trajectory + `p1_stp` easier to observe. + +Try different values to see the effect on execution time: +```python +for acc in [0.25, 0.5, 1.0, 2.0]: + stp.maxAcceleration = acc + p = stp.optimize(p1_opt) + print(f" maxAcceleration={acc:.2f} -> duration={p.length():.3f} s") +``` ## TOPPRA @@ -54,17 +80,18 @@ TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) compu time-optimal parameterization subject to velocity and torque constraints. Unlike `SimpleTimeParameterization`, it accounts for the robot's dynamics. -``` +```python from pyhpp_toppra import Toppra toppra = Toppra(problem) -toppra.velocityScale = 1.0 +toppra.velocityScale = 0.5 +toppra.effortScale = -1 toppra.N = 100 -p1_toppra = toppra.optimize(p1) +p1_toppra = toppra.optimize(p1_opt) ``` Compare the results: -``` +```python print(f"TOPPRA duration: {p1_toppra.length():.3f} s") ``` @@ -72,9 +99,11 @@ TOPPRA typically produces shorter execution times than `SimpleTimeParameterizati computes the time-optimal solution rather than a conservative polynomial approximation. The parameters are: -- `velocityScale`: scaling factor for velocity limits (1.0 = full velocity). +- `velocityScale`: scaling factor for velocity limits (1.0 = full velocity). Use a small value + (e.g. 0.5) for slower, more visible motions. - `effortScale`: scaling factor for torque limits. Set to a negative value to disable torque - constraints. + constraints. **Note**: torque constraints require mass and inertia data in the robot URDF. The + current Staubli model has no dynamics data, so `effortScale` must be set to `-1` (disabled). - `N`: minimal number of discretization points along the path. - `interpolationMethod`: `"constant_acceleration"` or `"hermite"`. - `gridpointMethod`: `"param_space"` or `"time_space"`. @@ -82,7 +111,7 @@ The parameters are: ## Visualization You can visualize the path in the viewer: -``` +```python v = display() -v.loadPath(p1_toppra) +v.loadPath(p1_stp) ``` diff --git a/tutorial_5/init.py b/tutorial_5/init.py index dbfb286..6c400a1 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -1,5 +1,6 @@ import numpy as np from pinocchio import SE3, neutral +from pyhpp.core import RandomShortcut, SimpleTimeParameterization from pyhpp.manipulation import ( Device, Graph, @@ -58,7 +59,7 @@ def display(): ) # Load an obstacle between robot and plate to force a non-straight path -obstacle_pose = SE3(rotation=np.identity(3), translation=np.array([0.5, -0.25, 1.2])) +obstacle_pose = SE3(rotation=np.identity(3), translation=np.array([0.5, -0.2, 1.2])) urdf.loadModel( robot, 0, "obstacle", "anchor", "package://hpp_tutorial/urdf/obstacle.urdf", "", obstacle_pose, @@ -133,3 +134,16 @@ def display(): print("Failed to plan p1 after 50 attempts. Try running the script again.") else: print(f"Path p1 planned (q_init -> qpg), length: {p1.length():.3f}") + + # Optimize the geometric path before time parameterization + shortcut = RandomShortcut(problem) + p1_opt = shortcut.optimize(p1) + print(f"Optimized path length: {p1_opt.length():.3f} (was {p1.length():.3f})") + + # Time parameterization with low acceleration to visualize the difference + stp = SimpleTimeParameterization(problem) + stp.order = 2 + stp.safety = 0.95 + stp.maxAcceleration = 0.5 + p1_stp = stp.optimize(p1_opt) + print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)") From 8a5eb7564e6f2f36fd049e69812534bfa704cc91 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 08:23:00 +0200 Subject: [PATCH 03/36] add tutorial 6: execute HPP motions on simulated robot - tutorial_6: Dockerfile, controllers, init.py, launch_sim.py - CMakeLists: install rules for tutorial_6 and tutorial_7 - Makefile: add hpp-core, hpp-manipulation, hpp-exec build targets - README: update tutorial 6 description, add tutorial 7 - tutorial_5: remove duplicate graph.initialize() - run_docker.sh: fix executable permission --- CMakeLists.txt | 6 ++ README.md | 3 +- tutorial_1/Makefile | 17 +++- tutorial_1/run_docker.sh | 0 tutorial_5/init.py | 1 - tutorial_6/Dockerfile | 57 ++++++++++++++ tutorial_6/README.md | 151 +++++++++++++++++++++++++++++++++++- tutorial_6/controllers.yaml | 28 +++++++ tutorial_6/init.py | 64 +++++++++++++++ tutorial_6/launch_sim.py | 124 +++++++++++++++++++++++++++++ 10 files changed, 446 insertions(+), 5 deletions(-) mode change 100644 => 100755 tutorial_1/run_docker.sh create mode 100644 tutorial_6/Dockerfile create mode 100644 tutorial_6/controllers.yaml create mode 100644 tutorial_6/init.py create mode 100644 tutorial_6/launch_sim.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 77662f7..25b5941 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,5 +95,11 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/materials/textures ) +install(DIRECTORY tutorial_6 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "Dockerfile" EXCLUDE + PATTERN "README.md" EXCLUDE) +install(DIRECTORY tutorial_7 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "README.md" EXCLUDE) + add_library(${PROJECT_NAME} INTERFACE) install(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) diff --git a/README.md b/README.md index 45bfe9d..8717927 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,8 @@ The various tutorials are: 3. [tutorial 3](./tutorial_3/README.md) How to use HPP in manufacturing. 4. [tutorial 4](./tutorial_4/README.md) How to control the trajectory of a tool. 5. [tutorial 5](./tutorial_5/README.md) How to optimize and time-parameterize paths. -6. [tutorial 6](./tutorial_6/README.md) How to execute motions on a real robot. +6. [tutorial 6](./tutorial_6/README.md) How to execute motions on a simulated robot. +7. [tutorial 7](./tutorial_7/README.md) Pick and place with gripper. [![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/commits/master) [![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp_tutorial/master/coverage/) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index e754a2b..e11e664 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -38,6 +38,14 @@ HPP_EXTRA_FLAGS= -DBUILD_TESTING=${BUILD_TESTING} jrl-cmakemodules_branch=master jrl-cmakemodules_repository=${JRL_REPO} +hpp-core_branch=devel +hpp-core_repository=${HPP_REPO} +hpp-core_extra_flags=${HPP_EXTRA_FLAGS} + +hpp-manipulation_branch=devel +hpp-manipulation_repository=${HPP_REPO} +hpp-manipulation_extra_flags=${HPP_EXTRA_FLAGS} + hpp-python_branch=devel hpp-python_repository=${HPP_REPO} hpp-python_extra_flags=${HPP_EXTRA_FLAGS} ${PYTHON_FLAGS} @@ -57,6 +65,10 @@ hpp-plot_branch=devel hpp-plot_repository=${HPP_REPO} hpp-plot_extra_flags= -DINSTALL_DOCUMENTATION=OFF +hpp-exec_branch=devel +hpp-exec_repository=${HPP_REPO} +hpp-exec_extra_flags=${PYTHON_FLAGS} + # }}} ################################## # {{{ Packages for hpp-plot @@ -103,13 +115,16 @@ python-venv: jrl-cmakemodules.configure.dep: jrl-cmakemodules.checkout hpp-doc.configure.dep: hpp-doc.checkout hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install qgv.install -hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install +hpp-core.configure.dep: hpp-core.checkout jrl-cmakemodules.install +hpp-manipulation.configure.dep: hpp-manipulation.checkout jrl-cmakemodules.install hpp-core.install +hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install hpp-core.install hpp-manipulation.install qgv.configure.dep: qgv.checkout jrl-cmakemodules.install hpp_tutorial.configure.dep: hpp_tutorial.checkout hpp-gepetto-viewer.install \ hpp-python.install hpp-gepetto-viewer.configure.dep: hpp-gepetto-viewer.checkout python-venv hpp-python.install toppra.configure.dep: toppra.checkout hpp-toppra.configure.dep: hpp-toppra.checkout jrl-cmakemodules.install toppra.install +hpp-exec.configure.dep: hpp-exec.checkout jrl-cmakemodules.install # }}} ################################## diff --git a/tutorial_1/run_docker.sh b/tutorial_1/run_docker.sh old mode 100644 new mode 100755 diff --git a/tutorial_5/init.py b/tutorial_5/init.py index 6c400a1..5c57a63 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -99,7 +99,6 @@ def display(): transition, "staubli/joint_6", "plate/root_joint", float("-inf") ) graph.initialize() -graph.initialize() shooter = problem.configurationShooter() diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile new file mode 100644 index 0000000..05ab7c1 --- /dev/null +++ b/tutorial_6/Dockerfile @@ -0,0 +1,57 @@ +# 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 . +# Run: See tutorial_6/README.md for docker run instructions. + +FROM hpp:tuto + +USER root + +# ROS2 control packages +RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ + ros-jazzy-control-msgs \ + ros-jazzy-trajectory-msgs \ + ros-jazzy-joint-trajectory-controller \ + ros-jazzy-controller-manager \ + ros-jazzy-ros2controlcli \ + python3-colcon-common-extensions + +# Gazebo Harmonic + ROS2 bridge +RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ + ros-jazzy-gz-sim-vendor \ + ros-jazzy-gz-plugin-vendor \ + ros-jazzy-sdformat-urdf \ + ros-jazzy-gz-ros2-control \ + ros-jazzy-ros-gz-sim \ + ros-jazzy-joint-state-publisher-gui \ + ros-jazzy-xacro \ + && apt-get clean && rm -rf /var/lib/apt/lists/* + +# Franka Panda packages (description + Gazebo bringup) +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/* + +RUN chown -R user:user /opt/franka_ws + +USER user + +RUN . /opt/ros/jazzy/setup.sh \ + && colcon build --packages-up-to \ + franka_description \ + franka_gazebo_bringup \ + --cmake-args -DCMAKE_BUILD_TYPE=Release + +RUN echo 'source /opt/franka_ws/install/setup.bash 2>/dev/null' >> /home/user/.bashrc + +WORKDIR /home/user/devel diff --git a/tutorial_6/README.md b/tutorial_6/README.md index aded22d..5a681bc 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -1,3 +1,150 @@ -# How to execute motions on a real robot. +# How to execute motions on a simulated robot. -Work in progress, please be patient. +## Prerequisite + +Having completed [tutorial 5](../tutorial_5/README.md). + +## Overview + +This tutorial plans a simple arm motion for the Franka FR3 robot and executes it +on a Gazebo simulation via ROS2. It introduces the `hpp_exec` package which +bridges HPP paths to `ros2_control`. + +## Setting up the simulation + +The base tutorial docker image does not include ROS2 control packages. Build the +extended image from the tutorial 6 directory **on the host machine** (not inside +the container): + +``` +cd /src/hpp_tutorial/tutorial_6 +docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ + -t hpp-tutorial-ros2 . +``` + +Then start the container: + +``` +cd +docker run --gpus all --env DISPLAY --env QT_X11_NO_MITSHM=1 \ + --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw --net host --privileged \ + -v .:/home/user/devel --rm --name hpp -it hpp-tutorial-ros2 +``` + +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 + +Source the ROS2 and Franka environments, then launch the Gazebo simulation with +a `joint_trajectory_controller`: + +``` +source /opt/ros/jazzy/setup.bash +source /opt/franka_ws/install/setup.bash +source ~/devel/config.sh +ros2 launch ~/devel/install/share/hpp_tutorial/tutorial_6/launch_sim.py +``` + +Wait until you see `Configured and activated joint_trajectory_controller` in the +output. + +## Terminal 2: Planning and executing + +Open a second terminal: + +``` +docker exec -it hpp bash +``` + +Source the environments and run the tutorial script: + +``` +source /opt/ros/jazzy/setup.bash +source /opt/franka_ws/install/setup.bash +source ~/devel/config.sh +cd ~/devel/src/hpp_tutorial/tutorial_6 +python -i init.py +``` + +The script loads the FR3 robot, plans a motion from the "ready" position to a +goal configuration using BiRRT, optimizes it with RandomShortcut, and applies +time parameterization using SimpleTimeParameterization (as seen in tutorial 5). + +You can visualize the planned path in the browser viewer: +```python +v = display() +v.loadPath(p_timed) +``` + +## Understanding the trajectory + +After time parameterization, `p_timed` is a continuous function mapping time (in +seconds) to robot configurations. To send it to ros2_control, we need discrete +waypoints. + +Check the trajectory duration: +```python +print(f"Trajectory duration: {p_timed.length():.2f} seconds") +``` + +Sample a configuration at a specific time (e.g., t=1.0s): +```python +q, success = p_timed(1.0) +print(f"Config at t=1.0s: {q[:7]}") # First 7 values are arm joints +``` + +The HPP configuration vector has 9 elements: 7 arm joints + 2 finger joints. + +## Extracting waypoints + +To execute the trajectory, we sample it at regular intervals. Try extracting +waypoints yourself: + +```python +import numpy as np + +n_samples = 50 +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 over {times[-1]:.2f} seconds") +``` + +## Sending the trajectory + +Now send the waypoints to Gazebo via ros2_control: + +```python +from hpp_exec import send_trajectory + +send_trajectory( + configs, times, + joint_names=[f'fr3_joint{i}' for i in range(1, 8)], + joint_indices=list(range(7)), +) +``` + +- `joint_names`: The ROS2 joint names expected by the controller. +- `joint_indices`: Which elements of the HPP config to send (indices 0-6 are the + arm joints; we skip indices 7-8 which are the fingers). + +You should see the robot move in Gazebo and `Trajectory execution complete` in +the terminal. + +## Experiment + +Try different values of `n_samples` (e.g., 10, 100, 200) and observe how it +affects the smoothness of motion in Gazebo. diff --git a/tutorial_6/controllers.yaml b/tutorial_6/controllers.yaml new file mode 100644 index 0000000..1a41ce2 --- /dev/null +++ b/tutorial_6/controllers.yaml @@ -0,0 +1,28 @@ +controller_manager: + ros__parameters: + update_rate: 1000 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_trajectory_controller: + ros__parameters: + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false diff --git a/tutorial_6/init.py b/tutorial_6/init.py new file mode 100644 index 0000000..46b18b4 --- /dev/null +++ b/tutorial_6/init.py @@ -0,0 +1,64 @@ +import numpy as np +from pinocchio import SE3 +from pyhpp.pinocchio import Device, urdf +from pyhpp.core import Problem, BiRRTPlanner, RandomShortcut, SimpleTimeParameterization +from pyhpp_viser import Viewer + + +def display(): + v = Viewer(robot) + v.initViewer(open=False, loadModel=True) + v.setProblem(problem) + return v + + +# use v = display() to create a Viewer instance. + +robot = Device("tuto") + +# Load the Franka FR3 robot +urdf_filename = "package://hpp_tutorial/urdf/fr3.urdf" +srdf_filename = "package://hpp_tutorial/srdf/fr3.srdf" +urdf.loadModel(robot, 0, "fr3", "anchor", urdf_filename, srdf_filename, SE3.Identity()) + +# Configuration: 7 arm joints + 2 finger joints = 9 DOF +# Fingers are kept open (0.035 m) + +# Start: "ready" position (matches Gazebo initial pose) +q_init = np.array([0, -0.785, 0, -2.356, 0, 1.571, 0.785, 0.035, 0.035]) + +# Goal: a different arm configuration +q_goal = np.array([1.0, -1.2, 0.5, -2.0, -0.5, 2.0, 0.3, 0.035, 0.035]) + +# Plan a path +problem = Problem(robot) +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) + +planner = BiRRTPlanner(problem) +planner.maxIterations(2000) + +print("Solving...") +try: + p = planner.solve() +except Exception as exc: + raise RuntimeError( + "BiRRT failed to find a path. Try running the script again " + "(randomized planner) or increase maxIterations." + ) from exc +print(f"Path found, length: {p.length():.3f}") + +# Optimize +optimizer = RandomShortcut(problem) +p_opt = optimizer.optimize(p) +print(f"Optimized path length: {p_opt.length():.3f}") + +# Time-parameterize for execution +stp = SimpleTimeParameterization(problem) +stp.order = 2 +stp.safety = 0.95 +stp.maxAcceleration = 1.0 +p_timed = stp.optimize(p_opt) +print(f"Trajectory duration: {p_timed.length():.3f} s") +print() +print("Path ready. Follow the README to extract waypoints and send to Gazebo.") diff --git a/tutorial_6/launch_sim.py b/tutorial_6/launch_sim.py new file mode 100644 index 0000000..f4e7da5 --- /dev/null +++ b/tutorial_6/launch_sim.py @@ -0,0 +1,124 @@ +"""Launch Franka fr3 in Gazebo with a joint_trajectory_controller for HPP tutorial 6. + +Usage (inside the docker container): + ros2 launch /home/user/devel/install/share/hpp_tutorial/tutorial_6/launch_sim.py +""" + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def get_robot_description_and_controllers(context: LaunchContext, robot_type): + robot_type_str = context.perform_substitution(robot_type) + + tutorial_dir = os.path.join( + get_package_share_directory("hpp_tutorial"), "tutorial_6" + ) + controllers_yaml = os.path.join(tutorial_dir, "controllers.yaml") + + franka_xacro = os.path.join( + get_package_share_directory("franka_gazebo_bringup"), + "urdf", + "franka_arm.gazebo.xacro", + ) + + robot_description_xml = xacro.process_file( + franka_xacro, + mappings={ + "robot_type": robot_type_str, + "hand": "false", + "ros2_control": "true", + "gazebo": "true", + "ee_id": "franka_hand", + }, + ).toxml() + + # Replace the Franka default controllers YAML with our tutorial controllers + franka_controllers = os.path.join( + get_package_share_directory("franka_gazebo_bringup"), + "config", + "franka_gazebo_controllers.yaml", + ) + robot_description_xml = robot_description_xml.replace( + franka_controllers, controllers_yaml + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[{"robot_description": robot_description_xml}], + ) + + # Spawn controllers using ros2_control spawner + spawn_jsb = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + output="screen", + ) + + spawn_jtc = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], + output="screen", + ) + + return [robot_state_publisher, spawn_jsb, spawn_jtc] + + +def generate_launch_description(): + robot_type = LaunchConfiguration("robot_type") + + # Gazebo Sim + os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname( + get_package_share_directory("franka_description") + ) + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ) + ), + launch_arguments={"gz_args": "empty.sdf -r"}.items(), + ) + + spawn = Node( + package="ros_gz_sim", + executable="create", + arguments=["-topic", "/robot_description"], + output="screen", + ) + + bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("robot_type", default_value="fr3"), + gazebo, + OpaqueFunction( + function=get_robot_description_and_controllers, args=[robot_type] + ), + spawn, + bridge, + ] + ) From 1413d26683c214bd04f820a0fa0398bcde8c30b3 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 08:24:26 +0200 Subject: [PATCH 04/36] tutorial 7: WIP pick and place with gripper --- tutorial_7/README.md | 233 ++++++++++++++++++++++++++++++++++++ tutorial_7/controllers.yaml | 44 +++++++ tutorial_7/init.py | 137 +++++++++++++++++++++ tutorial_7/launch_sim.py | 130 ++++++++++++++++++++ 4 files changed, 544 insertions(+) create mode 100644 tutorial_7/README.md create mode 100644 tutorial_7/controllers.yaml create mode 100644 tutorial_7/init.py create mode 100644 tutorial_7/launch_sim.py diff --git a/tutorial_7/README.md b/tutorial_7/README.md new file mode 100644 index 0000000..485c3d4 --- /dev/null +++ b/tutorial_7/README.md @@ -0,0 +1,233 @@ +# Pick and place with gripper + +## Prerequisite + +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. + +## 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). + +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 + +Source the environments, then launch Gazebo with the FR3 including its gripper: + +``` +source /opt/ros/jazzy/setup.bash +source /opt/franka_ws/install/setup.bash +source ~/devel/config.sh +ros2 launch ~/devel/install/share/hpp_tutorial/tutorial_7/launch_sim.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. + +## Terminal 2: Planning and executing + +Open a second terminal: + +``` +docker exec -it hpp bash +``` + +Source the environments and run the tutorial script: + +``` +source /opt/ros/jazzy/setup.bash +source /opt/franka_ws/install/setup.bash +source ~/devel/config.sh +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. + +You can visualize the path in the viewer: +```python +v = display() +v.loadPath(p_timed) +``` + +## Understanding segments and actions + +The key difference from 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. + +### How it works + +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: + +```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) +``` + +### Segments and pre_actions + +A `Segment` represents a slice of the trajectory with optional actions to run +before or after sending that segment: + +```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) +] +``` + +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) + +## Executing with gripper actions + +Define gripper actions as functions returning `True` on success: + +```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])], + [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])], + [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, +) + +# Execute all segments with gripper actions +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 robot approach the box, close its gripper, move the box to +the goal position, open the gripper, and retreat. + +## 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`: + +```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() + +# Usage with segments +gripper = FrankaGripper("fr3") +segments = segments_from_graph( + configs, times, graph, + on_grasp=gripper.close, + on_release=gripper.open, +) +execute_segments(segments, configs, times, ...) +gripper.destroy() +``` diff --git a/tutorial_7/controllers.yaml b/tutorial_7/controllers.yaml new file mode 100644 index 0000000..b1428e0 --- /dev/null +++ b/tutorial_7/controllers.yaml @@ -0,0 +1,44 @@ +controller_manager: + ros__parameters: + update_rate: 1000 + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_trajectory_controller: + ros__parameters: + joints: + - fr3_joint1 + - fr3_joint2 + - fr3_joint3 + - fr3_joint4 + - fr3_joint5 + - fr3_joint6 + - fr3_joint7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + +gripper_controller: + ros__parameters: + joints: + - fr3_finger_joint1 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false diff --git a/tutorial_7/init.py b/tutorial_7/init.py new file mode 100644 index 0000000..01a8168 --- /dev/null +++ b/tutorial_7/init.py @@ -0,0 +1,137 @@ +import numpy as np +from pinocchio import SE3 +from pyhpp.manipulation import Device, Graph, Problem, ManipulationPlanner, urdf +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory +from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint +from pyhpp.core import RandomShortcut, SimpleTimeParameterization +from pyhpp_viser import Viewer + + +def display(): + v = Viewer(robot) + v.initViewer(open=False, loadModel=True) + v.setProblem(problem) + v.setGraph(graph) + return v + + +# Load FR3 robot with gripper +robot = Device("tuto") + +urdf_filename = "package://hpp_tutorial/urdf/fr3.urdf" +srdf_filename = "package://hpp_tutorial/srdf/fr3.srdf" +urdf.loadModel(robot, 0, "fr3", "anchor", urdf_filename, srdf_filename, SE3.Identity()) + +# Load ground (fixed obstacle with contact surface) +urdf_filename = "package://hpp_tutorial/urdf/ground.urdf" +srdf_filename = "package://hpp_tutorial/srdf/ground.srdf" +urdf.loadModel(robot, 0, "ground", "anchor", urdf_filename, srdf_filename, SE3.Identity()) + +# Load box (freeflyer object with handle + contact surface) +urdf_filename = "package://hpp_tutorial/urdf/box.urdf" +srdf_filename = "package://hpp_tutorial/srdf/box.srdf" +urdf.loadModel(robot, 0, "box", "freeflyer", urdf_filename, srdf_filename, SE3.Identity()) + +# Set bounds for the box translation (needed for random sampling) +robot.setJointBounds("box/root_joint", [ + -1.5, 1.5, # x + -1.5, 1.5, # y + -0.2, 1.5, # z + -float("Inf"), float("Inf"), # quaternion + -float("Inf"), float("Inf"), + -float("Inf"), float("Inf"), + -float("Inf"), float("Inf"), +]) + +# Configuration vector: +# indices 0-6: fr3 arm joints (7 DOF) +# indices 7-8: fr3 finger joints (2 DOF) +# indices 9-15: box pose (x, y, z, qx, qy, qz, qw) + +# Build constraint graph for manipulation +problem = Problem(robot) +graph = Graph("robot", robot, problem) +factory = ConstraintGraphFactory(graph) + +graph.maxIterations(40) +graph.errorThreshold(1e-5) + +factory.setGrippers(["fr3/gripper"]) +factory.setObjects(["box"], [["box/handle"]], [["box/surface"]]) +factory.environmentContacts(["ground/surface"]) +factory.generate() + +# Lock finger joints open during planning +cts = ComparisonTypes() +cts[:] = [ComparisonType.EqualToZero] +locked_fingers = [] +for i in range(2): + lj = LockedJoint(robot, f'fr3/fr3_finger_joint{i+1}', np.array([0.035]), cts) + locked_fingers.append(lj) + +graph.addNumericalConstraintsToGraph(locked_fingers) +graph.initialize() + +# Initial config: ready pose, open gripper, box in front at (0.4, -0.2) +q_init = np.array([ + 0, -0.785, 0, -2.356, 0, 1.571, 0.785, # arm (ready) + 0.035, 0.035, # fingers (open) + 0.4, -0.2, 0.0251, 0, 0, 0, 1, # box pose (x,y,z, qx,qy,qz,qw) +]) + +# Goal config: same arm pose, box moved to (0.4, 0.2) +q_goal = np.array([ + 0, -0.785, 0, -2.356, 0, 1.571, 0.785, + 0.035, 0.035, + 0.4, 0.2, 0.0251, 0, 0, 0, 1, +]) + +# Solve the manipulation problem +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) +problem.constraintGraph(graph) + +planner = ManipulationPlanner(problem) +planner.maxIterations(500) + +print("Solving manipulation problem...") +try: + p = planner.solve() +except Exception as exc: + raise RuntimeError( + "ManipulationPlanner failed to find a path. Try running the script " + "again (randomized planner) or increase maxIterations." + ) from exc +print(f"Path found, length: {p.length():.3f}") + +# Optimize +optimizer = RandomShortcut(problem) +p_opt = optimizer.optimize(p) +print(f"Optimized path length: {p_opt.length():.3f}") + +# Time-parameterize for execution +stp = SimpleTimeParameterization(problem) +stp.order = 2 +stp.safety = 0.95 +stp.maxAcceleration = 1.0 +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() +print("To visualize:") +print(" v = display()") +print(" v.loadPath(p_timed)") +print() +print("To execute on Gazebo, see README.md for gripper setup and execution code.") diff --git a/tutorial_7/launch_sim.py b/tutorial_7/launch_sim.py new file mode 100644 index 0000000..108ffc5 --- /dev/null +++ b/tutorial_7/launch_sim.py @@ -0,0 +1,130 @@ +"""Launch Franka FR3 with gripper in Gazebo for HPP tutorial 7 (pick and place). + +Usage (inside the docker container): + ros2 launch /home/user/devel/install/share/hpp_tutorial/tutorial_7/launch_sim.py +""" + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + OpaqueFunction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def get_robot_description_and_controllers(context: LaunchContext, robot_type): + robot_type_str = context.perform_substitution(robot_type) + + tutorial_dir = os.path.join( + get_package_share_directory("hpp_tutorial"), "tutorial_7" + ) + controllers_yaml = os.path.join(tutorial_dir, "controllers.yaml") + + franka_xacro = os.path.join( + get_package_share_directory("franka_gazebo_bringup"), + "urdf", + "franka_arm.gazebo.xacro", + ) + + robot_description_xml = xacro.process_file( + franka_xacro, + mappings={ + "robot_type": robot_type_str, + "hand": "true", + "ros2_control": "true", + "gazebo": "true", + "ee_id": "franka_hand", + }, + ).toxml() + + # Replace the Franka default controllers YAML with our tutorial controllers + franka_controllers = os.path.join( + get_package_share_directory("franka_gazebo_bringup"), + "config", + "franka_gazebo_controllers.yaml", + ) + robot_description_xml = robot_description_xml.replace( + franka_controllers, controllers_yaml + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[{"robot_description": robot_description_xml}], + ) + + spawn_jsb = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster"], + output="screen", + ) + + spawn_jtc = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_trajectory_controller"], + output="screen", + ) + + spawn_gripper = Node( + package="controller_manager", + executable="spawner", + arguments=["gripper_controller"], + output="screen", + ) + + return [robot_state_publisher, spawn_jsb, spawn_jtc, spawn_gripper] + + +def generate_launch_description(): + robot_type = LaunchConfiguration("robot_type") + + # Gazebo Sim + os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname( + get_package_share_directory("franka_description") + ) + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ) + ), + launch_arguments={"gz_args": "empty.sdf -r"}.items(), + ) + + spawn = Node( + package="ros_gz_sim", + executable="create", + arguments=["-topic", "/robot_description"], + output="screen", + ) + + bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument("robot_type", default_value="fr3"), + gazebo, + OpaqueFunction( + function=get_robot_description_and_controllers, args=[robot_type] + ), + spawn, + bridge, + ] + ) From 4676a9d5907ec8baa25a4680ecb9d22a79fd6575 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 14:11:10 +0200 Subject: [PATCH 05/36] [tutorial_1] Fix build: avoid mixing robotpkg and source-built headers MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit robotpkg hpp-core/hpp-manipulation headers were shadowing the devel versions built from source, causing compilation failures in hpp-python. Remove robotpkg packages that overlap with source-built ones from the Dockerfile and add hpp-manipulation-urdf to the Makefile with proper dependency chain, so each package comes from exactly one source. Test build — to be validated. --- tutorial_1/Dockerfile | 10 ++++------ tutorial_1/Makefile | 11 ++++++++--- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 9e19398..a876eed 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -12,12 +12,10 @@ RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robot RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ libgraphviz-dev libqt5svg5-dev pyqt5-dev qtbase5-private-dev \ - robotpkg-py312-pinocchio robotpkg-py312-omniorbpy \ - robotpkg-py312-hpp-manipulation-corba robotpkg-hpp-statistics+doc robotpkg-hpp-util+doc \ - robotpkg-hpp-pinocchio+doc \ - robotpkg-hpp-constraints+doc robotpkg-hpp-core+doc robotpkg-hpp-manipulation+doc \ - robotpkg-hpp-manipulation-urdf+doc robotpkg-qt5-qgv \ - libboost-filesystem1.83-dev libboost-python1.83.0 \ + robotpkg-py312-pinocchio \ + robotpkg-hpp-util+doc robotpkg-hpp-statistics+doc \ + robotpkg-hpp-pinocchio+doc robotpkg-hpp-constraints+doc \ + libboost-filesystem1.83-dev libboost-python1.83-dev \ libboost-thread1.83-dev python3-numpy liburdfdom-dev wget python3.12-venv \ python-is-python3 doxygen \ libasound2-dev libatk1.0-0 libc6 libcairo-gobject2 libcairo2 libdbus-1-3 libdbus-glib-1-2 \ diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index e11e664..dbfbb01 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -46,6 +46,10 @@ hpp-manipulation_branch=devel hpp-manipulation_repository=${HPP_REPO} hpp-manipulation_extra_flags=${HPP_EXTRA_FLAGS} +hpp-manipulation-urdf_branch=devel +hpp-manipulation-urdf_repository=${HPP_REPO} +hpp-manipulation-urdf_extra_flags=${HPP_EXTRA_FLAGS} + hpp-python_branch=devel hpp-python_repository=${HPP_REPO} hpp-python_extra_flags=${HPP_EXTRA_FLAGS} ${PYTHON_FLAGS} @@ -114,10 +118,11 @@ python-venv: jrl-cmakemodules.configure.dep: jrl-cmakemodules.checkout hpp-doc.configure.dep: hpp-doc.checkout -hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install qgv.install hpp-core.configure.dep: hpp-core.checkout jrl-cmakemodules.install -hpp-manipulation.configure.dep: hpp-manipulation.checkout jrl-cmakemodules.install hpp-core.install -hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install hpp-core.install hpp-manipulation.install +hpp-manipulation.configure.dep: hpp-manipulation.checkout hpp-core.install +hpp-manipulation-urdf.configure.dep: hpp-manipulation-urdf.checkout hpp-manipulation.install +hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install hpp-manipulation-urdf.install +hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install qgv.install qgv.configure.dep: qgv.checkout jrl-cmakemodules.install hpp_tutorial.configure.dep: hpp_tutorial.checkout hpp-gepetto-viewer.install \ hpp-python.install From bb5aa298e415f43ebe9e2e28d7eca46c46646784 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 14:26:27 +0200 Subject: [PATCH 06/36] [tutorial_1] Use qgv from robotpkg instead of building from source qgv has no hpp dependency, safe to keep from robotpkg binary. Remove qgv source build from Makefile, add robotpkg-qt5-qgv to Dockerfile. --- tutorial_1/Dockerfile | 2 +- tutorial_1/Makefile | 11 +---------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index a876eed..57663eb 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -12,7 +12,7 @@ RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robot RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ libgraphviz-dev libqt5svg5-dev pyqt5-dev qtbase5-private-dev \ - robotpkg-py312-pinocchio \ + robotpkg-py312-pinocchio robotpkg-qt5-qgv \ robotpkg-hpp-util+doc robotpkg-hpp-statistics+doc \ robotpkg-hpp-pinocchio+doc robotpkg-hpp-constraints+doc \ libboost-filesystem1.83-dev libboost-python1.83-dev \ diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index dbfbb01..b2be1cd 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -73,14 +73,6 @@ hpp-exec_branch=devel hpp-exec_repository=${HPP_REPO} hpp-exec_extra_flags=${PYTHON_FLAGS} -# }}} -################################## -# {{{ Packages for hpp-plot - -qgv_branch=devel -qgv_repository=${HPP_REPO} -qgv_extra_flags=-DBINDINGS_QT5=ON -DBINDINGS_QT4=OFF - # }}} ################################## # {{{ Packages for toppra @@ -122,8 +114,7 @@ hpp-core.configure.dep: hpp-core.checkout jrl-cmakemodules.install hpp-manipulation.configure.dep: hpp-manipulation.checkout hpp-core.install hpp-manipulation-urdf.configure.dep: hpp-manipulation-urdf.checkout hpp-manipulation.install hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install hpp-manipulation-urdf.install -hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install qgv.install -qgv.configure.dep: qgv.checkout jrl-cmakemodules.install +hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install hpp_tutorial.configure.dep: hpp_tutorial.checkout hpp-gepetto-viewer.install \ hpp-python.install hpp-gepetto-viewer.configure.dep: hpp-gepetto-viewer.checkout python-venv hpp-python.install From b8d3727e16a44540a9cc3b4bbfab07d69fd988b3 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 14:57:53 +0200 Subject: [PATCH 07/36] [tutorial_1] Add --system-site-packages to python venv --- tutorial_1/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index b2be1cd..5b456ff 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -99,7 +99,7 @@ python-venv: if [ -f ${INSTALL_HPP_DIR}/pyvenv.cfg ]; then \ echo "python virtual environment already created in $INSTALL_HPP_DIR"; \ else \ - python -m venv ${INSTALL_HPP_DIR}; \ + python -m venv --system-site-packages ${INSTALL_HPP_DIR}; \ ${INSTALL_HPP_DIR}/bin/pip install --prefix=${INSTALL_HPP_DIR} \ "numpy==1.26.4" trimesh pycollada viser; \ fi From c6815d42aa70be98064f8efca1f4b4ffead0ebd1 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 13 Apr 2026 15:41:31 +0200 Subject: [PATCH 08/36] [tutorial_1] Add qttools5-dev dep, fix hpp-plot dependency [tutorial_5] Set constraint graph on problem before optimization --- tutorial_1/Dockerfile | 2 +- tutorial_1/Makefile | 2 +- tutorial_5/init.py | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 57663eb..6293390 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -11,7 +11,7 @@ RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robot RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/wip/packages/debian/pub noble robotpkg" >> /etc/apt/sources.list.d/robotpkg.list RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ - libgraphviz-dev libqt5svg5-dev pyqt5-dev qtbase5-private-dev \ + libgraphviz-dev libqt5svg5-dev pyqt5-dev qtbase5-private-dev qttools5-dev \ robotpkg-py312-pinocchio robotpkg-qt5-qgv \ robotpkg-hpp-util+doc robotpkg-hpp-statistics+doc \ robotpkg-hpp-pinocchio+doc robotpkg-hpp-constraints+doc \ diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index 5b456ff..4cf1de4 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -114,7 +114,7 @@ hpp-core.configure.dep: hpp-core.checkout jrl-cmakemodules.install hpp-manipulation.configure.dep: hpp-manipulation.checkout hpp-core.install hpp-manipulation-urdf.configure.dep: hpp-manipulation-urdf.checkout hpp-manipulation.install hpp-python.configure.dep: hpp-python.checkout jrl-cmakemodules.install hpp-manipulation-urdf.install -hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install +hpp-plot.configure.dep: hpp-plot.checkout jrl-cmakemodules.install hpp-manipulation.install hpp_tutorial.configure.dep: hpp_tutorial.checkout hpp-gepetto-viewer.install \ hpp-python.install hpp-gepetto-viewer.configure.dep: hpp-gepetto-viewer.checkout python-venv hpp-python.install diff --git a/tutorial_5/init.py b/tutorial_5/init.py index 5c57a63..dbed19d 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -99,6 +99,7 @@ def display(): transition, "staubli/joint_6", "plate/root_joint", float("-inf") ) graph.initialize() +problem.constraintGraph(graph) shooter = problem.configurationShooter() From d6d66ac583aff26a99bf6af2952da7fc35ae706c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 14 Apr 2026 12:03:04 +0000 Subject: [PATCH 09/36] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- CMakeLists.txt | 16 ++++++--- tutorial_5/init.py | 9 +++-- tutorial_7/init.py | 88 ++++++++++++++++++++++++++++++++++------------ 3 files changed, 84 insertions(+), 29 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 25b5941..b1e0f9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,11 +95,17 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/materials/textures ) -install(DIRECTORY tutorial_6 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "Dockerfile" EXCLUDE - PATTERN "README.md" EXCLUDE) -install(DIRECTORY tutorial_7 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "README.md" EXCLUDE) +install( + DIRECTORY tutorial_6 + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "Dockerfile" EXCLUDE + PATTERN "README.md" EXCLUDE +) +install( + DIRECTORY tutorial_7 + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "README.md" EXCLUDE +) add_library(${PROJECT_NAME} INTERFACE) install(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) diff --git a/tutorial_5/init.py b/tutorial_5/init.py index dbed19d..c598545 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -61,8 +61,13 @@ def display(): # Load an obstacle between robot and plate to force a non-straight path obstacle_pose = SE3(rotation=np.identity(3), translation=np.array([0.5, -0.2, 1.2])) urdf.loadModel( - robot, 0, "obstacle", "anchor", - "package://hpp_tutorial/urdf/obstacle.urdf", "", obstacle_pose, + robot, + 0, + "obstacle", + "anchor", + "package://hpp_tutorial/urdf/obstacle.urdf", + "", + obstacle_pose, ) # Position the plate in the environment diff --git a/tutorial_7/init.py b/tutorial_7/init.py index 01a8168..c94882b 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -25,23 +25,37 @@ def display(): # Load ground (fixed obstacle with contact surface) urdf_filename = "package://hpp_tutorial/urdf/ground.urdf" srdf_filename = "package://hpp_tutorial/srdf/ground.srdf" -urdf.loadModel(robot, 0, "ground", "anchor", urdf_filename, srdf_filename, SE3.Identity()) +urdf.loadModel( + robot, 0, "ground", "anchor", urdf_filename, srdf_filename, SE3.Identity() +) # Load box (freeflyer object with handle + contact surface) urdf_filename = "package://hpp_tutorial/urdf/box.urdf" srdf_filename = "package://hpp_tutorial/srdf/box.srdf" -urdf.loadModel(robot, 0, "box", "freeflyer", urdf_filename, srdf_filename, SE3.Identity()) +urdf.loadModel( + robot, 0, "box", "freeflyer", urdf_filename, srdf_filename, SE3.Identity() +) # Set bounds for the box translation (needed for random sampling) -robot.setJointBounds("box/root_joint", [ - -1.5, 1.5, # x - -1.5, 1.5, # y - -0.2, 1.5, # z - -float("Inf"), float("Inf"), # quaternion - -float("Inf"), float("Inf"), - -float("Inf"), float("Inf"), - -float("Inf"), float("Inf"), -]) +robot.setJointBounds( + "box/root_joint", + [ + -1.5, + 1.5, # x + -1.5, + 1.5, # y + -0.2, + 1.5, # z + -float("Inf"), + float("Inf"), # quaternion + -float("Inf"), + float("Inf"), + -float("Inf"), + float("Inf"), + -float("Inf"), + float("Inf"), + ], +) # Configuration vector: # indices 0-6: fr3 arm joints (7 DOF) @@ -66,25 +80,55 @@ def display(): cts[:] = [ComparisonType.EqualToZero] locked_fingers = [] for i in range(2): - lj = LockedJoint(robot, f'fr3/fr3_finger_joint{i+1}', np.array([0.035]), cts) + lj = LockedJoint(robot, f"fr3/fr3_finger_joint{i + 1}", np.array([0.035]), cts) locked_fingers.append(lj) graph.addNumericalConstraintsToGraph(locked_fingers) graph.initialize() # Initial config: ready pose, open gripper, box in front at (0.4, -0.2) -q_init = np.array([ - 0, -0.785, 0, -2.356, 0, 1.571, 0.785, # arm (ready) - 0.035, 0.035, # fingers (open) - 0.4, -0.2, 0.0251, 0, 0, 0, 1, # box pose (x,y,z, qx,qy,qz,qw) -]) +q_init = np.array( + [ + 0, + -0.785, + 0, + -2.356, + 0, + 1.571, + 0.785, # arm (ready) + 0.035, + 0.035, # fingers (open) + 0.4, + -0.2, + 0.0251, + 0, + 0, + 0, + 1, # box pose (x,y,z, qx,qy,qz,qw) + ] +) # Goal config: same arm pose, box moved to (0.4, 0.2) -q_goal = np.array([ - 0, -0.785, 0, -2.356, 0, 1.571, 0.785, - 0.035, 0.035, - 0.4, 0.2, 0.0251, 0, 0, 0, 1, -]) +q_goal = np.array( + [ + 0, + -0.785, + 0, + -2.356, + 0, + 1.571, + 0.785, + 0.035, + 0.035, + 0.4, + 0.2, + 0.0251, + 0, + 0, + 0, + 1, + ] +) # Solve the manipulation problem problem.initConfig(q_init) From ac6946230d797b0a81f6e860cd55b6aa3fd5af06 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 14 Apr 2026 14:51:41 +0200 Subject: [PATCH 10/36] CMakeLists: drop install rules for tutorial_6 and tutorial_7 --- CMakeLists.txt | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b1e0f9a..77662f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -95,17 +95,5 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/materials/textures ) -install( - DIRECTORY tutorial_6 - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "Dockerfile" EXCLUDE - PATTERN "README.md" EXCLUDE -) -install( - DIRECTORY tutorial_7 - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "README.md" EXCLUDE -) - add_library(${PROJECT_NAME} INTERFACE) install(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) From 60483e9b05ad0c192243eae46fcd99dbe7d64901 Mon Sep 17 00:00:00 2001 From: Paul Sardin <35958897+psardin001@users.noreply.github.com> Date: Wed, 15 Apr 2026 09:02:36 +0200 Subject: [PATCH 11/36] Use HPP fork of toppra, switch hpp-toppra to devel Co-authored-by: Florent Lamiraux --- tutorial_1/Makefile | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index 4cf1de4..dae9de4 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -84,6 +84,13 @@ toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF hpp-toppra_repository=${HPP_REPO} hpp-toppra_branch=main hpp-toppra_extra_flags= +toppra_repository=${HPP_REPO} +toppra_branch=main +toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF + +hpp-toppra_repository=${HPP_REPO} +hpp-toppra_branch=devel +hpp-toppra_extra_flags= # }}} ################################## From af453e07033684e80bebeee57e613a79aa692527 Mon Sep 17 00:00:00 2001 From: Paul Sardin <35958897+psardin001@users.noreply.github.com> Date: Wed, 15 Apr 2026 09:03:20 +0200 Subject: [PATCH 12/36] Group SimpleTimeParameterization under Time parameterization Co-authored-by: Florent Lamiraux --- tutorial_5/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tutorial_5/README.md b/tutorial_5/README.md index 2abe568..df8fa25 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -36,7 +36,8 @@ print(f"Optimized path length: {p1_opt.length():.3f} (was {p1.length():.3f})") `RandomShortcut` repeatedly picks two random points along the path and tries to connect them with a straight segment. If the shortcut is collision-free and shorter, it replaces the original sub-path. -## SimpleTimeParameterization +## Time parameterization +### SimpleTimeParameterization `SimpleTimeParameterization` computes a polynomial time parameterization that maps real time `t` to the path parameter `s`, while respecting joint velocity and acceleration limits. From 5fa8f80c238c25930bf6f3c0acab7ea40f1420bb Mon Sep 17 00:00:00 2001 From: Paul Sardin <35958897+psardin001@users.noreply.github.com> Date: Wed, 15 Apr 2026 09:03:34 +0200 Subject: [PATCH 13/36] Demote TOPPRA to subsection of Time parameterization Co-authored-by: Florent Lamiraux --- tutorial_5/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial_5/README.md b/tutorial_5/README.md index df8fa25..c8aa41f 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -75,7 +75,7 @@ for acc in [0.25, 0.5, 1.0, 2.0]: print(f" maxAcceleration={acc:.2f} -> duration={p.length():.3f} s") ``` -## TOPPRA +### TOPPRA TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) computes the time-optimal parameterization subject to velocity and torque constraints. Unlike From 7d2bfbc315b69f91d1e5351824d5f31b46d56d71 Mon Sep 17 00:00:00 2001 From: Paul Sardin <35958897+psardin001@users.noreply.github.com> Date: Wed, 15 Apr 2026 09:04:28 +0200 Subject: [PATCH 14/36] Add plotTraj helper for trajectory visualization Co-authored-by: Florent Lamiraux --- tutorial_5/init.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/tutorial_5/init.py b/tutorial_5/init.py index c598545..71d13e8 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -152,3 +152,35 @@ def display(): stp.maxAcceleration = 0.5 p1_stp = stp.optimize(p1_opt) print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)") + + In a separate file `plot.py`, we could add the following function that plots the trajectory, velocity and accelerations. This implies installing `matplotlib` in the docker image + +import matplotlib.pyplot as plt +# Plot a trajectory with respect to times +# +# \param p pyhpp.core.Path instance +# \param rmin, rmax: range of the path to be plot. For example passing rmin=0, rmax=6 will +# plot the 6 first coordinates of the path, +# \param dt: time step, +# \param order: 0 for configuration, 1 for configuration and velocity, 2 for configuration +# velocity and acceleration +def plotTraj(p, rmin, rmax, dt = 0.01, order = 2): + times = dt * np.arange(int(p.length()/dt)) + fig = plt.figure(layout = "constrained") + gs = fig.add_gridspec(order+1,1) + values = np.array([p.eval(t)[0] for t in times]) + ax0 = fig.add_subplot(gs[0,0]) + if order >= 1: + velocities = np.array([p.derivative(t,1) for t in times]) + ax1 = fig.add_subplot(gs[1,0]) + if order >= 2: + accelerations = np.array([p.derivative(t,2) for t in times]) + ax2 = fig.add_subplot(gs[2,0]) + for i in range(rmin, rmax): + ax0.plot(times, values[:,i], label=f"q{i}") + if order >= 1: + ax1.plot(times, velocities[:,i], label=f"q{i}") + if order >= 2: + ax2.plot(times, accelerations[:,i], label=f"q{i}") + fig.show() + From 75a816a1446bd61e867a0a91d48e9e6682f8672c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 15 Apr 2026 07:06:40 +0000 Subject: [PATCH 15/36] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tutorial_5/init.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tutorial_5/init.py b/tutorial_5/init.py index 71d13e8..560e3c5 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -152,7 +152,7 @@ def display(): stp.maxAcceleration = 0.5 p1_stp = stp.optimize(p1_opt) print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)") - + In a separate file `plot.py`, we could add the following function that plots the trajectory, velocity and accelerations. This implies installing `matplotlib` in the docker image import matplotlib.pyplot as plt @@ -183,4 +183,3 @@ def plotTraj(p, rmin, rmax, dt = 0.01, order = 2): if order >= 2: ax2.plot(times, accelerations[:,i], label=f"q{i}") fig.show() - From 5fecac566b3fc3d7d9451ca064748db19caa9563 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 09:09:16 +0200 Subject: [PATCH 16/36] [tutorial_5] Keep init.py to problem setup, move plotTraj to plot.py --- tutorial_5/init.py | 45 --------------------------------------------- tutorial_5/plot.py | 31 +++++++++++++++++++++++++++++++ 2 files changed, 31 insertions(+), 45 deletions(-) create mode 100644 tutorial_5/plot.py diff --git a/tutorial_5/init.py b/tutorial_5/init.py index 560e3c5..9d361d4 100644 --- a/tutorial_5/init.py +++ b/tutorial_5/init.py @@ -1,6 +1,5 @@ import numpy as np from pinocchio import SE3, neutral -from pyhpp.core import RandomShortcut, SimpleTimeParameterization from pyhpp.manipulation import ( Device, Graph, @@ -139,47 +138,3 @@ def display(): print("Failed to plan p1 after 50 attempts. Try running the script again.") else: print(f"Path p1 planned (q_init -> qpg), length: {p1.length():.3f}") - - # Optimize the geometric path before time parameterization - shortcut = RandomShortcut(problem) - p1_opt = shortcut.optimize(p1) - print(f"Optimized path length: {p1_opt.length():.3f} (was {p1.length():.3f})") - - # Time parameterization with low acceleration to visualize the difference - stp = SimpleTimeParameterization(problem) - stp.order = 2 - stp.safety = 0.95 - stp.maxAcceleration = 0.5 - p1_stp = stp.optimize(p1_opt) - print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)") - - In a separate file `plot.py`, we could add the following function that plots the trajectory, velocity and accelerations. This implies installing `matplotlib` in the docker image - -import matplotlib.pyplot as plt -# Plot a trajectory with respect to times -# -# \param p pyhpp.core.Path instance -# \param rmin, rmax: range of the path to be plot. For example passing rmin=0, rmax=6 will -# plot the 6 first coordinates of the path, -# \param dt: time step, -# \param order: 0 for configuration, 1 for configuration and velocity, 2 for configuration -# velocity and acceleration -def plotTraj(p, rmin, rmax, dt = 0.01, order = 2): - times = dt * np.arange(int(p.length()/dt)) - fig = plt.figure(layout = "constrained") - gs = fig.add_gridspec(order+1,1) - values = np.array([p.eval(t)[0] for t in times]) - ax0 = fig.add_subplot(gs[0,0]) - if order >= 1: - velocities = np.array([p.derivative(t,1) for t in times]) - ax1 = fig.add_subplot(gs[1,0]) - if order >= 2: - accelerations = np.array([p.derivative(t,2) for t in times]) - ax2 = fig.add_subplot(gs[2,0]) - for i in range(rmin, rmax): - ax0.plot(times, values[:,i], label=f"q{i}") - if order >= 1: - ax1.plot(times, velocities[:,i], label=f"q{i}") - if order >= 2: - ax2.plot(times, accelerations[:,i], label=f"q{i}") - fig.show() diff --git a/tutorial_5/plot.py b/tutorial_5/plot.py new file mode 100644 index 0000000..2ad01e1 --- /dev/null +++ b/tutorial_5/plot.py @@ -0,0 +1,31 @@ +import matplotlib.pyplot as plt +import numpy as np + + +# Plot a trajectory with respect to times +# +# \param p pyhpp.core.Path instance +# \param rmin, rmax: range of the path to be plot. For example passing rmin=0, rmax=6 will +# plot the 6 first coordinates of the path, +# \param dt: time step, +# \param order: 0 for configuration, 1 for configuration and velocity, 2 for configuration +# velocity and acceleration +def plotTraj(p, rmin, rmax, dt=0.01, order=2): + times = dt * np.arange(int(p.length() / dt)) + fig = plt.figure(layout="constrained") + gs = fig.add_gridspec(order + 1, 1) + values = np.array([p.eval(t)[0] for t in times]) + ax0 = fig.add_subplot(gs[0, 0]) + if order >= 1: + velocities = np.array([p.derivative(t, 1) for t in times]) + ax1 = fig.add_subplot(gs[1, 0]) + if order >= 2: + accelerations = np.array([p.derivative(t, 2) for t in times]) + ax2 = fig.add_subplot(gs[2, 0]) + for i in range(rmin, rmax): + ax0.plot(times, values[:, i], label=f"q{i}") + if order >= 1: + ax1.plot(times, velocities[:, i], label=f"q{i}") + if order >= 2: + ax2.plot(times, accelerations[:, i], label=f"q{i}") + fig.show() From 519b0fdb9431453a48aa99ab41e88f4945dea9e5 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 10:43:08 +0200 Subject: [PATCH 17/36] [tutorial_1] Append to AMENT_PREFIX_PATH/CMAKE_PREFIX_PATH instead of overwriting --- tutorial_1/config.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorial_1/config.sh b/tutorial_1/config.sh index dbdc4f2..f6dff9d 100644 --- a/tutorial_1/config.sh +++ b/tutorial_1/config.sh @@ -9,8 +9,8 @@ export PYTHONPATH=$INSTALL_HPP_DIR/lib/python3.12/site-packages:$INSTALL_PIP_DIR export LD_LIBRARY_PATH=$INSTALL_HPP_DIR/lib:$INSTALL_PIP_DIR/lib:$ROBOTPKG/lib:$INSTALL_HPP_DIR/lib64:$LD_LIBRARY_PATH -export AMENT_PREFIX_PATH=$INSTALL_HPP_DIR:$ROBOTPKG -export CMAKE_PREFIX_PATH=$INSTALL_HPP_DIR:$ROBOTPKG:/usr +export AMENT_PREFIX_PATH=$INSTALL_HPP_DIR:$ROBOTPKG${AMENT_PREFIX_PATH:+:$AMENT_PREFIX_PATH} +export CMAKE_PREFIX_PATH=$INSTALL_HPP_DIR:$ROBOTPKG:/usr${CMAKE_PREFIX_PATH:+:$CMAKE_PREFIX_PATH} if [ -f "${INSTALL_HPP_DIR}/etc/hpp-tools/bashrc" ]; then source "${INSTALL_HPP_DIR}/etc/hpp-tools/bashrc" From b32a4e091ce7fbb9930953a4177e452274cccd74 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 10:53:55 +0200 Subject: [PATCH 18/36] [tutorial_6] Clarify docker build runs on host machine --- tutorial_6/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial_6/README.md b/tutorial_6/README.md index 5a681bc..f2a2d55 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -16,8 +16,8 @@ The base tutorial docker image does not include ROS2 control packages. Build the extended image from the tutorial 6 directory **on the host machine** (not inside the container): +In the host machine, cd into tutorial_6 directory. In a bash terminal, run ``` -cd /src/hpp_tutorial/tutorial_6 docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ -t hpp-tutorial-ros2 . ``` From 7cf50430a1c066bf3fb2301252f7b1995871283a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 10:56:50 +0200 Subject: [PATCH 19/36] [tutorial_6,7] Sort imports (ruff I001) --- tutorial_6/init.py | 2 +- tutorial_6/launch_sim.py | 2 +- tutorial_7/init.py | 4 ++-- tutorial_7/launch_sim.py | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/tutorial_6/init.py b/tutorial_6/init.py index 46b18b4..9007518 100644 --- a/tutorial_6/init.py +++ b/tutorial_6/init.py @@ -1,7 +1,7 @@ import numpy as np from pinocchio import SE3 +from pyhpp.core import BiRRTPlanner, Problem, RandomShortcut, SimpleTimeParameterization from pyhpp.pinocchio import Device, urdf -from pyhpp.core import Problem, BiRRTPlanner, RandomShortcut, SimpleTimeParameterization from pyhpp_viser import Viewer diff --git a/tutorial_6/launch_sim.py b/tutorial_6/launch_sim.py index f4e7da5..bff3457 100644 --- a/tutorial_6/launch_sim.py +++ b/tutorial_6/launch_sim.py @@ -5,8 +5,8 @@ """ import os -import xacro +import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchContext, LaunchDescription from launch.actions import ( diff --git a/tutorial_7/init.py b/tutorial_7/init.py index c94882b..42b2379 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -1,9 +1,9 @@ import numpy as np from pinocchio import SE3 -from pyhpp.manipulation import Device, Graph, Problem, ManipulationPlanner, urdf -from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory 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.constraint_graph_factory import ConstraintGraphFactory from pyhpp_viser import Viewer diff --git a/tutorial_7/launch_sim.py b/tutorial_7/launch_sim.py index 108ffc5..5e904ab 100644 --- a/tutorial_7/launch_sim.py +++ b/tutorial_7/launch_sim.py @@ -5,8 +5,8 @@ """ import os -import xacro +import xacro from ament_index_python.packages import get_package_share_directory from launch import LaunchContext, LaunchDescription from launch.actions import ( From 9317d16930e3a5c76c27edf057bdf4b4922e601a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 11:10:51 +0200 Subject: [PATCH 20/36] [tutorial_1,6] Use default UID for docker user instead of host UID build-args --- tutorial_1/Dockerfile | 6 +----- tutorial_1/README.md | 2 +- tutorial_6/Dockerfile | 2 +- tutorial_6/README.md | 3 +-- 4 files changed, 4 insertions(+), 9 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 6293390..8b7522e 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -1,8 +1,5 @@ FROM ros:jazzy -ARG DOCKER_USER=user -ARG DOCKER_GROUP=user - RUN apt-get update -y \ && DEBIAN_FRONTEND=noninteractive apt-get install -qqy curl RUN mkdir -p /etc/apt/keyrings @@ -29,8 +26,7 @@ RUN dpkg -i midori_11.6-1_amd64.deb RUN rm -f midori_11.6-1_amd64.deb # Add user -RUN addgroup --gid $DOCKER_GROUP user -RUN adduser --uid $DOCKER_USER --gid $DOCKER_GROUP user +RUN adduser user USER user RUN mkdir /home/user/devel diff --git a/tutorial_1/README.md b/tutorial_1/README.md index 4fedfd8..9de1671 100644 --- a/tutorial_1/README.md +++ b/tutorial_1/README.md @@ -26,7 +26,7 @@ wget https://github.com/goastian/midori-desktop/releases/download/v11.6/midori_1 4. Build the docker image ``` -docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp:tuto -f Dockerfile . +docker build -t hpp:tuto -f Dockerfile . ``` 5. Create directory `src` diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 05ab7c1..7a3e82c 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -1,7 +1,7 @@ # 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 -t hpp-tutorial-ros2 . # Run: See tutorial_6/README.md for docker run instructions. FROM hpp:tuto diff --git a/tutorial_6/README.md b/tutorial_6/README.md index f2a2d55..b1944cd 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -18,8 +18,7 @@ the container): In the host machine, cd into tutorial_6 directory. In a bash terminal, run ``` -docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ - -t hpp-tutorial-ros2 . +docker build -t hpp-tutorial-ros2 . ``` Then start the container: From 4f4e9d6f5a178595c456d729e81ef954dfa02573 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 14:34:11 +0200 Subject: [PATCH 21/36] [tutorial_1,6] Match container UID to host; remove Ubuntu 24.04 default user --- tutorial_1/Dockerfile | 10 ++++++++-- tutorial_1/README.md | 2 +- tutorial_6/Dockerfile | 2 +- tutorial_6/README.md | 3 ++- 4 files changed, 12 insertions(+), 5 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 8b7522e..caa099e 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -25,8 +25,14 @@ COPY midori_11.6-1_amd64.deb . RUN dpkg -i midori_11.6-1_amd64.deb RUN rm -f midori_11.6-1_amd64.deb -# Add user -RUN adduser user +# Add user matching the host UID/GID so bind mounts work read/write. +# ros:jazzy (Ubuntu 24.04) ships with a default `ubuntu` user at UID 1000 +# that occupies the slot we need, so remove it first. +ARG DOCKER_USER=1000 +ARG DOCKER_GROUP=1000 +RUN touch /var/mail/ubuntu && chown ubuntu /var/mail/ubuntu && userdel -r ubuntu +RUN addgroup --gid $DOCKER_GROUP user +RUN adduser --uid $DOCKER_USER --gid $DOCKER_GROUP --disabled-password --gecos "" user USER user RUN mkdir /home/user/devel diff --git a/tutorial_1/README.md b/tutorial_1/README.md index 9de1671..4fedfd8 100644 --- a/tutorial_1/README.md +++ b/tutorial_1/README.md @@ -26,7 +26,7 @@ wget https://github.com/goastian/midori-desktop/releases/download/v11.6/midori_1 4. Build the docker image ``` -docker build -t hpp:tuto -f Dockerfile . +docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp:tuto -f Dockerfile . ``` 5. Create directory `src` diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 7a3e82c..05ab7c1 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -1,7 +1,7 @@ # Extends the base tutorial image with ROS2 control + Gazebo + Franka packages # for executing planned trajectories on a simulated Panda/FR3 robot. # -# Build: docker build -t hpp-tutorial-ros2 . +# Build: docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp-tutorial-ros2 . # Run: See tutorial_6/README.md for docker run instructions. FROM hpp:tuto diff --git a/tutorial_6/README.md b/tutorial_6/README.md index b1944cd..f2a2d55 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -18,7 +18,8 @@ the container): In the host machine, cd into tutorial_6 directory. In a bash terminal, run ``` -docker build -t hpp-tutorial-ros2 . +docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ + -t hpp-tutorial-ros2 . ``` Then start the container: From fba54e8eed8353de1097edab98073e21574cfafc Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 15 Apr 2026 14:46:39 +0200 Subject: [PATCH 22/36] [tutorial_1] Remove dead toppra/hpp-toppra assignments --- tutorial_1/Makefile | 7 ------- 1 file changed, 7 deletions(-) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index dae9de4..70a9250 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -77,13 +77,6 @@ hpp-exec_extra_flags=${PYTHON_FLAGS} ################################## # {{{ Packages for toppra -toppra_repository=https://github.com/hungpham2511 -toppra_branch=develop -toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF - -hpp-toppra_repository=${HPP_REPO} -hpp-toppra_branch=main -hpp-toppra_extra_flags= toppra_repository=${HPP_REPO} toppra_branch=main toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF From e1a8614e24cfd4fca5bc0b67210d00db0d149a04 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 16 Apr 2026 07:59:14 +0200 Subject: [PATCH 23/36] [tutorial_5] Switch pipeline to Bezier3 + TOPPRA --- tutorial_5/README.md | 120 ++++++++++++++++++++++++------------------- tutorial_5/plot.py | 2 +- 2 files changed, 67 insertions(+), 55 deletions(-) diff --git a/tutorial_5/README.md b/tutorial_5/README.md index c8aa41f..3b8bf60 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -20,99 +20,111 @@ The path `p1` uses a dimensionless path parameter `s` ranging from `0` to `p1.le parameter does not correspond to real time: the robot has no notion of velocity or acceleration along this path. -## Path optimization +## Visualizing the raw path -The path returned by the planner is collision-free but typically jagged, with unnecessary detours. -Before time-parameterizing, we should optimize the geometric path using shortcutting algorithms: +A small helper in `plot.py` plots joint positions, velocities, and accelerations against the path +parameter. It returns the `matplotlib` figure so you can inspect it interactively or save it: ```python -from pyhpp.core import RandomShortcut +from plot import plotTraj -shortcut = RandomShortcut(problem) -p1_opt = shortcut.optimize(p1) -print(f"Optimized path length: {p1_opt.length():.3f} (was {p1.length():.3f})") +fig = plotTraj(p1, 0, 7, order=2) +fig.savefig("/tmp/p1.png") ``` -`RandomShortcut` repeatedly picks two random points along the path and tries to connect them with a -straight segment. If the shortcut is collision-free and shorter, it replaces the original sub-path. +The raw path `p1` is collision-free but typically jagged, with abrupt direction changes. The +acceleration plot shows large spikes — this is what the rest of this tutorial smooths out. -## Time parameterization -### SimpleTimeParameterization +## Path optimization -`SimpleTimeParameterization` computes a polynomial time parameterization that maps real time `t` to -the path parameter `s`, while respecting joint velocity and acceleration limits. +Before time parameterization we smooth the path with `SplineGradientBased_bezier3`, which fits a +cubic-Bézier spline that minimizes integral squared acceleration while remaining collision-free: ```python -from pyhpp.core import SimpleTimeParameterization - -stp = SimpleTimeParameterization(problem) -stp.order = 2 -stp.safety = 0.95 -stp.maxAcceleration = 0.5 -p1_stp = stp.optimize(p1_opt) -``` +from pyhpp.core import SplineGradientBased_bezier3 -After optimization, `p1_stp.length()` returns the total execution time in seconds: -```python -print(f"Optimized path length: {p1_opt.length():.3f}") -print(f"STP duration: {p1_stp.length():.3f} s") +bezier = SplineGradientBased_bezier3(problem) +p2 = bezier.optimize(p1) +print(f"Smoothed path length: {p2.length():.3f} (was {p1.length():.3f})") ``` -The parameters control the time parameterization: -- `order`: polynomial continuity order. `0` = linear (C0), `1` = cubic (C1, zero velocity at - endpoints), `2` = quintic (C2, zero velocity and acceleration at endpoints). -- `safety`: scaling factor for velocity limits (0.95 = use 95% of max velocity). -- `maxAcceleration`: maximum acceleration per DOF in rad/s². Only used when `order >= 2`. Set to a - negative value to disable. **Use a small value** (e.g. 0.5) to produce slower, more visible - motions — this makes the difference between the raw path `p1` and the parameterized trajectory - `p1_stp` easier to observe. +The resulting path is C²-continuous — a prerequisite for producing a smooth velocity profile in +the next step. -Try different values to see the effect on execution time: -```python -for acc in [0.25, 0.5, 1.0, 2.0]: - stp.maxAcceleration = acc - p = stp.optimize(p1_opt) - print(f" maxAcceleration={acc:.2f} -> duration={p.length():.3f} s") -``` +## Time parameterization ### TOPPRA TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) computes the -time-optimal parameterization subject to velocity and torque constraints. Unlike -`SimpleTimeParameterization`, it accounts for the robot's dynamics. +time-optimal parameterization subject to velocity, acceleration, and torque constraints. Applied +directly to the raw path `p1`, TOPPRA produces endpoint velocity discontinuities and an +acceleration spike at the start. Applied after `SplineGradientBased_bezier3`, the smoothed +spline eliminates these and keeps accelerations bounded throughout. ```python +import numpy as np from pyhpp_toppra import Toppra toppra = Toppra(problem) toppra.velocityScale = 0.5 toppra.effortScale = -1 toppra.N = 100 -p1_toppra = toppra.optimize(p1_opt) -``` - -Compare the results: -```python -print(f"TOPPRA duration: {p1_toppra.length():.3f} s") +toppra.accelerationLimits = np.array(12 * [0.5]) +p3 = toppra.optimize(p2) +print(f"TOPPRA duration: {p3.length():.3f} s") ``` -TOPPRA typically produces shorter execution times than `SimpleTimeParameterization` because it -computes the time-optimal solution rather than a conservative polynomial approximation. - -The parameters are: +Parameters: - `velocityScale`: scaling factor for velocity limits (1.0 = full velocity). Use a small value (e.g. 0.5) for slower, more visible motions. - `effortScale`: scaling factor for torque limits. Set to a negative value to disable torque constraints. **Note**: torque constraints require mass and inertia data in the robot URDF. The - current Staubli model has no dynamics data, so `effortScale` must be set to `-1` (disabled). + Staubli model has no dynamics data, so `effortScale` must be set to `-1` (disabled). +- `accelerationLimits`: per-DOF acceleration cap. The vector size must match the problem's + **velocity** dimension, not the configuration size — here it is `12` (6-DOF Staubli arm + + 6-DOF plate freeflyer). Passing a 7-vector raises + `ValueError: Acceleration limits should be of size 12 ...`. - `N`: minimal number of discretization points along the path. - `interpolationMethod`: `"constant_acceleration"` or `"hermite"`. - `gridpointMethod`: `"param_space"` or `"time_space"`. +Compare the profiles with and without Bézier smoothing: + +```python +fig = plotTraj(toppra.optimize(p1), 0, 7, order=2) +fig.savefig("/tmp/toppra_raw.png") +fig = plotTraj(p3, 0, 7, order=2) +fig.savefig("/tmp/toppra_smoothed.png") +``` + +### SimpleTimeParameterization (simpler alternative) + +When `hpp-toppra` is not available, `SimpleTimeParameterization` computes a polynomial time +parameterization that maps real time `t` to the path parameter `s`, while respecting joint +velocity and acceleration limits. It is less optimal than TOPPRA but needs no extra dependency. + +```python +from pyhpp.core import SimpleTimeParameterization + +stp = SimpleTimeParameterization(problem) +stp.order = 2 +stp.safety = 0.95 +stp.maxAcceleration = 0.5 +p_stp = stp.optimize(p2) +print(f"STP duration: {p_stp.length():.3f} s") +``` + +Parameters: +- `order`: polynomial continuity order. `0` = linear (C0), `1` = cubic (C1, zero velocity at + endpoints), `2` = quintic (C2, zero velocity and acceleration at endpoints). +- `safety`: scaling factor for velocity limits (0.95 = use 95% of max velocity). +- `maxAcceleration`: maximum acceleration per DOF in rad/s². Only used when `order >= 2`. Set to + a negative value to disable. + ## Visualization You can visualize the path in the viewer: ```python v = display() -v.loadPath(p1_stp) +v.loadPath(p3) ``` diff --git a/tutorial_5/plot.py b/tutorial_5/plot.py index 2ad01e1..272d681 100644 --- a/tutorial_5/plot.py +++ b/tutorial_5/plot.py @@ -28,4 +28,4 @@ def plotTraj(p, rmin, rmax, dt=0.01, order=2): ax1.plot(times, velocities[:, i], label=f"q{i}") if order >= 2: ax2.plot(times, accelerations[:, i], label=f"q{i}") - fig.show() + return fig From 37b6b52f58f738d31320ed350d02c9dd4322959a Mon Sep 17 00:00:00 2001 From: Paul Sardin <35958897+psardin001@users.noreply.github.com> Date: Thu, 16 Apr 2026 11:34:22 +0200 Subject: [PATCH 24/36] Update tutorial_1/Dockerfile Co-authored-by: Florent Lamiraux --- tutorial_1/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index caa099e..0536161 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -41,4 +41,4 @@ ENV DEVEL_HPP_DIR=/home/user/devel RUN echo "source $DEVEL_HPP_DIR/config.sh" >> /home/user/.bashrc RUN python3 -m venv /home/user/install-pip -RUN /home/user/install-pip/bin/pip install "numpy==1.26.4" trimesh pycollada viser +RUN /home/user/install-pip/bin/pip install "numpy==1.26.4" trimesh pycollada viser matplotlib From e474120e15f16271e7846f2507005b69491746fe Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 17 Apr 2026 13:03:05 +0200 Subject: [PATCH 25/36] [tutorial_5] Fix matplotlib display. --- tutorial_1/Dockerfile | 5 +++-- tutorial_5/README.md | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 0536161..565cce5 100644 --- a/tutorial_1/Dockerfile +++ b/tutorial_1/Dockerfile @@ -19,7 +19,7 @@ RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ libffi8 libfontconfig1 libfreetype6 libglib2.0-0 libgtk-3-0 libnspr4 libnss3 libpango-1.0-0 \ libstdc++6 libvpx9 apulse \ libx11-6 libx11-xcb1 libxcb-shm0 libxcb1 libxcomposite1 libxdamage1 libxext6 libxfixes3 \ - libxrandr2 libxtst6 zlib1g fontconfig procps + libxrandr2 libxtst6 zlib1g fontconfig procps libxcb-cursor0 COPY midori_11.6-1_amd64.deb . RUN dpkg -i midori_11.6-1_amd64.deb @@ -41,4 +41,5 @@ ENV DEVEL_HPP_DIR=/home/user/devel RUN echo "source $DEVEL_HPP_DIR/config.sh" >> /home/user/.bashrc RUN python3 -m venv /home/user/install-pip -RUN /home/user/install-pip/bin/pip install "numpy==1.26.4" trimesh pycollada viser matplotlib +RUN /home/user/install-pip/bin/pip install "numpy==1.26.4" trimesh pycollada viser \ + matplotlib PyQt6 diff --git a/tutorial_5/README.md b/tutorial_5/README.md index 3b8bf60..e7ac654 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -29,7 +29,7 @@ parameter. It returns the `matplotlib` figure so you can inspect it interactivel from plot import plotTraj fig = plotTraj(p1, 0, 7, order=2) -fig.savefig("/tmp/p1.png") +fig.show() ``` The raw path `p1` is collision-free but typically jagged, with abrupt direction changes. The @@ -92,9 +92,9 @@ Compare the profiles with and without Bézier smoothing: ```python fig = plotTraj(toppra.optimize(p1), 0, 7, order=2) -fig.savefig("/tmp/toppra_raw.png") +fig.show() fig = plotTraj(p3, 0, 7, order=2) -fig.savefig("/tmp/toppra_smoothed.png") +fig.show() ``` ### SimpleTimeParameterization (simpler alternative) From 1b14b110c815d27c54322cbc22536c264a67baf9 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 17 Apr 2026 13:20:43 +0200 Subject: [PATCH 26/36] [tutorial_1] Use tag 0.6.7 of TOPPRA official repository. --- tutorial_1/Makefile | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index 70a9250..b727f8e 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -6,6 +6,7 @@ HPP_REPO=https://github.com/humanoid-path-planner FLORENT_REPO=https://github.com/florent-lamiraux JRL_REPO=https://github.com/jrl-umi3218 +TOPPRA_REPO=https://github.com/hungpham2511 SRC_DIR=${DEVEL_HPP_DIR}/src ifndef INSTALL_HPP_DIR @@ -77,8 +78,8 @@ hpp-exec_extra_flags=${PYTHON_FLAGS} ################################## # {{{ Packages for toppra -toppra_repository=${HPP_REPO} -toppra_branch=main +toppra_repository=${TOPPRA_REPO} +toppra_branch=0.6.7 toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF hpp-toppra_repository=${HPP_REPO} From 7ce99931f6d53803eaba1640677e81fea9ba10fd Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 17 Apr 2026 14:00:42 +0200 Subject: [PATCH 27/36] [tutorial_6] Add script run_docker.sh and rename docker image. --- tutorial_6/README.md | 7 ++----- tutorial_6/run_docker.sh | 45 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 5 deletions(-) create mode 100755 tutorial_6/run_docker.sh diff --git a/tutorial_6/README.md b/tutorial_6/README.md index f2a2d55..18f687f 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -19,16 +19,13 @@ the container): In the host machine, cd into tutorial_6 directory. In a bash terminal, run ``` docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ - -t hpp-tutorial-ros2 . + -t hpp-ros2:tuto . ``` Then start the container: ``` -cd -docker run --gpus all --env DISPLAY --env QT_X11_NO_MITSHM=1 \ - --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw --net host --privileged \ - -v .:/home/user/devel --rm --name hpp -it hpp-tutorial-ros2 +./run_docker.sh ``` Inside the container, build the packages (first time only): diff --git a/tutorial_6/run_docker.sh b/tutorial_6/run_docker.sh new file mode 100755 index 0000000..860879e --- /dev/null +++ b/tutorial_6/run_docker.sh @@ -0,0 +1,45 @@ +#!/bin/bash + +# Variables for forwarding ssh agent into docker container +SSH_AUTH_ARGS="" +if [ ! -z $SSH_AUTH_SOCK ]; then + DOCKER_SSH_AUTH_ARGS="-v $SSH_AUTH_SOCK:/run/host_ssh_auth_sock -e SSH_AUTH_SOCK=/run/host_ssh_auth_sock" +fi + +# Settings required for having nvidia GPU acceleration inside the docker +DOCKER_GPU_ARGS="--env DISPLAY --env QT_X11_NO_MITSHM=1 --volume=/tmp/.X11-unix:/tmp/.X11-unix:rw" + +dpkg -l | grep nvidia-container-toolkit &> /dev/null +HAS_NVIDIA_TOOLKIT=$? +which nvidia-docker > /dev/null +HAS_NVIDIA_DOCKER=$? +if [ $HAS_NVIDIA_TOOLKIT -eq 0 ]; then + docker_version=`docker version --format '{{.Client.Version}}' | cut -d. -f1` + if [ $docker_version -ge 19 ]; then + DOCKER_COMMAND="docker run --gpus all" + else + DOCKER_COMMAND="docker run --runtime=nvidia" + fi +elif [ $HAS_NVIDIA_DOCKER -eq 0 ]; then + DOCKER_COMMAND="nvidia-docker run" +else + echo "Running without nvidia-docker, if you have an NVidia card you may need it"\ + "to have GPU acceleration" + DOCKER_COMMAND="docker run" +fi + +DOCKER_NETWORK_ARGS="--net host" +if [[ "$@" == *"--net "* ]]; then + DOCKER_NETWORK_ARGS="" +fi + +xhost + + +$DOCKER_COMMAND \ +$DOCKER_GPU_ARGS \ +$DOCKER_SSH_AUTH_ARGS \ +$DOCKER_NETWORK_ARGS \ +--privileged \ +-v .:/home/user/devel \ +-v /var/run/docker.sock:/var/run/docker.sock \ +--rm --name hpp -it hpp-ros2:tuto From 83492386a86aea3b75ce9c61a68c6e0edafbbbc2 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 17 Apr 2026 14:39:37 +0200 Subject: [PATCH 28/36] [tutorial_6] Clean up instructions. --- tutorial_1/config.sh | 7 +++++++ tutorial_6/README.md | 6 ++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/tutorial_1/config.sh b/tutorial_1/config.sh index f6dff9d..65f7073 100644 --- a/tutorial_1/config.sh +++ b/tutorial_1/config.sh @@ -2,6 +2,13 @@ export INSTALL_HPP_DIR=$DEVEL_HPP_DIR/install export INSTALL_PIP_DIR=$HOME/install-pip export ROBOTPKG=/opt/openrobots +if [ -f "/opt/ros/jazzy/setup.bash" ]; then + source /opt/ros/jazzy/setup.bash +fi +if [ -f "/opt/franka_ws/install/setup.bash" ]; then + source /opt/franka_ws/install/setup.bash +fi + export PATH=$INSTALL_HPP_DIR/bin:$ROBOTPKG/bin:$PATH export PKG_CONFIG_PATH=$INSTALL_HPP_DIR/lib/pkgconfig/:$ROBOTPKG/lib/pkgconfig diff --git a/tutorial_6/README.md b/tutorial_6/README.md index 18f687f..fe630a4 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -25,7 +25,8 @@ docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` \ Then start the container: ``` -./run_docker.sh +cd ../../.. +./src/hpp_tutorial/tutorial_6/run_docker.sh ``` Inside the container, build the packages (first time only): @@ -42,9 +43,6 @@ Source the ROS2 and Franka environments, then launch the Gazebo simulation with a `joint_trajectory_controller`: ``` -source /opt/ros/jazzy/setup.bash -source /opt/franka_ws/install/setup.bash -source ~/devel/config.sh ros2 launch ~/devel/install/share/hpp_tutorial/tutorial_6/launch_sim.py ``` From 1ff274ec7ea0787ef2dd827437e0f0367ed7627e Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 17 Apr 2026 16:43:25 +0200 Subject: [PATCH 29/36] [tutorial_6] Fix launch command: install tutorial_6 directory and use package syntax tutorial_6/ was not installed by CMake, so ros2 launch could not find launch_sim.py at the install path. Add the missing install rule and switch the launch command to the standard ros2 package syntax. --- CMakeLists.txt | 1 + tutorial_6/README.md | 2 +- tutorial_6/launch_sim.py | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 77662f7..1aea67b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ set(CATKIN_PACKAGE_SHARE_DESTINATION install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY tutorial_6 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install( FILES Media/models/meshes/glasses/__Color_A05_4.png diff --git a/tutorial_6/README.md b/tutorial_6/README.md index fe630a4..3458a42 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -43,7 +43,7 @@ Source the ROS2 and Franka environments, then launch the Gazebo simulation with a `joint_trajectory_controller`: ``` -ros2 launch ~/devel/install/share/hpp_tutorial/tutorial_6/launch_sim.py +ros2 launch hpp_tutorial tutorial_6/launch_sim.py ``` Wait until you see `Configured and activated joint_trajectory_controller` in the diff --git a/tutorial_6/launch_sim.py b/tutorial_6/launch_sim.py index bff3457..ad1adf6 100644 --- a/tutorial_6/launch_sim.py +++ b/tutorial_6/launch_sim.py @@ -1,7 +1,7 @@ """Launch Franka fr3 in Gazebo with a joint_trajectory_controller for HPP tutorial 6. Usage (inside the docker container): - ros2 launch /home/user/devel/install/share/hpp_tutorial/tutorial_6/launch_sim.py + ros2 launch hpp_tutorial tutorial_6/launch_sim.py """ import os From bc7e6611131029f261ca845ae89474473861c87b Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Tue, 21 Apr 2026 16:52:53 +0200 Subject: [PATCH 30/36] [package.xml] Update license. --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index ba56115..5a10aed 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ Florent Lamiraux Florent Lamiraux - LGPL v2 + BSD 2-Clause hpp-corbaserver hpp-manipulation-corba From 4bd00600057530866717dc4eaef75646f9e32b41 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Tue, 21 Apr 2026 16:53:14 +0200 Subject: [PATCH 31/36] [tutorial_6] Fix instructions. --- tutorial_6/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorial_6/README.md b/tutorial_6/README.md index 3458a42..e9cdc65 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -39,7 +39,7 @@ make hpp_tutorial.install ## Terminal 1: Launching the simulation -Source the ROS2 and Franka environments, then launch the Gazebo simulation with +Launch the Gazebo simulation with a `joint_trajectory_controller`: ``` From 76f2e731c4708e53421c737a98330f0a9d16f4a6 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Tue, 21 Apr 2026 17:53:36 +0200 Subject: [PATCH 32/36] [tutorial_6] Move launch file in launch directory. --- CMakeLists.txt | 1 + tutorial_6/launch_sim.py => launch/tutorial_6.launch | 0 2 files changed, 1 insertion(+) rename tutorial_6/launch_sim.py => launch/tutorial_6.launch (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1aea67b..6be340a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,6 +73,7 @@ install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY tutorial_6 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install( FILES Media/models/meshes/glasses/__Color_A05_4.png diff --git a/tutorial_6/launch_sim.py b/launch/tutorial_6.launch similarity index 100% rename from tutorial_6/launch_sim.py rename to launch/tutorial_6.launch From c69fbc61c18ea7b5a6726131c78af5efbc48b85f Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Apr 2026 18:04:13 +0200 Subject: [PATCH 33/36] [tutorial_6] Add missing fr3.urdf and fr3.srdf --- srdf/fr3.srdf | 92 ++++++++++ urdf/fr3.urdf | 452 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 544 insertions(+) create mode 100644 srdf/fr3.srdf create mode 100644 urdf/fr3.urdf diff --git a/srdf/fr3.srdf b/srdf/fr3.srdf new file mode 100644 index 0000000..d30023c --- /dev/null +++ b/srdf/fr3.srdf @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/fr3.urdf b/urdf/fr3.urdf new file mode 100644 index 0000000..4b819a2 --- /dev/null +++ b/urdf/fr3.urdf @@ -0,0 +1,452 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From b0620b4ecf11cd34e02572f16d4fcfe8217ef5f6 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 22 Apr 2026 14:45:49 +0200 Subject: [PATCH 34/36] [tutorial_6] Fix instructions. --- CMakeLists.txt | 11 ++++++++++- ...{tutorial_6.launch => tutorial_6_launch.py} | 0 tutorial_6/README.md | 18 ++++++++++-------- 3 files changed, 20 insertions(+), 9 deletions(-) rename launch/{tutorial_6.launch => tutorial_6_launch.py} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6be340a..f79a665 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,9 +72,18 @@ set(CATKIN_PACKAGE_SHARE_DESTINATION install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -install(DIRECTORY tutorial_6 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install( + FILES tutorial_6/controllers.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/tutorial_6 +) + +install( + FILES tutorial_7/controllers.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/tutorial_7 +) + install( FILES Media/models/meshes/glasses/__Color_A05_4.png DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/models/meshes/glasses diff --git a/launch/tutorial_6.launch b/launch/tutorial_6_launch.py similarity index 100% rename from launch/tutorial_6.launch rename to launch/tutorial_6_launch.py diff --git a/tutorial_6/README.md b/tutorial_6/README.md index e9cdc65..c3a73fa 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -43,7 +43,7 @@ Launch the Gazebo simulation with a `joint_trajectory_controller`: ``` -ros2 launch hpp_tutorial tutorial_6/launch_sim.py +ros2 launch hpp_tutorial tutorial_6_launch.py ``` Wait until you see `Configured and activated joint_trajectory_controller` in the @@ -60,9 +60,6 @@ docker exec -it hpp bash Source the environments and run the tutorial script: ``` -source /opt/ros/jazzy/setup.bash -source /opt/franka_ws/install/setup.bash -source ~/devel/config.sh cd ~/devel/src/hpp_tutorial/tutorial_6 python -i init.py ``` @@ -91,7 +88,7 @@ print(f"Trajectory duration: {p_timed.length():.2f} seconds") Sample a configuration at a specific time (e.g., t=1.0s): ```python q, success = p_timed(1.0) -print(f"Config at t=1.0s: {q[:7]}") # First 7 values are arm joints +print(f"Config at t=1.0s: {q}") # First 7 values are arm joints ``` The HPP configuration vector has 9 elements: 7 arm joints + 2 finger joints. @@ -111,9 +108,9 @@ 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) + assert(success) + configs.append(np.array(q)) + times.append(t) print(f"Extracted {len(configs)} waypoints over {times[-1]:.2f} seconds") ``` @@ -143,3 +140,8 @@ the terminal. Try different values of `n_samples` (e.g., 10, 100, 200) and observe how it affects the smoothness of motion in Gazebo. + +You can also play the reverse motion using +``` +p_reversed = p_timed.reverse() +``` From 348526a7b0f77535c68015ff3046cbde29e040d7 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 22 Apr 2026 15:16:43 +0200 Subject: [PATCH 35/36] [tutorial_7] Fix instructions. --- .../tutorial_7_launch.py | 0 tutorial_7/README.md | 20 +++---------------- 2 files changed, 3 insertions(+), 17 deletions(-) rename tutorial_7/launch_sim.py => launch/tutorial_7_launch.py (100%) diff --git a/tutorial_7/launch_sim.py b/launch/tutorial_7_launch.py similarity index 100% rename from tutorial_7/launch_sim.py rename to launch/tutorial_7_launch.py diff --git a/tutorial_7/README.md b/tutorial_7/README.md index 485c3d4..c86ba39 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -16,23 +16,12 @@ gripper open/close actions between trajectory segments. 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). -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 -Source the environments, then launch Gazebo with the FR3 including its gripper: +Launch Gazebo with the FR3 including its gripper: ``` -source /opt/ros/jazzy/setup.bash -source /opt/franka_ws/install/setup.bash -source ~/devel/config.sh -ros2 launch ~/devel/install/share/hpp_tutorial/tutorial_7/launch_sim.py +ros2 launch hpp_tutorial tutorial_7_launch.py ``` Wait until you see `Configured and activated gripper_controller` in the output. @@ -52,9 +41,6 @@ docker exec -it hpp bash Source the environments and run the tutorial script: ``` -source /opt/ros/jazzy/setup.bash -source /opt/franka_ws/install/setup.bash -source ~/devel/config.sh cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` @@ -72,7 +58,7 @@ v.loadPath(p_timed) ## Understanding segments and actions -The key difference from tutorial 6 is that the trajectory has multiple 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. From 5642ddbe2a07a3f7362a9ce5ddc8d350a4d24001 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 22 Apr 2026 15:21:16 +0200 Subject: [PATCH 36/36] [tutorial_1] Install hpp_tutorial from official repo. --- tutorial_1/Makefile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index b727f8e..4bb0891 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -4,7 +4,6 @@ # HPP_REPO=https://github.com/humanoid-path-planner -FLORENT_REPO=https://github.com/florent-lamiraux JRL_REPO=https://github.com/jrl-umi3218 TOPPRA_REPO=https://github.com/hungpham2511 @@ -59,7 +58,7 @@ hpp-doc_branch=devel hpp-doc_repository=${HPP_REPO} hpp_tutorial_branch=devel -hpp_tutorial_repository=${FLORENT_REPO} +hpp_tutorial_repository=${HPP_REPO} hpp_tutorial_extra_flags=${HPP_EXTRA_FLAGS} ${PYTHON_FLAGS} hpp-gepetto-viewer_branch=devel