diff --git a/CMakeLists.txt b/CMakeLists.txt index 77662f7..f79a665 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,17 @@ 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 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 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/launch/tutorial_6_launch.py b/launch/tutorial_6_launch.py new file mode 100644 index 0000000..ad1adf6 --- /dev/null +++ b/launch/tutorial_6_launch.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 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, + ] + ) diff --git a/launch/tutorial_7_launch.py b/launch/tutorial_7_launch.py new file mode 100644 index 0000000..5e904ab --- /dev/null +++ b/launch/tutorial_7_launch.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, + ] + ) 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 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/tutorial_1/Dockerfile b/tutorial_1/Dockerfile index 9e19398..565cce5 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 @@ -11,28 +8,31 @@ 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 \ - 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 \ + 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 \ + 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 \ 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 RUN rm -f midori_11.6-1_amd64.deb -# Add 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 user +RUN adduser --uid $DOCKER_USER --gid $DOCKER_GROUP --disabled-password --gecos "" user USER user RUN mkdir /home/user/devel @@ -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 +RUN /home/user/install-pip/bin/pip install "numpy==1.26.4" trimesh pycollada viser \ + matplotlib PyQt6 diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile index e754a2b..4bb0891 100644 --- a/tutorial_1/Makefile +++ b/tutorial_1/Makefile @@ -4,8 +4,8 @@ # 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 @@ -38,6 +38,18 @@ 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-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} @@ -46,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 @@ -57,24 +69,20 @@ hpp-plot_branch=devel hpp-plot_repository=${HPP_REPO} hpp-plot_extra_flags= -DINSTALL_DOCUMENTATION=OFF -# }}} -################################## -# {{{ Packages for hpp-plot - -qgv_branch=devel -qgv_repository=${HPP_REPO} -qgv_extra_flags=-DBINDINGS_QT5=ON -DBINDINGS_QT4=OFF +hpp-exec_branch=devel +hpp-exec_repository=${HPP_REPO} +hpp-exec_extra_flags=${PYTHON_FLAGS} # }}} ################################## # {{{ Packages for toppra -toppra_repository=https://github.com/hungpham2511 -toppra_branch=develop +toppra_repository=${TOPPRA_REPO} +toppra_branch=0.6.7 toppra_extra_flags= -DBUILD_TESTS=OFF -DPYTHON_BINDINGS=OFF hpp-toppra_repository=${HPP_REPO} -hpp-toppra_branch=main +hpp-toppra_branch=devel hpp-toppra_extra_flags= # }}} @@ -91,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 @@ -102,14 +110,17 @@ 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 -qgv.configure.dep: qgv.checkout jrl-cmakemodules.install +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-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 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/config.sh b/tutorial_1/config.sh index dbdc4f2..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 @@ -9,8 +16,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" diff --git a/tutorial_1/run_docker.sh b/tutorial_1/run_docker.sh old mode 100644 new mode 100755 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..e7ac654 100644 --- a/tutorial_5/README.md +++ b/tutorial_5/README.md @@ -1,3 +1,130 @@ # 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. + +## Visualizing the raw path + +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 plot import plotTraj + +fig = plotTraj(p1, 0, 7, order=2) +fig.show() +``` + +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. + +## Path optimization + +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 SplineGradientBased_bezier3 + +bezier = SplineGradientBased_bezier3(problem) +p2 = bezier.optimize(p1) +print(f"Smoothed path length: {p2.length():.3f} (was {p1.length():.3f})") +``` + +The resulting path is C²-continuous — a prerequisite for producing a smooth velocity profile in +the next step. + +## Time parameterization + +### TOPPRA + +TOPPRA (Time-Optimal Path Parameterization based on Reachability Analysis) computes the +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 +toppra.accelerationLimits = np.array(12 * [0.5]) +p3 = toppra.optimize(p2) +print(f"TOPPRA duration: {p3.length():.3f} s") +``` + +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 + 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.show() +fig = plotTraj(p3, 0, 7, order=2) +fig.show() +``` + +### 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(p3) +``` diff --git a/tutorial_5/init.py b/tutorial_5/init.py new file mode 100644 index 0000000..9d361d4 --- /dev/null +++ b/tutorial_5/init.py @@ -0,0 +1,140 @@ +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.2, 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() +problem.constraintGraph(graph) + +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/tutorial_5/plot.py b/tutorial_5/plot.py new file mode 100644 index 0000000..272d681 --- /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}") + return fig 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..c3a73fa 100644 --- a/tutorial_6/README.md +++ b/tutorial_6/README.md @@ -1,3 +1,147 @@ -# 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): + +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-ros2:tuto . +``` + +Then start the container: + +``` +cd ../../.. +./src/hpp_tutorial/tutorial_6/run_docker.sh +``` + +Inside the container, build the packages (first time only): + +``` +cd ~/devel/src +make hpp-exec.install +make hpp_tutorial.install +``` + +## Terminal 1: Launching the simulation + +Launch the Gazebo simulation with +a `joint_trajectory_controller`: + +``` +ros2 launch hpp_tutorial tutorial_6_launch.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: + +``` +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}") # 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) + assert(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. + +You can also play the reverse motion using +``` +p_reversed = p_timed.reverse() +``` 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..9007518 --- /dev/null +++ b/tutorial_6/init.py @@ -0,0 +1,64 @@ +import numpy as np +from pinocchio import SE3 +from pyhpp.core import BiRRTPlanner, Problem, RandomShortcut, SimpleTimeParameterization +from pyhpp.pinocchio import Device, urdf +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/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 diff --git a/tutorial_7/README.md b/tutorial_7/README.md new file mode 100644 index 0000000..c86ba39 --- /dev/null +++ b/tutorial_7/README.md @@ -0,0 +1,219 @@ +# 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). + +## Terminal 1: Launching the simulation + +Launch Gazebo with the FR3 including its gripper: + +``` +ros2 launch hpp_tutorial tutorial_7_launch.py +``` + +Wait until you see `Configured and activated gripper_controller` in the output. + +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: + +``` +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 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. + +### 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..42b2379 --- /dev/null +++ b/tutorial_7/init.py @@ -0,0 +1,181 @@ +import numpy as np +from pinocchio import SE3 +from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint +from pyhpp.core import RandomShortcut, SimpleTimeParameterization +from pyhpp.manipulation import Device, Graph, ManipulationPlanner, Problem, urdf +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory +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/urdf/fr3.urdf b/urdf/fr3.urdf new file mode 100644 index 0000000..4b819a2 --- /dev/null +++ b/urdf/fr3.urdf @@ -0,0 +1,452 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + +