Tutorial fixes and additions: build deps, path optimization, Gazebo execution by psardin001 · Pull Request #165 · humanoid-path-planner/hpp_tutorial

psardin001 · 2026-04-14T12:02:33Z

tutorial_1 : build fixes

Add qttools5-dev dependency and fix hpp-plot dependency chain
Add --system-site-packages to the python venv
Use qgv from robotpkg instead of building from source
Avoid mixing robotpkg and source-built headers (was causing
compilation failures in hpp-python)

tutorial_5 : path optimization

New tutorial: path optimization and time parameterization
Fix obstacle position, add path optimization and slow acceleration
Set constraint graph on problem before optimization

tutorial_6 : Gazebo execution (new)

Execute HPP-generated motions on a simulated robot via hpp_exec

…celeration - 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_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

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.

qgv has no hpp dependency, safe to keep from robotpkg binary. Remove qgv source build from Makefile, add robotpkg-qt5-qgv to Dockerfile.

[tutorial_5] Set constraint graph on problem before optimization

for more information, see https://pre-commit.ci

florent-lamiraux · 2026-04-14T14:16:50Z

tutorial_1/Makefile

 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=



Suggested change

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=

florent-lamiraux · 2026-04-14T14:19:11Z

tutorial_5/README.md

+`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


Suggested change

## SimpleTimeParameterization

## Time parameterization

### SimpleTimeParameterization

florent-lamiraux · 2026-04-14T14:19:26Z

tutorial_5/README.md

+    print(f"  maxAcceleration={acc:.2f} -> duration={p.length():.3f} s")
+```
+
+## TOPPRA


Suggested change

## TOPPRA

### TOPPRA

florent-lamiraux · 2026-04-14T14:21:56Z

tutorial_5/README.md

+```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})")


This portion of code is already in init.py. For consistency with other tutorials, it would be better to

remove it from the script or

move it in a section "Explaining the code".

florent-lamiraux · 2026-04-14T14:42:55Z

tutorial_5/README.md

+TOPPRA typically produces shorter execution times than `SimpleTimeParameterization` because it
+computes the time-optimal solution rather than a conservative polynomial approximation.


Actually, TOPPRA does not stop at waypoint, making the velocity discontinuous. We need to better understand how those algorithms work.
Probably, TOPPRA should be used after SplineGradientBased_bezier3.

florent-lamiraux · 2026-04-14T14:46:27Z

tutorial_5/init.py

+    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)")


Suggested change

print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)")

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()

psardin001 and others added 10 commits April 14, 2026 13:55

Add tutorial 5: path optimization and time parameterization

2e32e6c

tutorial 7: WIP pick and place with gripper

1413d26

[tutorial_1] Use qgv from robotpkg instead of building from source

bb5aa29

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] Add --system-site-packages to python venv

b8d3727

[tutorial_1] Add qttools5-dev dep, fix hpp-plot dependency

c6815d4

[tutorial_5] Set constraint graph on problem before optimization

[pre-commit.ci] auto fixes from pre-commit.com hooks

d6d66ac

for more information, see https://pre-commit.ci

CMakeLists: drop install rules for tutorial_6 and tutorial_7

ac69462

florent-lamiraux requested changes Apr 14, 2026

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Tutorial fixes and additions: build deps, path optimization, Gazebo execution#165

Tutorial fixes and additions: build deps, path optimization, Gazebo execution#165
psardin001 wants to merge 10 commits intohumanoid-path-planner:develfrom
psardin001:devel

psardin001 commented Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

florent-lamiraux Apr 14, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

	## SimpleTimeParameterization
	## Time parameterization
	### SimpleTimeParameterization

		TOPPRA typically produces shorter execution times than `SimpleTimeParameterization` because it
		computes the time-optimal solution rather than a conservative polynomial approximation.

-    print(f"STP duration: {p1_stp.length():.3f} s (maxAcceleration=0.5)")
+    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()

Conversation

psardin001 commented Apr 14, 2026

tutorial_6 : Gazebo execution (new)

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

florent-lamiraux Apr 14, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants