diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 4ad7bd5..8ac6b8c 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -1,6 +1,6 @@ version: 2 updates: -- package-ecosystem: "github-actions" - directory: "/" - schedule: + - package-ecosystem: "github-actions" + directory: "/" + schedule: interval: "monthly" diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml index edbfe30..c121b43 100644 --- a/.github/workflows/nix.yml +++ b/.github/workflows/nix.yml @@ -16,12 +16,13 @@ jobs: nix: runs-on: "${{ matrix.os }}-latest" strategy: + fail-fast: false matrix: os: [ubuntu] steps: - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - uses: cachix/install-nix-action@v31 - - uses: cachix/cachix-action@v16 + - uses: cachix/cachix-action@v17 with: name: gepetto authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml index 20d06aa..93ec37b 100644 --- a/.github/workflows/update-flake-lock.yml +++ b/.github/workflows/update-flake-lock.yml @@ -1,10 +1,8 @@ name: update-flake-lock - on: workflow_dispatch: schedule: - cron: '0 18 1 * *' - jobs: update-flake-inputs: runs-on: ubuntu-slim @@ -14,7 +12,7 @@ jobs: steps: - name: Generate GitHub App Token id: app-token - uses: actions/create-github-app-token@v2 + uses: actions/create-github-app-token@v3 with: app-id: ${{ secrets.GEPETTO_NIX_APP_ID }} private-key: ${{ secrets.GEPETTO_NIX_APP_PRIVATE_KEY }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 3c0cdb0..7dd0581 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,44 +1,40 @@ ci: autoupdate_schedule: quarterly repos: -- repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.5 - hooks: - - id: ruff - args: - - --fix - - --exit-non-zero-on-fix - - id: ruff-format -- repo: https://github.com/cheshirekow/cmake-format-precommit - rev: v0.6.13 - hooks: - - id: cmake-format -- repo: https://github.com/pappasam/toml-sort - rev: v0.24.3 - hooks: - - id: toml-sort-fix - exclude: poetry.lock -- repo: https://github.com/pre-commit/mirrors-clang-format - rev: v22.1.0 - hooks: - - id: clang-format - args: - - --style=Google -- repo: https://github.com/pre-commit/pre-commit-hooks - rev: v6.0.0 - hooks: - - id: check-added-large-files - - id: check-ast - - id: check-executables-have-shebangs - - id: check-json - - id: check-merge-conflict - - id: check-symlinks - - id: check-toml - - id: check-yaml - - id: debug-statements - - id: destroyed-symlinks - - id: detect-private-key - - id: end-of-file-fixer - - id: fix-byte-order-marker - - id: mixed-line-ending - - id: trailing-whitespace + - repo: https://github.com/BlankSpruce/gersemi-pre-commit + rev: 0.27.1 + hooks: + - id: gersemi + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.15.10 + hooks: + - id: ruff-check + - id: ruff-format + - repo: https://github.com/pappasam/toml-sort + rev: v0.24.4 + hooks: + - id: toml-sort-fix + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v22.1.3 + hooks: + - id: clang-format + args: + - --style=Google + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v6.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-executables-have-shebangs + - id: check-json + - id: check-merge-conflict + - id: check-symlinks + - id: check-toml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker + - id: mixed-line-ending + - id: trailing-whitespace diff --git a/CMakeLists.txt b/CMakeLists.txt index 557d885..77662f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,24 +23,32 @@ set(CXX_DISABLE_WERROR TRUE) # Check if the submodule cmake have been initialized set(JRL_CMAKE_MODULES "${CMAKE_CURRENT_LIST_DIR}/cmake") if(EXISTS "${JRL_CMAKE_MODULES}/base.cmake") - message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") + message(STATUS "JRL cmakemodules found in 'cmake/' git submodule") else() - find_package(jrl-cmakemodules QUIET CONFIG) - if(jrl-cmakemodules_FOUND) - get_property( - JRL_CMAKE_MODULES - TARGET jrl-cmakemodules::jrl-cmakemodules - PROPERTY INTERFACE_INCLUDE_DIRECTORIES) - message(STATUS "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}") - else() - message(STATUS "JRL cmakemodules not found. Let's fetch it.") - include(FetchContent) - FetchContent_Declare( - "jrl-cmakemodules" - GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git") - FetchContent_MakeAvailable("jrl-cmakemodules") - FetchContent_GetProperties("jrl-cmakemodules" SOURCE_DIR JRL_CMAKE_MODULES) - endif() + find_package(jrl-cmakemodules QUIET CONFIG) + if(jrl-cmakemodules_FOUND) + get_property( + JRL_CMAKE_MODULES + TARGET jrl-cmakemodules::jrl-cmakemodules + PROPERTY INTERFACE_INCLUDE_DIRECTORIES + ) + message( + STATUS + "JRL cmakemodules found on system at ${JRL_CMAKE_MODULES}" + ) + else() + message(STATUS "JRL cmakemodules not found. Let's fetch it.") + include(FetchContent) + FetchContent_Declare( + "jrl-cmakemodules" + GIT_REPOSITORY "https://github.com/jrl-umi3218/jrl-cmakemodules.git" + ) + FetchContent_MakeAvailable("jrl-cmakemodules") + FetchContent_GetProperties( + "jrl-cmakemodules" + SOURCE_DIR JRL_CMAKE_MODULES + ) + endif() endif() include("${JRL_CMAKE_MODULES}/hpp.cmake") @@ -49,50 +57,30 @@ include("${JRL_CMAKE_MODULES}/python.cmake") compute_project_args(PROJECT_ARGS LANGUAGES CXX) project(${PROJECT_NAME} ${PROJECT_ARGS}) -add_project_dependency("hpp-manipulation-corba" REQUIRED) +add_project_dependency("hpp-manipulation" REQUIRED) add_project_dependency("example-robot-data" REQUIRED) add_project_dependency("hpp-gepetto-viewer" REQUIRED) if(NOT FINDPYTHON_ALREADY_CALLED) - findpython() + findpython() endif() -add_subdirectory(src) - set(CATKIN_PACKAGE_SHARE_DESTINATION - ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME}) + ${CMAKE_INSTALL_DATAROOTDIR}/${PROJECT_NAME} +) -install( - FILES urdf/box.urdf - urdf/cup.urdf - urdf/door.urdf - urdf/kitchen_area.urdf - urdf/kitchen_area_obstacle.urdf - urdf/box_color.urdf - urdf/rod.urdf - urdf/table.urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf) -install(FILES srdf/box.srdf srdf/cup.srdf srdf/kitchen_area.srdf srdf/rod.srdf - srdf/door.srdf srdf/table.srdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf) -install(FILES meshes/box.dae - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes) -install(FILES src/hpp/corbaserver/pr2/robot.py - src/hpp/corbaserver/pr2/__init__.py - DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/pr2) -install(FILES src/hpp/corbaserver/rod/robot.py - src/hpp/corbaserver/rod/__init__.py - DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rod) -install(FILES src/hpp/corbaserver/manipulation/pr2/robot.py - src/hpp/corbaserver/manipulation/pr2/__init__.py - DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/manipulation/pr2) +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( - FILES Media/models/meshes/glasses/__Color_A05_4.png - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/models/meshes/glasses) + FILES Media/models/meshes/glasses/__Color_A05_4.png + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/models/meshes/glasses +) install( - FILES Media/materials/textures/ganz_rechts_neben_kuecheninsel.png + FILES + Media/materials/textures/ganz_rechts_neben_kuecheninsel.png Media/materials/textures/hinter_shopschraenken_an_saeule.png Media/materials/textures/wallpaper_ai_unihb.png Media/materials/textures/stove.png @@ -104,23 +92,8 @@ install( Media/materials/textures/wallpaper.png Media/materials/textures/sink.png Media/materials/textures/mittig_neben_roboterplakat.png - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/materials/textures) - -# Installation for documentation -if(INSTALL_DOCUMENTATION) - install( - FILES urdf/box.urdf - DESTINATION - ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html/urdf) - install( - FILES script/tutorial_1.py script/tutorial_2.py script/tutorial_3.py - script/tutorial_4.py - DESTINATION - ${CMAKE_INSTALL_DATAROOTDIR}/doc/${PROJECT_NAME}/doxygen-html/script) -endif(INSTALL_DOCUMENTATION) + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/Media/materials/textures +) add_library(${PROJECT_NAME} INTERFACE) -install( - TARGETS ${PROJECT_NAME} - EXPORT ${TARGETS_EXPORT_NAME} - DESTINATION lib) +install(TARGETS ${PROJECT_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib) diff --git a/README.md b/README.md index 3394cdb..45bfe9d 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,15 @@ # Tutorial for humanoid path planner platform. +This package provides some tutorials to learn how to use [Humanoid Path Planner](https://humanoid-path-planner.github.io/hpp-doc) software. +The various tutorials are: + +1. [tutorial 1](./tutorial_1/README.md) How to install and run the software. +2. [tutorial 2](./tutorial_2/README.md) How to define and solve a simple pick and place problem. +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. + [![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/) [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black) diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in index 1a2941a..bbbd040 100644 --- a/doc/Doxyfile.extra.in +++ b/doc/Doxyfile.extra.in @@ -3,7 +3,7 @@ TAGFILES = @HPP_CORE_DOXYGENDOCDIR@/hpp-core.doxytag=@HPP_CORE_DOXYGENDOCDIR@ \ @HPP_MANIPULATION_DOXYGENDOCDIR@/hpp-manipulation.doxytag=@HPP_MANIPULATION_DOXYGENDOCDIR@ \ @HPP_MANIPULATION_CORBA_DOXYGENDOCDIR@/hpp-manipulation-corba.doxytag=@HPP_MANIPULATION_CORBA_DOXYGENDOCDIR@ \ -INPUT=@CMAKE_SOURCE_DIR@/include @CMAKE_SOURCE_DIR@/src/hpp/corbaserver/pr2/robot.py +INPUT=@CMAKE_SOURCE_DIR@/include IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/figures USE_MATHJAX= YES diff --git a/flake.lock b/flake.lock index a1c93df..b511429 100644 --- a/flake.lock +++ b/flake.lock @@ -1,15 +1,54 @@ { "nodes": { + "flake-compat": { + "flake": false, + "locked": { + "lastModified": 1767039857, + "narHash": "sha256-vNpUSpF5Nuw8xvDLj2KCwwksIbjua2LZCqhV1LNRDns=", + "owner": "edolstra", + "repo": "flake-compat", + "rev": "5edf11c44bc78a0d334f6334cdaf7d60d732daab", + "type": "github" + }, + "original": { + "owner": "edolstra", + "repo": "flake-compat", + "type": "github" + } + }, "flake-parts": { "inputs": { "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1769996383, - "narHash": "sha256-AnYjnFWgS49RlqX7LrC4uA+sCCDBj0Ry/WOJ5XWAsa0=", + "lastModified": 1775087534, + "narHash": "sha256-91qqW8lhL7TLwgQWijoGBbiD4t7/q75KTi8NxjVmSmA=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "57928607ea566b5db3ad13af0e57e921e6b12381", + "rev": "3107b77cd68437b9a76194f0f7f9c55f2329ca5b", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "flake-parts", + "type": "github" + } + }, + "flake-parts_2": { + "inputs": { + "nixpkgs-lib": [ + "gepetto", + "system-manager", + "userborn", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1768135262, + "narHash": "sha256-PVvu7OqHBGWN16zSi6tEmPwwHQ4rLPU9Plvs8/1TUBY=", + "owner": "hercules-ci", + "repo": "flake-parts", + "rev": "80daad04eddbbf5a4d883996a73f3f542fa437ac", "type": "github" }, "original": { @@ -36,35 +75,84 @@ "type": "github" } }, - "gazebros2nix": { + "flakoboros": { "inputs": { "flake-parts": "flake-parts", - "gepetto-lib": "gepetto-lib", "nix-ros-overlay": "nix-ros-overlay", "nixpkgs": [ "gepetto", "gazebros2nix", + "flakoboros", "nix-ros-overlay", "nixpkgs" ], - "pyproject-build-systems": "pyproject-build-systems", - "pyproject-nix": "pyproject-nix", "systems": [ "gepetto", "gazebros2nix", + "flakoboros", "nix-ros-overlay", "flake-utils", "systems" ], - "treefmt-nix": "treefmt-nix", + "treefmt-nix": "treefmt-nix" + }, + "locked": { + "lastModified": 1776113076, + "narHash": "sha256-rQp/6mgQc4szevay50RbGXOXwL3xC7oHktywFPrmbAM=", + "owner": "gepetto", + "repo": "flakoboros", + "rev": "07a89fe2a47f2f0364e795cf3e69c045de14a3b2", + "type": "github" + }, + "original": { + "owner": "gepetto", + "repo": "flakoboros", + "type": "github" + } + }, + "gazebros2nix": { + "inputs": { + "flake-parts": [ + "gepetto", + "gazebros2nix", + "flakoboros", + "flake-parts" + ], + "flakoboros": "flakoboros", + "nix-ros-overlay": [ + "gepetto", + "gazebros2nix", + "flakoboros", + "nix-ros-overlay" + ], + "nixpkgs": [ + "gepetto", + "gazebros2nix", + "flakoboros", + "nixpkgs" + ], + "pyproject-build-systems": "pyproject-build-systems", + "pyproject-nix": "pyproject-nix", + "systems": [ + "gepetto", + "gazebros2nix", + "flakoboros", + "systems" + ], + "treefmt-nix": [ + "gepetto", + "gazebros2nix", + "flakoboros", + "treefmt-nix" + ], "uv2nix": "uv2nix" }, "locked": { - "lastModified": 1772816781, - "narHash": "sha256-Ac0KEl+8ygy+BnDgczNHgTumw8HpCasp/zJU5Yx3kQs=", + "lastModified": 1776113232, + "narHash": "sha256-VtzAiez4xLmUY/TrQ31ECKjnOFDrnorQcu6WfsAF2nk=", "owner": "gepetto", "repo": "gazebros2nix", - "rev": "ea8aff2fca6d45fa85fe5e90ef3c71fe0fcc0d12", + "rev": "43783392a112e894b857bdb56ede7044437069f4", "type": "github" }, "original": { @@ -80,7 +168,13 @@ "gazebros2nix", "flake-parts" ], + "flakoboros": [ + "gepetto", + "gazebros2nix", + "flakoboros" + ], "gazebros2nix": "gazebros2nix", + "home-manager": "home-manager", "nix-ros-overlay": [ "gepetto", "gazebros2nix", @@ -92,8 +186,6 @@ "gazebros2nix", "nixpkgs" ], - "src-odri-control-interface": "src-odri-control-interface", - "src-odri-masterboard-sdk": "src-odri-masterboard-sdk", "system-manager": "system-manager", "systems": [ "gepetto", @@ -107,11 +199,11 @@ ] }, "locked": { - "lastModified": 1772825496, - "narHash": "sha256-ZCgGWufV1suEVlft03k9TGOD190kGRCA3rrO8qsjeQ0=", + "lastModified": 1776351070, + "narHash": "sha256-sHbumFcaksKKksN+ZkUeS64llNG+kXkHC6sPk00gjQw=", "owner": "gepetto", "repo": "nix", - "rev": "5c1a5edffd02c51e267c42f8dfd36a13c7817950", + "rev": "fc10d1772e120e1a4cdf1666e983125ecfd3a6ea", "type": "github" }, "original": { @@ -120,18 +212,49 @@ "type": "github" } }, - "gepetto-lib": { + "gitignore": { + "inputs": { + "nixpkgs": [ + "gepetto", + "system-manager", + "userborn", + "pre-commit-hooks-nix", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1709087332, + "narHash": "sha256-HG2cCnktfHsKV0s4XW83gU3F57gaTljL9KNSuG6bnQs=", + "owner": "hercules-ci", + "repo": "gitignore.nix", + "rev": "637db329424fd7e46cf4185293b9cc8c88c95394", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "gitignore.nix", + "type": "github" + } + }, + "home-manager": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, "locked": { - "lastModified": 1770945346, - "narHash": "sha256-L88f+oJbpIkMm9Ln1GP9SFyGztMvnOowbdshQHBeGGs=", - "owner": "Gepetto", - "repo": "nix-lib", - "rev": "82ef58cdf50514f6b1fde96b9d5b38fd8d3e83f5", + "lastModified": 1775425411, + "narHash": "sha256-KY6HsebJHEe5nHOWP7ur09mb0drGxYSzE3rQxy62rJo=", + "owner": "nix-community", + "repo": "home-manager", + "rev": "0d02ec1d0a05f88ef9e74b516842900c41f0f2fe", "type": "github" }, "original": { - "owner": "Gepetto", - "repo": "nix-lib", + "owner": "nix-community", + "ref": "release-25.11", + "repo": "home-manager", "type": "github" } }, @@ -141,11 +264,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1771885942, - "narHash": "sha256-TlBFvE4YHNlbhKVkayP/FWBNAAv+yG9APA8vMR+5NBw=", + "lastModified": 1775764892, + "narHash": "sha256-l1UbHppd5KaU4K/TQPIXf9wnVifH4Tgrx5z2GFoq/bw=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "f891b118c8f4ddb2b6f38d6ce1bfe2f8079552ba", + "rev": "0a4e8cf51001d2d1c613f95dadd3853da42c0164", "type": "github" }, "original": { @@ -178,27 +301,27 @@ }, "nixpkgs": { "locked": { - "lastModified": 1759381078, - "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=", + "lastModified": 1771369470, + "narHash": "sha256-0NBlEBKkN3lufyvFegY4TYv5mCNHbi5OmBDrzihbBMQ=", "owner": "NixOS", "repo": "nixpkgs", - "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee", + "rev": "0182a361324364ae3f436a63005877674cf45efb", "type": "github" }, "original": { - "owner": "lopsided98", - "ref": "nix-ros", + "owner": "NixOS", + "ref": "nixos-unstable", "repo": "nixpkgs", "type": "github" } }, "nixpkgs-lib": { "locked": { - "lastModified": 1769909678, - "narHash": "sha256-cBEymOf4/o3FD5AZnzC3J9hLbiZ+QDT/KDuyHXVJOpM=", + "lastModified": 1774748309, + "narHash": "sha256-+U7gF3qxzwD5TZuANzZPeJTZRHS29OFQgkQ2kiTJBIQ=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "72716169fe93074c333e8d0173151350670b824c", + "rev": "333c4e0545a6da976206c74db8773a1645b5870a", "type": "github" }, "original": { @@ -207,6 +330,36 @@ "type": "github" } }, + "pre-commit-hooks-nix": { + "inputs": { + "flake-compat": [ + "gepetto", + "system-manager", + "userborn", + "flake-compat" + ], + "gitignore": "gitignore", + "nixpkgs": [ + "gepetto", + "system-manager", + "userborn", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1769069492, + "narHash": "sha256-Efs3VUPelRduf3PpfPP2ovEB4CXT7vHf8W+xc49RL/U=", + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "rev": "a1ef738813b15cf8ec759bdff5761b027e3e1d23", + "type": "github" + }, + "original": { + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "type": "github" + } + }, "pyproject-build-systems": { "inputs": { "nixpkgs": [ @@ -226,11 +379,11 @@ ] }, "locked": { - "lastModified": 1771423342, - "narHash": "sha256-7uXPiWB0YQ4HNaAqRvVndYL34FEp1ZTwVQHgZmyMtC8=", + "lastModified": 1773870109, + "narHash": "sha256-ZoTdqZP03DcdoyxvpFHCAek4bkPUTUPUF3oCCgc3dP4=", "owner": "pyproject-nix", "repo": "build-system-pkgs", - "rev": "04e9c186e01f0830dad3739088070e4c551191a4", + "rev": "b6e74f433b02fa4b8a7965ee24680f4867e2926f", "type": "github" }, "original": { @@ -248,11 +401,11 @@ ] }, "locked": { - "lastModified": 1771518446, - "narHash": "sha256-nFJSfD89vWTu92KyuJWDoTQJuoDuddkJV3TlOl1cOic=", + "lastModified": 1775439158, + "narHash": "sha256-NHY9SJNU019n+8NCabBDtmuzRFeE2gZlYKHowp9bV24=", "owner": "pyproject-nix", "repo": "pyproject.nix", - "rev": "eb204c6b3335698dec6c7fc1da0ebc3c6df05937", + "rev": "fb6b728260f3f32761367e9fd1e1a25b4245bcd0", "type": "github" }, "original": { @@ -263,80 +416,24 @@ }, "root": { "inputs": { - "flake-parts": [ - "gepetto", - "flake-parts" - ], - "gazebros2nix": [ - "gepetto", - "gazebros2nix" - ], - "gepetto": "gepetto", - "nix-ros-overlay": [ - "gepetto", - "nix-ros-overlay" - ], - "nixpkgs": [ - "gepetto", - "nixpkgs" - ], - "systems": [ - "gepetto", - "systems" - ], - "treefmt-nix": [ - "gepetto", - "treefmt-nix" - ] - } - }, - "src-odri-control-interface": { - "flake": false, - "locked": { - "lastModified": 1749138911, - "narHash": "sha256-+x+1NxiTwyg5Pwd1oBUMG3Z+eIj+VtVIRFvdPpXStU8=", - "owner": "gwennlbh", - "repo": "odri_control_interface", - "rev": "1137873714fd326fd3ab86cc5632bae72f965866", - "type": "github" - }, - "original": { - "owner": "gwennlbh", - "ref": "nix", - "repo": "odri_control_interface", - "type": "github" - } - }, - "src-odri-masterboard-sdk": { - "flake": false, - "locked": { - "lastModified": 1749026399, - "narHash": "sha256-0VhxOdC2cQwAJfAdHfeHovXrj9jrWb56F35rPN/u+eA=", - "owner": "gwennlbh", - "repo": "master-board", - "rev": "facc7f954294523a0f4b8389ac89fabad4c536db", - "type": "github" - }, - "original": { - "owner": "gwennlbh", - "ref": "nix", - "repo": "master-board", - "type": "github" + "gepetto": "gepetto" } }, "system-manager": { "inputs": { + "flake-compat": "flake-compat", "nixpkgs": [ "gepetto", "nixpkgs" - ] + ], + "userborn": "userborn" }, "locked": { - "lastModified": 1770135975, - "narHash": "sha256-J3qmZ4rTfmgyjrsQRrQWT7ZIYVtYqtLomMNDUibuw2k=", + "lastModified": 1776108115, + "narHash": "sha256-QDa17vvigUyEAk/kTkDHOBFqmbWIe7aZDCjSgl9BG2c=", "owner": "numtide", "repo": "system-manager", - "rev": "413f296fb1fd210c44e38744e270b3afc4c733d7", + "rev": "e028253c2f1ea1b3eba2ce84ae8f770d823fe4ac", "type": "github" }, "original": { @@ -360,20 +457,36 @@ "type": "github" } }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, "treefmt-nix": { "inputs": { "nixpkgs": [ "gepetto", "gazebros2nix", + "flakoboros", "nixpkgs" ] }, "locked": { - "lastModified": 1770228511, - "narHash": "sha256-wQ6NJSuFqAEmIg2VMnLdCnUc0b7vslUohqqGGD+Fyxk=", + "lastModified": 1775636079, + "narHash": "sha256-pc20NRoMdiar8oPQceQT47UUZMBTiMdUuWrYu2obUP0=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "337a4fe074be1042a35086f15481d763b8ddc0e7", + "rev": "790751ff7fd3801feeaf96d7dc416a8d581265ba", "type": "github" }, "original": { @@ -382,6 +495,37 @@ "type": "github" } }, + "userborn": { + "inputs": { + "flake-compat": [ + "gepetto", + "system-manager", + "flake-compat" + ], + "flake-parts": "flake-parts_2", + "nixpkgs": [ + "gepetto", + "system-manager", + "nixpkgs" + ], + "pre-commit-hooks-nix": "pre-commit-hooks-nix", + "systems": "systems_2" + }, + "locked": { + "lastModified": 1770377964, + "narHash": "sha256-q2pnlX2IW0kg80GLFnwWd/GigIpkuZnyKPLhrgJql3E=", + "owner": "jfroche", + "repo": "userborn", + "rev": "55c2cd7952c207a62736a5bbd9499ea73da18d24", + "type": "github" + }, + "original": { + "owner": "jfroche", + "ref": "system-manager", + "repo": "userborn", + "type": "github" + } + }, "uv2nix": { "inputs": { "nixpkgs": [ @@ -396,11 +540,11 @@ ] }, "locked": { - "lastModified": 1772187362, - "narHash": "sha256-gCojeIlQ/rfWMe3adif3akyHsT95wiMkLURpxTeqmPc=", + "lastModified": 1775706324, + "narHash": "sha256-BTb4sydzX2B5/oNbvCdQFeSbk97xEnbb8bk84CiKCOs=", "owner": "pyproject-nix", "repo": "uv2nix", - "rev": "abe65de114300de41614002fe9dce2152ac2ac23", + "rev": "5707df99097375896a3dda811d492a2fabe63500", "type": "github" }, "original": { diff --git a/flake.nix b/flake.nix index 3bb357d..4e8eda3 100644 --- a/flake.nix +++ b/flake.nix @@ -1,45 +1,31 @@ { description = "Tutorial for humanoid path planner platform"; - inputs = { - gepetto.url = "github:gepetto/nix"; - gazebros2nix.follows = "gepetto/gazebros2nix"; - flake-parts.follows = "gepetto/flake-parts"; - nixpkgs.follows = "gepetto/nixpkgs"; - nix-ros-overlay.follows = "gepetto/nix-ros-overlay"; - systems.follows = "gepetto/systems"; - treefmt-nix.follows = "gepetto/treefmt-nix"; - }; + inputs.gepetto.url = "github:gepetto/nix"; outputs = inputs: - inputs.flake-parts.lib.mkFlake { inherit inputs; } ( + inputs.gepetto.lib.mkFlakoboros inputs ( { lib, ... }: { - systems = import inputs.systems; - imports = [ - inputs.gepetto.flakeModule - { - gazebros2nix.overrides.hpp-tutorial = _final: { - src = lib.fileset.toSource { - root = ./.; - fileset = lib.fileset.unions [ - ./CMakeLists.txt - ./doc - ./include - ./Media - ./meshes - ./package.xml - ./rviz - ./script - ./src - ./srdf - ./urdf - ]; - }; - }; - } - ]; + overrideAttrs.hpp-tutorial = { + src = lib.fileset.toSource { + root = ./.; + fileset = lib.fileset.unions [ + ./CMakeLists.txt + ./doc + ./include + ./Media + ./meshes + ./package.xml + ./rviz + ./script + ./src + ./srdf + ./urdf + ]; + }; + }; } ); } diff --git a/include/hpp_tutorial/doc.hh b/include/hpp_tutorial/doc.hh index 202315f..5651ecf 100644 --- a/include/hpp_tutorial/doc.hh +++ b/include/hpp_tutorial/doc.hh @@ -2,50 +2,36 @@ // Copyright (c) 2014 CNRS // Authors: Florent Lamiraux // -// -// This file is part of hpp_tutorial -// hpp_tutorial is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp_tutorial is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp_tutorial If not, see -// . +// BSD 2-Clause License + +// Copyright (c) 2014-2024, CNRS + +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: + +// 1. Redistributions of source code must retain the above copyright notice, +// this +// list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. /// \mainpage Tutorials /// \anchor hpp_tutorial_documentation /// -/// \par Introduction -/// -/// These tutorials explain how to define and solve a path planning -/// problem, how to display the resulting paths in gepetto-viewer, and -/// how to implement a new path planning algorithm. -/// -/// \par Setting up your environment -/// -/// Before starting, make sure that -/// \li \c bash is you default shell script language, -/// \li the line \code source $DEVEL_DIR/config.sh \endcode is in your .bashrc -/// file, where \c DEVEL_DIR is the environment variable defined in the -/// -/// installation instructions. -/// -/// \par Tutorials -/// -/// \li \link hpp_tutorial_tutorial_1 Tutorial 1 (python)\endlink: how to -/// define and solve a path planning problem using CORBA. -/// \li \link hpp_tutorial_tutorial_1_cpp Tutorial 1 (C++)\endlink how to -/// define and solve the same path planning problem without middleware. -/// \li \link hpp_tutorial_tutorial_2 Tutorial 2 - plugin\endlink : how to -/// implement -/// a new path planning algorithm in C++. -/// \li \link hpp_tutorial_tutorial_3 Tutorial 3 - manipulation \endlink : how -/// to define and solve a manipulation planning problem. \li \link -/// hpp_tutorial_tutorial_4 Tutorial 4 - inverse kinematics \endlink: how to -/// plan -/// end-effector trajectoties +/// The tutorials are accessible on +/// github. diff --git a/include/hpp_tutorial/tutorial_1.hh b/include/hpp_tutorial/tutorial_1.hh deleted file mode 100644 index 0dbd3b8..0000000 --- a/include/hpp_tutorial/tutorial_1.hh +++ /dev/null @@ -1,243 +0,0 @@ -// -// Copyright (c) 2014 CNRS -// Authors: Florent Lamiraux -// -// -// This file is part of hpp_tutorial -// hpp_tutorial is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp_tutorial is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp_tutorial If not, see -// . - -/// \page hpp_tutorial_tutorial_1 Tutorial 1 - Python -/// -/// To run the tutorial, open a terminal and open 3 tabs by typing -/// \c CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab -/// by typing \c ALT-[1|2|3]. -/// -/// \section hpp_tutorial_1_starting_hppcorbaserver Starting hppcorbaserver -/// -/// In the first tab, type -/// \code -/// hppcorbaserver -/// \endcode -/// See package \c hpp-corbaserver for details. -/// -/// \section hpp_tutorial_1_starting_gui Starting gepetto-gui -/// -/// In the second tab, type -/// \code -/// gepetto-gui -c basic -/// \endcode -/// A window opens and is ready to display the scene containing the robot. The -/// robot and environment will appear later. -/// -/// Note that \c gepetto-gui and \c hppcorbaserver executables are -/// completely independent. -/// -/// \section hpp_tutorial_1_python Controlling via a python terminal -/// -/// In the third tab, type -/// \code -/// cd script -/// python -i tutorial_1.py -/// \endcode -/// to run the script script/tutorial_1.py in an -/// interactive python terminal. -/// -/// To display the scene, type -/// \code -/// >>> v = vf.createViewer () -/// \endcode -/// gepetto-gui window should now display a scene containing a PR2 robot in a -/// kitchen environment. -/// -/// To display initial and goal configurations type the following commands -/// \code -/// >>> v (q_init) -/// >>> v (q_goal) -/// \endcode -/// -/// To solve the path planning problem between those configurations, type -/// \code -/// >>> ps.solve () -/// \endcode -/// -/// To display the resulting of RRT, type -/// \code -/// >>> from hpp.gepetto import PathPlayer -/// >>> pp = PathPlayer (v) -/// >>> pp(0) -/// \endcode -/// -/// To display an optimized solutions, -/// \code -/// >>> pp(1) -/// >>> pp(2) -/// \endcode -/// -/// \section hpp_tutorial_1_script Detailed explanation -/// This section presents in more details the content of \c -/// script/tutorial_1.py. -/// -/// \code -/// from hpp.corbaserver.pr2 import Robot -/// robot = Robot ('pr2') -/// robot.setJointBounds ("root_joint", [-4, -3, -5, -3]) -/// \endcode -/// Import class pr2.robot.Robot and create an instance and set bounds of -/// translation degrees of freedom of the base. -/// Note that the constructor of the instance calls idl method -/// hpp::corbaserver::Robot::loadRobotModel. This triggers the loading of the -/// urdf/srdf model of the PR2 robot in \c hppcorbaserver executable. -/// -/// \code -/// from hpp.corbaserver import ProblemSolver -/// ps = ProblemSolver (robot) -/// \endcode -/// Import class hpp.corbaserver.problem_solver.ProblemSolver and create an -/// instance. This class is a helper class to define and solve path planning -/// problems. -/// -/// \code -/// from hpp.gepetto import ViewerFactory -/// vf = ViewerFactory (ps) -/// \endcode -/// Import class gepetto.viewerFactory.ViewerFactory and create an instance. -/// This object takes as input the \c ProblemSolver instance that enables the -/// viewer client to also control \c hppcorbaserver executable -/// -/// \code -/// q_init = robot.getCurrentConfig () -/// q_goal = q_init [::] -/// q_init [0:2] = [-3.2, -4] -/// rank = robot.rankInConfiguration ['torso_lift_joint'] -/// q_init [rank] = 0.2 -/// \endcode -/// Define initial configuration. -/// \note Initial configuration is built from configuration of the robot at -/// construction, and by modification of joints retrieved by name. This method -/// is more robust than specifying a hard-coded configuration vector since the -/// ordering of joints in the configuration vector is not unique. -/// -/// \code -/// q_goal [0:2] = [-3.2, -4] -/// rank = robot.rankInConfiguration ['l_shoulder_lift_joint'] -/// q_goal [rank] = 0.5 -/// rank = robot.rankInConfiguration ['l_elbow_flex_joint'] -/// q_goal [rank] = -0.5 -/// rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] -/// q_goal [rank] = 0.5 -/// rank = robot.rankInConfiguration ['r_elbow_flex_joint'] -/// q_goal [rank] = -0.5 -/// \endcode -/// Define goal configuration. -/// -/// \code -/// vf.loadObstacleModel ("package://hpp_tutorial/urdf/kitchen_area.urdf", -/// "kitchen") - -/// \endcode -/// Load obstacles from urdf file. -/// \note this method loads the objects defined in the urdf file both in -/// hppcorbaserver and in \c gepetto-viewer-server. -/// -/// \code -/// ps.setInitialConfig (q_init) -/// ps.addGoalConfig (q_goal) -/// \endcode -/// Define initial and goal configurations. -/// -/// \code -/// ps.addPathOptimizer ("RandomShortcut") -/// \endcode -/// Add a path optimizer (hpp::core::pathOptimization::RandomShortcut). -/// -/// \code -/// loaded = ps.client.problem.loadPlugin("spline-gradient-based.so") -/// if loaded: -/// ps.addPathOptimizer("SplineGradientBased_bezier1") -/// else: -/// print("Could not load spline-gradient-based.so") -/// \endcode -/// Load a plugin that implements another path optimizer -/// (hpp::core::pathOptimization::SplineGradientBasedAbstract) and add the path -/// optimizer. The two selected path optimizers will be called in sequence. -/// Note that in this example, the second path optimizer will not improve the -/// result of the first one. -/// -/// \code -/// print (ps.solve ()) -/// \endcode -/// Solve problem and print the results. -/// -/// \code -/// v = vf.createViewer() -/// from hpp.gepetto import PathPlayer -/// pp = PathPlayer (v) -/// \endcode -/// Create the display window. -/// Import and create an instance of PathPlayer. This class samples a path in -/// \c hppcorbaserver and displays it in \c gepetto-viewer-server. -/// -/// \note Paths can be displayed in \c gepetto-gui after installing \c hpp-gui -/// package and loading plugin \c hppwidgetsplugin. -/// -/// \code -/// pp(0) -/// \endcode -/// Display first path, result of RRT. -/// -/// \code -/// pp(1) -/// \endcode -/// Display second path after optimization by random shortcut algorithm. -/// -/// \code -/// pp(2) -/// \endcode -/// Display third path after optimization by spline gradient based algorithm. - -/// \page hpp_tutorial_tutorial_1_cpp Tutorial 1 - C++ -/// -/// Currently, there is no visualization with the C++ version. -/// -/// \section hpp_tutorial_tutorial_1_cpp_execution Execute the binary. -/// -/// Compile the code and run the following command in a terminal, type -/// \code -/// build-folder/src/hpp-tutorial-1 -/// \endcode -/// -/// \section hpp_tutorial_tutorial_1_cpp_source Understanding the source code. -/// -/// Have a look at the file \c src/tutorial_1.cc. It contains the C++ code that -/// defines and solves the same path planning problem as \c tutorial_1.py. -/// -/// \code -/// ProblemSolverPtr_t ps = ProblemSolver::create(); -/// \endcode -/// -/// Class hpp::core::ProblemSolver is a container class that stores -/// \li a robot, -/// \li objstacles, -/// \li various types of path planning algorithms (hpp::core::PathPlanner), -/// \li various types of path optimizers (hpp::core::PathOptimizer), -/// \li various types of configuration shooter -/// (hpp::core::ConfigurationShooter), -/// \li various types of steering methods (hpp::core::SteeringMethod), -/// \li various types of path validation methods -/// (hpp::core::PathValidation) -/// \li various types of path projection algorithms (hpp::core::PathProjector) -/// \li various types of metrics defined on the configuration space -/// (hpp::core::Distance). -/// diff --git a/include/hpp_tutorial/tutorial_2.hh b/include/hpp_tutorial/tutorial_2.hh deleted file mode 100644 index c2b8aca..0000000 --- a/include/hpp_tutorial/tutorial_2.hh +++ /dev/null @@ -1,134 +0,0 @@ -// -// Copyright (c) 2014 CNRS -// Authors: Florent Lamiraux -// -// -// This file is part of hpp_tutorial -// hpp_tutorial is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp_tutorial is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp_tutorial If not, see -// . - -/// \page hpp_tutorial_tutorial_2 Tutorial 2 - Plugin -/// -/// In this tutorial, we are going to implement a new path planning algorithm -/// within a plugin. -/// -/// \section hpp_tutorial_tutorial_2_implementation Implementing a new path -/// planning algorithm in a plugin -/// -/// The code of this tutorial can be found in \c src/tutorial_2.cc. -/// The compilation and installation instructions can be found in -/// \c src/CMakeLists.txt. -/// -/// \subsection hpp_tutorial_tutorial_2_class_planner Implementation of class -/// Planner -/// -/// File \c src/tutorial_2.cc implements \c class -/// hpp::tutorial::Planner, deriving from abstract class hpp::core::PathPlanner. -/// In this section, we explain some specific parts of the code. -/// -/// \code HPP_PREDEF_CLASS (Planner);\endcode -/// is a macro containing the forward declaration of class \c Planner as well as -/// \c PlannerWkPtr_t for weak pointer to class \c Planner. See -/// \c hpp/util/pointer.hh -/// for details. -/// -/// \code -/// static PlannerPtr_t create (const core::Problem& problem, -/// const core::RoadmapPtr_t& roadmap) -/// \endcode -/// As most classes, hpp::core::PathPlanner and any derived class are -/// manipulated -/// via shared pointers. Users are not allowed to define variables of the type. -/// Instead, they are required to call method \c create and to store the -/// resulting shared pointer. For this reason, the constructors are all -/// protected. -/// \note method \c create calls protected method \c init that is explained -/// later on. -/// -/// \code -/// virtual void oneStep () -/// \endcode -/// This method runs one step of our the algorithm. The new algorithm is a -/// basic version of PRM. Notice the compactness of the code. -/// -/// \code -/// void init (const PlannerWkPtr_t& weak) -/// \endcode -/// Method \c init takes as input a weak pointer to a new instance and stores -/// this weak pointer as a private member. This enables any object to -/// create a shared pointer to itself on demand using the following line of code -/// \code -/// weakPtr_.lock (); -/// \endcode -/// which is the shared pointer equivalent to \c this when using simple -/// pointers. \note Method \c init always calls the parent implementation so -/// that the parent part of the object also stores a weak pointer to itself. -/// -/// \subsection hpp_tutorial_tutorial_2_plugin Implementation of plugin -/// tutorial-2.so -/// -/// Now that the new class \c hpp::tutorial::Planner has been implemented, we -/// are going to add it via a plugin. -/// -/// \code -/// class Plugin : public core::ProblemSolverPlugin { -/// public: -/// Plugin() : ProblemSolverPlugin("TutorialPlugin", "0.0") {} -/// protected: -/// virtual bool impl_initialize(core::ProblemSolverPtr_t ps) { -/// ps->pathPlanners.add("TutorialPRM", Planner::create); -/// return true; -/// } -/// }; // class Plugin -/// \endcode -/// class \c hpp::tutorial::Plugin derives from abstract class -/// \c hpp::core::ProblemSolverPlugin. Upon loading of the plugin by -/// \c hppcorbaserver, method \c impl_initialize is called. This method register -/// our new path planning class with key "TutorialPRM" -/// -/// \code -/// HPP_CORE_DEFINE_PLUGIN(hpp::tutorial::Plugin) -/// \endcode -/// This macro register the new plugin. -/// -/// \section hpp_tutorial_tutorial_2_CMakeLists Compilation and installation -/// -/// The compilation and installation is done in file \c src/CMakeLists.txt by -/// the following lines -/// \code -/// ## Tutorial 2 -/// include(${HPP_CORE_CMAKE_PLUGIN}) -/// # Create and install the plugin -/// hpp_add_plugin(tutorial-2 SOURCES tutorial_2.cc LINK_DEPENDENCIES -/// hpp-corbaserver::hpp-corbaserver) -/// \endcode -/// These two lines declare a new plugin the source file of which is -/// tutorial-2.cc and install this plugin into lib/hppPlugins subdirectory -/// of the installation prefix. -/// -/// \section hpp_tutorial_tutorial_2_running Using the plugin and solving a -/// problem. -/// -/// To solve a problem with the new path planning -/// algorithm, we simply need to follow the same steps as in tutorial 1, -/// except that we should source \c script/tutorial_2.py instead of -/// \c script/tutorial_1.py -/// -/// \code -/// loaded = ps.client.problem.loadPlugin("tutorial-2.so") -/// assert(loaded) -/// -/// ps.selectPathPlanner("TutorialPRM") -/// \endcode -/// The above lines load the plugin and select the new path planner. -/// diff --git a/include/hpp_tutorial/tutorial_3.hh b/include/hpp_tutorial/tutorial_3.hh deleted file mode 100644 index 4fcc446..0000000 --- a/include/hpp_tutorial/tutorial_3.hh +++ /dev/null @@ -1,87 +0,0 @@ -// -// Copyright (c) 2017 CNRS -// Authors: Florent Lamiraux -// -// -// This file is part of hpp_tutorial -// hpp_tutorial is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp_tutorial is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp_tutorial If not, see -// . - -/// \page hpp_tutorial_tutorial_3 Tutorial 3 - manipulation -/// -/// To run the tutorial, open a terminal and open 3 tabs by typing -/// \c CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab -/// by typing \c ALT-[1|2|3]. -/// -/// \section hpp_tutorial_3_starting_hpp_manipulation_server Starting -/// hppcorbaserver -/// -/// In the first tab, type -/// \code -/// hppcorbaserver -/// \endcode -/// See package \c hpp-manipulation-corba for details. -/// -/// \section hpp_tutorial_3_python Controlling via a python terminal -/// -/// In the second tab, type -/// \code -/// cd script -/// python -i tutorial_3.py -/// \endcode -/// Script script/tutorial_3.py -/// defines a manipulation planning problem. -/// -/// \section hpp_tutorial_3_starting_gui Starting gepetto-gui -/// -/// In the third tab, type -/// \code -/// gepetto-gui -/// \endcode -/// A window opens and is ready to display the scene containing the robot. The -/// robot, environment and object will appear later. -/// -/// Note that \c gepetto-gui and \c hppcorbaserver executables are -/// completely independent. -/// -/// \section hpp_tutorial_3_python Controlling via a python terminal -/// -/// To display the scene, create a client to the viewer in the python terminal. -/// \code -/// >>> v = vf.createViewer () -/// \endcode -/// The robot and environment should appear in the viewer. If the viewer -/// window is black, select the window and hit space. -/// -/// To solve the problem, type -/// \code -/// >>> ps.solve () -/// \endcode -/// -/// and to display the (non optimized) solution path, type -/// \code -/// >>> pp = PathPlayer (v) -/// >>> pp (0) -/// \endcode -/// -/// \section hpp_tutorial_3_optimization Optimizing the solution path -/// -/// To optimize the solution path, select a path optimizer: -/// \code -/// >>> ps.addPathOptimizer('Graph-RandomShortcut') -/// >>> ps.optimizePath(0) -/// \endcode -/// To display the solution: -/// \code -/// >>> pp(1) -/// \endcode diff --git a/include/hpp_tutorial/tutorial_4.hh b/include/hpp_tutorial/tutorial_4.hh deleted file mode 100644 index 3c5743c..0000000 --- a/include/hpp_tutorial/tutorial_4.hh +++ /dev/null @@ -1,315 +0,0 @@ -// -// Copyright (c) 2017 CNRS -// Authors: Florent Lamiraux -// -// -// This file is part of hpp_tutorial -// hpp_tutorial is free software: you can redistribute it -// and/or modify it under the terms of the GNU Lesser General Public -// License as published by the Free Software Foundation, either version -// 3 of the License, or (at your option) any later version. -// -// hpp_tutorial is distributed in the hope that it will be -// useful, but WITHOUT ANY WARRANTY; without even the implied warranty -// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU -// General Lesser Public License for more details. You should have -// received a copy of the GNU Lesser General Public License along with -// hpp_tutorial If not, see -// . - -/// \page hpp_tutorial_tutorial_4 Tutorial 4 - inverse kinematics -/// -/// To run this tutorial, you need to install package \c ur_description. -/// -/// To run the tutorial, open a terminal and open 3 tabs by typing -/// \c CTRL+SHIFT+T twice. When the terminal is selected, you can select a tab -/// by typing \c ALT-[1|2|3]. -/// -/// \section hpp_tutorial_4_starting_hpp_manipulation_server Starting -/// hppcorbaserver -/// -/// In the first tab, type -/// \code -/// hppcorbaserver -/// \endcode -/// See package \c hpp-manipulation-corba for details. -/// -/// \section hpp_tutorial_4_python Controlling via a python terminal -/// -/// In the second tab, type -/// \code -/// cd script -/// python -i tutorial_4.py -/// \endcode -/// Script script/tutorial_4.py -/// defines a path for an UR10 end-effector -/// -/// \section hpp_tutorial_4_starting_gui Starting gepetto-gui -/// -/// In the third tab, type -/// \code -/// gepetto-gui -/// \endcode -/// A window opens and is ready to display the scene containing the robot. The -/// robot, environment and object will appear later. -/// -/// \section hpp_tutorial_4_python Controlling via a python terminal -/// -/// To display the scene, create a client to the viewer in the python terminal. -/// \code -/// >>> v = vf.createViewer () -/// \endcode -/// The robot and environment should appear in the viewer. If the viewer -/// window is black, select the window and hit space. -/// -/// \section hpp_tutorial_4_explanition Explaining the script -/// -/// The first part of the script below -/// \li creates the robot from a xacro file, -/// \li creates the problem solver and the viewer factory. -/// -/// \code -/// ## Load robot from processing of a xacro file -/// Robot.urdfString = process_xacro\ -/// ("package://hpp_tutorial/urdf/ur10e.urdf.xacro", -/// "transmission_hw_interface:=hardware_interface/PositionJointInterface") -/// # Deactivate collision checking between consecutive links -/// Robot.srdfString = """ -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// -/// """ -/// loadServerPlugin ("corbaserver", "manipulation-corba.so") -/// newProblem() -/// -/// robot = Robot('robot', 'ur10e', rootJointType="anchor") -/// ps = ProblemSolver(robot) -/// vf = ViewerFactory(ps) -/// \endcode -/// -/// The following lines -/// \li create a gripper attached to the end-effector of the robot, -/// \li create two handles attached to the base of the robot. -/// -/// These grippers and handles will ease the creation of pose constraints for -/// the end-effector. -/// -/// \code -/// ## Add a gripper to the robot -/// robot.client.manipulation.robot.addGripper\ -/// ('ur10e/wrist_3_link', 'ur10e/gripper', [0,0,.1,0.5,0.5,0.5,-0.5], 0.1) -/// -/// ## Create two handles -/// robot.client.manipulation.robot.addHandle\ -/// ('ur10e/base_link', 'handle1', [.8, -.4, .5, 0, 0, 0, 1], .1, 6*[True]) -/// robot.client.manipulation.robot.addHandle\ -/// ('ur10e/base_link', 'handle2', [.8, .4, .5, 0, 0, 0, 1], .1, 6*[True]) -/// \endcode -/// -/// Then two grasp constraints are created to defined initial and goal pose -/// of the gripper -/// -/// \code -/// ## Create grasp constraints -/// robot.client.manipulation.problem.createGrasp\ -/// ("ur10e/gripper grasps handle1", "ur10e/gripper", "handle1") -/// robot.client.manipulation.problem.createGrasp\ -/// ("ur10e/gripper grasps handle2", "ur10e/gripper", "handle2") -/// \endcode -/// -/// Configurations satisfying the pose constraints are computed by creating -/// a constraint graph with two nodes and no edge. -/// -/// \code -/// ## Create a constraint graph with one node for each grasp -/// cg = ConstraintGraph(robot, "graph") -/// cg.createNode(["ur10e/gripper grasps handle1", "ur10e/gripper grasps -/// handle2"]) -/// cg.addConstraints(node = "ur10e/gripper grasps handle1", constraints = \ -/// Constraints(numConstraints = ["ur10e/gripper grasps handle1"])) -/// cg.addConstraints(node = "ur10e/gripper grasps handle2", constraints = \ -/// Constraints(numConstraints = ["ur10e/gripper grasps handle2"])) -/// cg.initialize() -///\endcode -/// -/// We compute the initial and goal configurations by numerical inverse -/// kinematics. -/// -/// \code -/// # Generate one configuration satisfying each constraint -/// found = False -/// while not found: -/// q0 = robot.shootRandomConfig() -/// res, q1, err = cg.applyNodeConstraints("ur10e/gripper grasps handle1", -/// q0) if not res: continue res, msg = robot.isConfigValid(q1) if not res: -/// continue res, q2, err = cg.applyNodeConstraints("ur10e/gripper grasps -/// handle2", q1) if not res: continue res, msg = robot.isConfigValid(q2) if -/// not res: continue found = True -/// -/// We create several CORBA objects: -/// \li the current manipulation planning problem \c cmp, -/// \li the robot stored in this problem \c crobot, -/// \li a steering method \c csm of type -/// hpp::manipulation::steeringMethod::EndEffectorTrajectory -/// -/// \code -/// ## Create an EndEffectorTrajectory steering method -/// cmp = wd(ps.client.basic.problem.getProblem()) -/// crobot = wd(cmp.robot()) -/// cproblem = wd(ps.client.basic.problem.createProblem(crobot)) -/// csm = -/// wd(ps.client.basic.problem.createSteeringMethod("EndEffectorTrajectory", -/// cproblem)) \endcode -/// -/// Then we create a \link hpp::core::ConstraintSet ContraintSet \endlink -/// containing an empty \link hpp::core::ConfigProjector ConfigProjector -/// \endlink that we pass to the problem. We set \link -/// hpp::manipulation::steeringMethod::EndEffectorTrajectory \c csm \endlink as -/// the steering method of the problem. Note that the last line passes the \link -/// hpp::core::ConstraintSet ContraintSet \endlink of the problem to the -/// steering method. The order is important here since at construction the -/// problem is given an empty \link hpp::core::ConstraintSet ContraintSet -/// \endlink and setting the steering method of -/// the problem passes the \link hpp::core::ConstraintSet ContraintSet \endlink -/// of the problem to the steering method. -/// -/// \code -/// cs = wd(ps.client.basic.problem.createConstraintSet(crobot, -/// "sm-constraints")) cp = -/// wd(ps.client.basic.problem.createConfigProjector(crobot, "cp", 1e-4, 40)) -/// cs.addConstraint(cp) -/// cproblem.setConstraints(cs) -/// cproblem.setSteeringMethod(csm) -/// \endcode -/// -/// We need to create a time varying constraint for the end-effector. For that, -/// we create a new grasp between the gripper and the first handle. Note that -/// we cannot use the previously created identical grasp, since the comparison -/// type of this one should be \c Equality. -/// -/// \code -/// # Create a new grasp constraint for the steering method right hand side -/// # The previously created one has EqualToZero as comparison types. -/// robot.client.manipulation.problem.createGrasp \ -/// ("end-effector-tc", "ur10e/gripper", "handle1") -/// # Set comparison type to Equality -/// ps.setConstantRightHandSide("end-effector-tc", False) -/// \endcode -/// -/// We insert this constraint into the \link hpp::core::ConfigProjector -/// ConfigProjector \endlink of the \link hpp::core::Problem problem \endlink -/// (and thus of the steering method) -/// -/// \code -/// tc = wd(ps.client.basic.problem.getConstraint("end-effector-tc")) -/// cp.add(tc, 0) -/// \endcode -/// -/// We pass this constraint to the steering method as the trajectory constraint. -/// Note that from a mathematical point of view, the trajectory constraint is -/// a mapping from the robot configuration space \f$\mathcal{C}\f$ to -/// \f$SE(3)\f$ defined by -/// \f{equation} -/// tc(\mathbf{q}) = g^{-1}(\mathbf{q})h_1 -/// \f} -/// where \f$g(\mathbf{q})\in SE(3)\f$ is the pose of the gripper in -/// configuration \f$\mathbf{q}\f$ and \f$h_1\in SE(3)\f$ is the pose of -/// \c handle1. -/// -/// \code -/// csm.trajectoryConstraint(tc) -/// \endcode -/// -/// We now need to build the right hand side of the constraint as a linear -/// interpolation between the initial and final values. For that we evaluate -/// the fonction \f$tc\f$ of the constraint at the initial and goal -/// configurations, -/// we create a path in SE(3) linking these two values and we give this path -/// to the steering method to define the time-varying right hand side. -/// -/// From a mathematical point of view, \f$\mathbf{p}\f$ is a mapping from an -/// interval \f$[0,T]\f$ to \f$SE(3)\f$ such that -/// \f{eqnarray} -/// \mathbf{p}(0) &=& tc(\mathbf{q}_1) \\ -/// \mathbf{p}(T) &=& tc(\mathbf{q}_2) \\ -/// \f} -/// The constraint applied along the path computed by the steering method is -/// thus: -/// \f{equation} -/// \forall t\in[0,T],\ \ \mathbf{tc}(\mathbf{q}(t)) = \mathbf{p}(t) -/// \f} -/// -/// \code -/// # Get right hand side for q1 and q2 -/// rhs1 = tc.function().value(q1) -/// rhs2 = tc.function().value(q2) -/// # Create linear path for end-effector -/// p = wd(csm.makePiecewiseLinearTrajectory([rhs1, rhs2], 6*[1.])) -/// # Set this path as the time-varying right hand side of the constraint -/// csm.trajectory(p, True) -///\endcode -/// -/// We can now call the steering method between the initial and goal -/// configurations and insert this path in the ProblemSolver in order to -/// make it visible in \c gepetto-gui. -/// -/// \code -/// ## Call steering method -/// p1 = wd(csm.call(q1,q2)) -/// if p1: -/// ps.client.basic.problem.addPath(p1.asVector()) -///\endcode -/// -/// After connecting and refreshing \c gepetto-gui, you should be able to -/// display the path. Notice that the path might be discontinuous. -/// -/// To get a continuous path for sure, we need to use the \link -/// hpp::manipulation::pathPlanner::EndEffectorTrajectory EndEffectorTrajectory -/// path planner \endlink. -/// -/// \code -/// ## Using EndEffectorTrajectory path planner -/// cdistance = wd(cproblem.getDistance()) -/// croadmap = wd(ps.client.basic.problem.createRoadmap(cdistance, crobot)) -/// cplanner = wd(ps.client.basic.problem.createPathPlanner( -/// "EndEffectorTrajectory", cproblem, croadmap)) -/// cplanner.setNRandomConfig(0) -/// cplanner.maxIterations(1) -/// cplanner.setNDiscreteSteps(20) -/// -/// cproblem.setInitConfig(q1) -/// cproblem.addGoalConfig(q2) -/// -/// p2 = wd(cplanner.solve()) -/// if p2: -/// ps.client.basic.problem.addPath(p2) -///\endcode -/// -/// We set the number of random configurations to 0 in order to force the -/// planner to start from \c q1. Otherwise, in case of failure to plan a -/// continuous path starting from \c q1, the planner would generate random -/// initial configurations that satisfy the constraints at the beginning. -/// To be consistent, we set the maximal number of iterations to 1 since new -/// iterations would simply retry to start from \c q1. We set the number of -/// steps at which a configuration is computed for the corresponding pose of the -/// end effector to 20. -/// -/// Notice that the path satisfies the end-effector time-varying constraint, but -/// does not necessarily end at \f$\mathbf{q}_2\f$ since the final -/// configuration is completely determined by the initial one. diff --git a/meshes/drill.stl b/meshes/drill.stl new file mode 100644 index 0000000..505e3ff Binary files /dev/null and b/meshes/drill.stl differ diff --git a/meshes/staubli-tx2-90/collision/base_link.stl b/meshes/staubli-tx2-90/collision/base_link.stl new file mode 100644 index 0000000..34bd8fe Binary files /dev/null and b/meshes/staubli-tx2-90/collision/base_link.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_1.stl b/meshes/staubli-tx2-90/collision/link_1.stl new file mode 100644 index 0000000..7ff5850 Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_1.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_2.stl b/meshes/staubli-tx2-90/collision/link_2.stl new file mode 100644 index 0000000..de5ff0f Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_2.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_3.stl b/meshes/staubli-tx2-90/collision/link_3.stl new file mode 100644 index 0000000..d63c2a9 Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_3.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_4.stl b/meshes/staubli-tx2-90/collision/link_4.stl new file mode 100644 index 0000000..98c49cf Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_4.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_5.stl b/meshes/staubli-tx2-90/collision/link_5.stl new file mode 100644 index 0000000..3010533 Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_5.stl differ diff --git a/meshes/staubli-tx2-90/collision/link_6.stl b/meshes/staubli-tx2-90/collision/link_6.stl new file mode 100644 index 0000000..29a569f Binary files /dev/null and b/meshes/staubli-tx2-90/collision/link_6.stl differ diff --git a/meshes/staubli-tx2-90/visual/base_link.stl b/meshes/staubli-tx2-90/visual/base_link.stl new file mode 100644 index 0000000..d671f41 Binary files /dev/null and b/meshes/staubli-tx2-90/visual/base_link.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_1.stl b/meshes/staubli-tx2-90/visual/link_1.stl new file mode 100644 index 0000000..ceaec76 Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_1.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_2.stl b/meshes/staubli-tx2-90/visual/link_2.stl new file mode 100644 index 0000000..38582ad Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_2.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_3.stl b/meshes/staubli-tx2-90/visual/link_3.stl new file mode 100644 index 0000000..0400eeb Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_3.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_4.stl b/meshes/staubli-tx2-90/visual/link_4.stl new file mode 100644 index 0000000..20fe9c7 Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_4.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_5.stl b/meshes/staubli-tx2-90/visual/link_5.stl new file mode 100644 index 0000000..ad0ca31 Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_5.stl differ diff --git a/meshes/staubli-tx2-90/visual/link_6.stl b/meshes/staubli-tx2-90/visual/link_6.stl new file mode 100644 index 0000000..f022e7c Binary files /dev/null and b/meshes/staubli-tx2-90/visual/link_6.stl differ diff --git a/script/hpp_python_tutorial_1.py b/script/hpp_python_tutorial_1.py deleted file mode 100644 index 5f5d6ef..0000000 --- a/script/hpp_python_tutorial_1.py +++ /dev/null @@ -1,85 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pyhpp.core import DiffusingPlanner, Problem, RandomShortcut -from pyhpp.pinocchio import Device, urdf -from pyhpp_viser import Viewer - -urdfFilename = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -srdfFilename = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" -rootJointType = "planar" -# Initialize robot and viewer -robot = Device("ur2") - -urdf.loadModel( - robot, 0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity() -) - -# Define initial and goal configurations -q_init = np.array(robot.currentConfiguration()) -q_goal = np.array(q_init[::].copy()) -model = robot.model() - -# Set root_joint bounds only -lowerLimit = model.lowerPositionLimit -upperLimit = model.upperPositionLimit - -# Set root_joint bounds specifically -root_joint_bounds = [-4, -3, -5, -3] -ij = model.getJointId("r0/root_joint") -iq = model.idx_qs[ij] - -# Apply bounds (assuming first 2 DOF are x,y position) -lowerLimit[iq] = -4 -upperLimit[iq] = -3 -lowerLimit[iq + 1] = -5 -upperLimit[iq + 1] = -3 - -rankInConfiguration = dict() -current_rank = 0 -for joint_id in range(1, model.njoints): - joint_name = model.names[joint_id] - joint = model.joints[joint_id] - rankInConfiguration[joint_name[3:]] = current_rank - - current_rank += joint.nq - -q_init[0:2] = [-3.2, -4] - -rank = rankInConfiguration["torso_lift_joint"] -q_init[rank] = 0.2 -q_goal[0:2] = [-3.2, -4] -rank = rankInConfiguration["l_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["l_elbow_flex_joint"] -q_goal[rank] = -0.5 -rank = rankInConfiguration["r_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["r_elbow_flex_joint"] -q_goal[rank] = -0.5 - -urdf.loadModel( - robot, - 0, - "kitchen_area", - "anchor", - "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf", - "", - SE3.Identity(), -) - -problem = Problem(robot) -problem.initConfig(q_init) -problem.addGoalConfig(q_goal) - -diffusingPlanner = DiffusingPlanner(problem) - -path_optimizer = RandomShortcut(problem) -path = diffusingPlanner.solve() -opt_path = path_optimizer.optimize(path) - -# launch in interactive mode to use viewer -viewer = Viewer(robot) -viewer.initViewer(open=True, loadModel=True) -viewer(np.array(q_init)) -viewer.loadPath(path, "path") -viewer.loadPath(opt_path, "opt_path") diff --git a/script/tutorial_1.py b/script/tutorial_1.py index 9665e17..7b079a5 100644 --- a/script/tutorial_1.py +++ b/script/tutorial_1.py @@ -1,51 +1,16 @@ -from hpp.gepetto import ViewerFactory +from pinocchio import SE3, neutral +from pyhpp.manipulation import Device, urdf -from hpp.corbaserver import ProblemSolver -from hpp.corbaserver.pr2 import Robot +robot = Device("tuto") -robot = Robot("pr2") -robot.setJointBounds("root_joint", [-4, -3, -5, -3]) +urdf_filename = "package://example-robot-data/robots/panda_description/urdf/panda.urdf" +srdf_filename = "package://hpp_tutorial/srdf/panda.srdf" -ps = ProblemSolver(robot) +urdf.loadModel( + robot, 0, "panda", "anchor", urdf_filename, srdf_filename, SE3.Identity() +) -vf = ViewerFactory(ps) - -q_init = robot.getCurrentConfig() -q_goal = q_init[::] -q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["torso_lift_joint"] -q_init[rank] = 0.2 - -q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["l_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = robot.rankInConfiguration["l_elbow_flex_joint"] -q_goal[rank] = -0.5 -rank = robot.rankInConfiguration["r_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = robot.rankInConfiguration["r_elbow_flex_joint"] -q_goal[rank] = -0.5 - -vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen") - -ps.setInitialConfig(q_init) -ps.addGoalConfig(q_goal) - -ps.addPathOptimizer("RandomShortcut") -loaded = ps.client.problem.loadPlugin("spline-gradient-based.so") -if loaded: - ps.addPathOptimizer("SplineGradientBased_bezier1") -else: - print("Could not load spline-gradient-based.so") - -# print (ps.solve ()) - -# # Uncomment this to connect to a viewer server and play solution paths -# -# v = vf.createViewer() -# from hpp.gepetto import PathPlayer -# pp = PathPlayer (v) - -# pp(0) -# pp(1) -# pp(2) +# Get neutral configuration of robot +q = neutral(robot.model()) +# Open the gripper +q[-2:] = [0.035, 0.035] diff --git a/script/tutorial_2.py b/script/tutorial_2.py deleted file mode 100644 index 01b84d6..0000000 --- a/script/tutorial_2.py +++ /dev/null @@ -1,49 +0,0 @@ -from hpp.gepetto import ViewerFactory - -from hpp.corbaserver import ProblemSolver -from hpp.corbaserver.pr2 import Robot - -robot = Robot("pr2") -robot.setJointBounds("root_joint", [-4, -3, -5, -3]) - -ps = ProblemSolver(robot) - -vf = ViewerFactory(ps) - -q_init = robot.getCurrentConfig() -q_goal = q_init[::] -q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["torso_lift_joint"] -q_init[rank] = 0.2 - -q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["l_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = robot.rankInConfiguration["l_elbow_flex_joint"] -q_goal[rank] = -0.5 -rank = robot.rankInConfiguration["r_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = robot.rankInConfiguration["r_elbow_flex_joint"] -q_goal[rank] = -0.5 - -vf.loadObstacleModel("package://hpp_tutorial/urdf/kitchen_area.urdf", "kitchen") - -ps.setInitialConfig(q_init) -ps.addGoalConfig(q_goal) - -loaded = ps.client.problem.loadPlugin("tutorial-2.so") -assert loaded - -ps.selectPathPlanner("TutorialPRM") -ps.addPathOptimizer("RandomShortcut") - -# print (ps.solve ()) - -# # Uncomment this to connect to a viewer server and play solution paths -# -# v = vf.createViewer() -# from hpp.gepetto import PathPlayer -# pp = PathPlayer (v) - -# pp (0) -# pp (1) diff --git a/script/tutorial_3.py b/script/tutorial_3.py deleted file mode 100644 index 497a5bf..0000000 --- a/script/tutorial_3.py +++ /dev/null @@ -1,153 +0,0 @@ -# Import libraries and load robots. {{{1 - -# Import. -from math import sqrt - -from hpp.gepetto import PathPlayer # noqa: F401 -from hpp.gepetto.manipulation import ViewerFactory - -from hpp.corbaserver import loadServerPlugin -from hpp.corbaserver.manipulation import ( - Client, - ConstraintGraph, - ConstraintGraphFactory, - Constraints, - ProblemSolver, - Rule, -) -from hpp.corbaserver.manipulation.pr2 import Robot - -loadServerPlugin("corbaserver", "manipulation-corba.so") -Client().problem.resetProblem() - -# Load PR2 and a box to be manipulated. - - -class Box: - rootJointType = "freeflyer" - packageName = "hpp_tutorial" - urdfName = "box" - urdfSuffix = "" - srdfSuffix = "" - - -class Environment: - packageName = "hpp_tutorial" - urdfName = "kitchen_area" - urdfSuffix = "" - srdfSuffix = "" - - -robot = Robot("pr2-box", "pr2") -ps = ProblemSolver(robot) -# ViewerFactory is a class that generates Viewer on the go. It means you can -# restart the server and regenerate a new windows. -# To generate a window: -# vf.createViewer () -vf = ViewerFactory(ps) - -vf.loadObjectModel(Box, "box") -vf.loadEnvironmentModel(Environment, "kitchen_area") - -robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) - -# Initialization. - -# Set parameters. -# robot.client.basic.problem.resetRoadmap () -ps.setErrorThreshold(1e-3) -ps.setMaxIterProjection(40) - -# Generate initial and goal configuration. -q_init = robot.getCurrentConfig() -rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] -q_init[rank] = 0.5 -rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] -q_init[rank] = 0.5 -q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["pr2/torso_lift_joint"] -q_init[rank] = 0.2 -rank = robot.rankInConfiguration["box/root_joint"] -q_init[rank : rank + 3] = [-2.5, -4, 0.746] - -# Put box in right orientation -q_init[rank + 3 : rank + 7] = [0, -sqrt(2) / 2, 0, sqrt(2) / 2] - -q_goal = q_init[::] -q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration["box/root_joint"] -q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746] - -# Create the constraints. -locklhand = ["l_l_finger", "l_r_finger"] -ps.createLockedJoint( - "l_l_finger", - "pr2/l_gripper_l_finger_joint", - [ - 0.5, - ], -) -ps.createLockedJoint( - "l_r_finger", - "pr2/l_gripper_r_finger_joint", - [ - 0.5, - ], -) - -# Create the constraint graph. -# Define the set of grippers used for manipulation -grippers = [ - "pr2/l_gripper", -] -# Define the set of objects that can be manipulated -objects = [ - "box", -] -# Define the set of handles for each object -handlesPerObject = [ - [ - "box/handle2", - ], -] -# Define the set of contact surfaces used for each object -contactSurfacesPerObject = [ - [ - "box/box_surface", - ], -] -# Define the set of contact surfaces of the environment used to put objects -envContactSurfaces = [ - "kitchen_area/pancake_table_table_top", -] -# Define rules for associating grippers and handles (here all associations are -# allowed) -rules = [ - Rule([".*"], [".*"], True), -] - -cg = ConstraintGraph(robot, "graph") -factory = ConstraintGraphFactory(cg) -factory.setGrippers(grippers) -factory.environmentContacts(envContactSurfaces) -factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject) -factory.setRules(rules) -factory.generate() -cg.addConstraints(graph=True, constraints=Constraints(numConstraints=locklhand)) -cg.initialize() - -ps.setInitialConfig(q_init) -ps.addGoalConfig(q_goal) - -# uncomment to solve -# ps.solve() - -# Path optimization uncomment to optimize -# -# ps.loadPlugin('manipulation-spline-gradient-based.so') -# ps.addPathOptimizer('SplineGradientBased_bezier1') -# ps.optimizePath(0) - -# display in gepetto-gui -# v = vf.createViewer () diff --git a/script/tutorial_4.py b/script/tutorial_4.py deleted file mode 100644 index f11e418..0000000 --- a/script/tutorial_4.py +++ /dev/null @@ -1,135 +0,0 @@ -from hpp.gepetto.manipulation import ViewerFactory - -from hpp.corbaserver import loadServerPlugin -from hpp.corbaserver import wrap_delete as wd -from hpp.corbaserver.manipulation import ( - ConstraintGraph, - Constraints, - ProblemSolver, - Robot, - newProblem, -) - -# Specify path for robot urdf and srdf files -Robot.urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur10_robot.urdf" -) -Robot.srdfFilename = "package://example-robot-data/robots/ur_description/srdf/ur5.srdf" - -loadServerPlugin("corbaserver", "manipulation-corba.so") -newProblem() - -robot = Robot("robot", "ur10e", rootJointType="anchor") -ps = ProblemSolver(robot) -vf = ViewerFactory(ps) - -# Add a gripper to the robot -robot.client.manipulation.robot.addGripper( - "ur10e/wrist_3_link", "ur10e/gripper", [0, 0.137, 0, 0.5, 0.5, 0.5, 0.5], 0.1 -) - -# Create two handles -robot.client.manipulation.robot.addHandle( - "ur10e/base_link", "handle1", [0.8, -0.4, 0.5, 0, 0, 0, 1], 0.1, 6 * [True] -) -robot.client.manipulation.robot.addHandle( - "ur10e/base_link", "handle2", [0.8, 0.4, 0.5, 0, 0, 0, 1], 0.1, 6 * [True] -) - -# Create grasp constraints -robot.client.manipulation.problem.createGrasp( - "ur10e/gripper grasps handle1", "ur10e/gripper", "handle1" -) -robot.client.manipulation.problem.createGrasp( - "ur10e/gripper grasps handle2", "ur10e/gripper", "handle2" -) - -# Create a constraint graph with one node for each grasp -cg = ConstraintGraph(robot, "graph") -cg.createNode(["ur10e/gripper grasps handle1", "ur10e/gripper grasps handle2"]) - -cg.addConstraints( - node="ur10e/gripper grasps handle1", - constraints=Constraints(numConstraints=["ur10e/gripper grasps handle1"]), -) -cg.addConstraints( - node="ur10e/gripper grasps handle2", - constraints=Constraints(numConstraints=["ur10e/gripper grasps handle2"]), -) -cg.initialize() - -# Generate one configuration satisfying each constraint -found = False -while not found: - q0 = robot.shootRandomConfig() - res, q1, err = cg.applyNodeConstraints("ur10e/gripper grasps handle1", q0) - if not res: - continue - res, msg = robot.isConfigValid(q1) - if not res: - continue - res, q2, err = cg.applyNodeConstraints("ur10e/gripper grasps handle2", q1) - if not res: - continue - res, msg = robot.isConfigValid(q2) - if not res: - continue - found = True - -# Create an EndEffectorTrajectory steering method -cmp = wd(ps.client.basic.problem.getProblem()) -crobot = wd(cmp.robot()) -cproblem = wd(ps.client.basic.problem.createProblem(crobot)) -csm = wd( - ps.client.basic.problem.createSteeringMethod("EndEffectorTrajectory", cproblem) -) -cs = wd(ps.client.basic.problem.createConstraintSet(crobot, "sm-constraints")) -cp = wd(ps.client.basic.problem.createConfigProjector(crobot, "cp", 1e-4, 40)) -cs.addConstraint(cp) -cproblem.setConstraints(cs) -cproblem.setSteeringMethod(csm) - -# Create a new grasp constraint for the steering method right hand side -# The previously created one has EqualToZero as comparison types. -robot.client.manipulation.problem.createGrasp( - "end-effector-tc", "ur10e/gripper", "handle1" -) -# Set comparison type to Equality -ps.setConstantRightHandSide("end-effector-tc", False) -tc = wd(ps.client.basic.problem.getConstraint("end-effector-tc")) -cp.add(tc, 0) - -csm.trajectoryConstraint(tc) -# Get right hand side for q1 and q2 -rhs1 = tc.function().value(q1) -rhs2 = tc.function().value(q2) -# Create linear path for end-effector -p = wd(csm.makePiecewiseLinearTrajectory([rhs1, rhs2], 6 * [1.0])) -# Set this path as the time-varying right hand side of the constraint -csm.trajectory(p, True) - -# Call steering method -p1 = wd(csm.call(q1, q2)) -if p1: - ps.client.basic.problem.addPath(p1.asVector()) - -# Notice that the path is discontinuous. - -# Using EndEffectorTrajectory path planner -cdistance = wd(cproblem.getDistance()) -croadmap = wd(ps.client.basic.problem.createRoadmap(cdistance, crobot)) -cplanner = wd( - ps.client.basic.problem.createPathPlanner( - "EndEffectorTrajectory", cproblem, croadmap - ) -) -cplanner.setNRandomConfig(0) -cplanner.maxIterations(1) -cplanner.setNDiscreteSteps(20) - -cproblem.setInitConfig(q1) -cproblem.addGoalConfig(q2) - -p2 = wd(cplanner.solve()) -if p2: - ps.client.basic.problem.addPath(p2) diff --git a/script/tutorial_kinodynamic.py b/script/tutorial_kinodynamic.py deleted file mode 100644 index bc7a2b5..0000000 --- a/script/tutorial_kinodynamic.py +++ /dev/null @@ -1,123 +0,0 @@ -from hpp.corbaserver.robot import Robot -from hpp.gepetto import PathPlayer, ViewerFactory - -from hpp.corbaserver import ProblemSolver - -# This tutorial shows how to use kinodynamic motion planning methods. -# In the current implementation, only the translation part of a freeflyer is considered -# by the Kinodynamic methods, all the other degree of freedom use classical geometrical -# methods. - - -# Take a rod with a freeflyer base as robot -class RobotRod(Robot): - rootJointType = "freeflyer" - packageName = "hpp_tutorial" - meshPackageName = "hpp_tutorial" - urdfName = "rod" - urdfSuffix = "" - srdfSuffix = "" - - def __init__(self, robotName, load=True): - Robot.__init__(self, robotName, self.rootJointType, load) - self.tf_root = "base_footprint" - - -robot = RobotRod("rod") - -robot.setJointBounds("root_joint", [-7, 6.5, -7, 7, 0.4, 0.4]) - -# Kinodynamic methods need at least 6 extraConfigs, to store the velocity (first 3) and -# acceleration (last 3) of the translation of the root -robot.client.robot.setDimensionExtraConfigSpace(6) -# set the bounds for velocity and acceleration : -aMax = 1.0 -vMax = 2.0 -robot.client.robot.setExtraConfigSpaceBounds( - [-vMax, vMax, -vMax, vMax, 0, 0, -aMax, aMax, -aMax, aMax, 0, 0] -) - - -ps = ProblemSolver(robot) -# define the velocity and acceleration bounds used by the steering method. This bounds -# will be stastified along the whole trajectory. -ps.setParameter("Kinodynamic/velocityBound", vMax) -ps.setParameter("Kinodynamic/accelerationBound", aMax) -ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) -# Uncomment the following line if you want to constraint the orientation of the base of -# the robot to follow the direction of the motion. Note that in this case the initial -# and final orientation are not considered. -# ps.setParameter("Kinodynamic/forceOrientation",True) - -# The following line constraint the random sampling method to fix all the extraDOF at 0 -# during sampling. Comment it if you want to sample states with non-null velocity and -# acceleration. Note that it increase the complexity of the problem and greatly increase -# the computation time. -ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) - - -vf = ViewerFactory(ps) - -q_init = robot.getCurrentConfig() -q_goal = q_init[::] - -q_init[0:3] = [6.5, -4, 0.4] # root position -q_init[3:7] = [0, 0, 0, 1] # root rotation -# set initial velocity (along x,y,z axis) : -q_init[-6:-3] = [0, 0, 0] - - -# q_goal[0:3] = [6.5,-1,0.4] # straight line -q_goal[0:3] = [3, -4, 0.4] # easy goal position -# q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position -# set goal velocity (along x,y,z axis) : -q_goal[-6:-3] = [0, 0, 0] - -vf.loadObstacleModel("iai_maps", "room", "room") -# with displayArrow parameter the viewer will display velocity and acceleration of the -# center of the robot with 3D arrow. The length scale with the amplitude with a length -# of 1 for the maximal amplitude. -v = vf.createViewer(displayArrows=True) -ps.setInitialConfig(q_init) -ps.addGoalConfig(q_goal) - -ps.addPathOptimizer("RandomShortcut") -# select kinodynamic methods : -ps.selectSteeringMethod("Kinodynamic") -ps.selectDistance("Kinodynamic") -# the Kinodynamic steering method require a planner that build directionnal roadmap -# (with oriented edges) as the trajectories cannot always be reverse. -ps.selectPathPlanner("BiRRTPlanner") - - -print(ps.solve()) - -# display the computed roadmap. Note that the edges are all represented as straight line -# and may not show the real motion of the robot between the nodes : -v.displayRoadmap("rm") - -# Alternatively, use the following line instead of ps.solve() to display the roadmap as -# it's computed (note that it slow down a lot the computation) -# v.solveAndDisplay('rm',1) - -# Highlight the solution path used in the roadmap -v.displayPathMap("pm", 0) - -# remove the roadmap for the scene : -# v.client.gui.removeFromGroup("rm",v.sceneName) -# v.client.gui.removeFromGroup("pm",v.sceneName) - - -# Connect to a viewer server and play solution paths - -pp = PathPlayer(v) -# play path before optimization -pp(0) - -# Display the optimized path, with a color variation depending on the velocity along the -# path (green for null velocity, red for maximal velocity) -pp.dt = 0.1 -pp.displayVelocityPath(1) -# play path after random shortcut -pp.dt = 0.001 -pp(1) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 00b79bc..6246434 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -25,4 +25,5 @@ install(TARGETS hpp-tutorial-1 DESTINATION ${CMAKE_INSTALL_BINDIR}) include(${HPP_CORE_CMAKE_PLUGIN}) # Create and install the plugin hpp_add_plugin(tutorial-2 SOURCES tutorial_2.cc LINK_DEPENDENCIES - hpp-corbaserver::hpp-corbaserver) + hpp-corbaserver::hpp-corbaserver +) diff --git a/srdf/box.srdf b/srdf/box.srdf index 42553e4..c83e112 100644 --- a/srdf/box.srdf +++ b/srdf/box.srdf @@ -1,18 +1,13 @@ - - + + - - + - - - - -0.015 -0.025 -0.025 -0.015 0.025 -0.025 -0.015 -0.025 0.025 -0.015 0.025 0.025 - +0.015 -0.025 -0.025 +0.015 0.025 -0.025 +0.015 -0.025 0.025 +0.015 0.025 0.025 - 4 0 2 3 1 4 4 5 7 6 + -0.025 -0.025 -0.025 -0.025 0.025 -0.025 0.025 0.025 -0.025 0.025 -0.025 -0.025 + + 4 0 1 2 3 diff --git a/srdf/ground.srdf b/srdf/ground.srdf new file mode 100644 index 0000000..e2dddc5 --- /dev/null +++ b/srdf/ground.srdf @@ -0,0 +1,8 @@ + + + + 1 -1 0 1 1 0 -1 1 0 -1 -1 0 + + 4 0 1 2 3 + + diff --git a/srdf/panda.srdf b/srdf/panda.srdf new file mode 100644 index 0000000..2a0056c --- /dev/null +++ b/srdf/panda.srdf @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/srdf/staubli-drill.srdf b/srdf/staubli-drill.srdf new file mode 100644 index 0000000..e7fd133 --- /dev/null +++ b/srdf/staubli-drill.srdf @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/tutorial_1/Dockerfile b/tutorial_1/Dockerfile new file mode 100644 index 0000000..9e19398 --- /dev/null +++ b/tutorial_1/Dockerfile @@ -0,0 +1,44 @@ +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 +RUN curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc +RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub noble robotpkg" > /etc/apt/sources.list.d/robotpkg.list +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 \ + 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 + +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 addgroup --gid $DOCKER_GROUP user +RUN adduser --uid $DOCKER_USER --gid $DOCKER_GROUP user + +USER user +RUN mkdir /home/user/devel + +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 diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile new file mode 100644 index 0000000..e754a2b --- /dev/null +++ b/tutorial_1/Makefile @@ -0,0 +1,236 @@ +# +# Copyright (c) 2014 CNRS-LAAS +# Author: Florent Lamiraux +# + +HPP_REPO=https://github.com/humanoid-path-planner +FLORENT_REPO=https://github.com/florent-lamiraux +JRL_REPO=https://github.com/jrl-umi3218 + +SRC_DIR=${DEVEL_HPP_DIR}/src +ifndef INSTALL_HPP_DIR +INSTALL_HPP_DIR=${DEVEL_HPP_DIR}/install +endif + +BUILD_TYPE=Release +BUILD_TESTING=OFF +ifeq (${BUILD_TYPE},Debug) + BUILD_FOLDER=build +else + BUILD_FOLDER=build-rel + BUILD_TESTING=OFF +endif + +WGET=wget --quiet +UNZIP=unzip -qq +TAR=tar +GIT_QUIET=--quiet +# Qt version should be either 4 or 5 +QT_VERSION=5 +INSTALL_DOCUMENTATION=ON +PYTHON_FLAGS=-DPYTHON_STANDARD_LAYOUT=ON + +################################## +# {{{ Dependencies + +HPP_EXTRA_FLAGS= -DBUILD_TESTING=${BUILD_TESTING} + +jrl-cmakemodules_branch=master +jrl-cmakemodules_repository=${JRL_REPO} + +hpp-python_branch=devel +hpp-python_repository=${HPP_REPO} +hpp-python_extra_flags=${HPP_EXTRA_FLAGS} ${PYTHON_FLAGS} + +hpp-doc_branch=devel +hpp-doc_repository=${HPP_REPO} + +hpp_tutorial_branch=devel +hpp_tutorial_repository=${FLORENT_REPO} +hpp_tutorial_extra_flags=${HPP_EXTRA_FLAGS} ${PYTHON_FLAGS} + +hpp-gepetto-viewer_branch=devel +hpp-gepetto-viewer_repository=${HPP_REPO} +hpp-gepetto-viewer_extra_flags= ${PYTHON_FLAGS} -DUSE_HPP_PYTHON=ON -DINSTALL_DOCUMENTATION=OFF + +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 + +# }}} +################################## +# {{{ 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= + +# }}} +################################## +# {{{ High-level targets + +all: hpp-plot.install hpp_tutorial.install hpp-toppra.install + ${MAKE} hpp-doc.install + +# }}} +################################## +# {{{ python virtual environment +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}; \ + ${INSTALL_HPP_DIR}/bin/pip install --prefix=${INSTALL_HPP_DIR} \ + "numpy==1.26.4" trimesh pycollada viser; \ + fi + +# }}} +################################## +# {{{ Dependencies declaration + +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_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 + +# }}} +################################## +# {{{ Targets + +status: + @for child_dir in $$(ls ${SRC_DIR}); do \ + test -d "$$child_dir" || continue; \ + test -d "$$child_dir/.git" || continue; \ + ${MAKE} "$$child_dir".status; \ + done + +log: + @for child_dir in $$(ls ${SRC_DIR}); do \ + test -d "$$child_dir" || continue; \ + test -d "$$child_dir/.git" || continue; \ + ${MAKE} "$$child_dir".log; \ + done + +fetch: + @for child_dir in $$(ls ${SRC_DIR}); do \ + test -d "$$child_dir" || continue; \ + test -d "$$child_dir/.git" || continue; \ + ${MAKE} "$$child_dir".fetch; \ + done + +update: + @for child_dir in $$(ls ${SRC_DIR}); do \ + test -d "$$child_dir" || continue; \ + test -d "$$child_dir/.git" || continue; \ + ${MAKE} "$$child_dir".update; \ + done + +%.checkout: + if [ -d $(@:.checkout=) ]; then \ + echo "$(@:.checkout=) already checkout out."; \ + else \ + git clone ${GIT_QUIET} --recursive -b ${$(@:.checkout=)_branch} ${$(@:.checkout=)_repository}/$(@:.checkout=); \ + fi \ + +%.fetch: + if [ "${$(@:.fetch=)_repository}" = "" ]; then \ + echo "$(@:.fetch=) is not referenced"; \ + else \ + cd ${SRC_DIR}/$(@:.fetch=);\ + git fetch ${GIT_QUIET} origin; \ + git fetch ${GIT_QUIET} origin --tags; \ + fi + +%.update: + if [ "${$(@:.update=)_repository}" = "" ]; then \ + echo "$(@:.update=) is not referenced"; \ + else \ + cd ${SRC_DIR}/$(@:.update=);\ + git remote rm origin;\ + git remote add origin ${$(@:.update=)_repository}/$(@:.update=);\ + git fetch origin;\ + git fetch origin --tags;\ + git checkout -q --detach;\ + git branch -f ${$(@:.update=)_branch} origin/${$(@:.update=)_branch};\ + git checkout -q ${$(@:.update=)_branch};\ + git submodule update; \ + fi + + +%.configure.dep: %.checkout + +%.configure: %.configure.dep + ${MAKE} $(@:.configure=).configure_nodep + +%.configure_nodep:%.checkout + mkdir -p ${SRC_DIR}/$(@:.configure_nodep=)/${BUILD_FOLDER}; \ + cd ${SRC_DIR}/$(@:.configure_nodep=)/${BUILD_FOLDER}; \ + cmake -DCMAKE_INSTALL_PREFIX=${INSTALL_HPP_DIR} -DCMAKE_INSTALL_LIBDIR=lib -DCMAKE_BUILD_TYPE=${BUILD_TYPE} \ + -DENFORCE_MINIMAL_CXX_STANDARD=ON \ + -DINSTALL_DOCUMENTATION=${INSTALL_DOCUMENTATION} \ + -DCMAKE_CXX_FLAGS_RELWITHDEBINFO="-g -O3 -DNDEBUG" \ + ${CLANG_FLAGS} \ + ${$(@:.configure_nodep=)_extra_flags} .. + +%.install:%.configure + ${MAKE} -C ${SRC_DIR}/$(@:.install=)/${BUILD_FOLDER} install + +%.install_nodep:%.configure_nodep + ${MAKE} -C ${SRC_DIR}/$(@:.install_nodep=)/${BUILD_FOLDER} install + +%.uninstall: + ${MAKE} -C ${SRC_DIR}/$(@:.uninstall=)/${BUILD_FOLDER} uninstall + +%.clean: + ${MAKE} -C ${SRC_DIR}/$(@:.clean=)/${BUILD_FOLDER} clean + +%.very-clean: + rm -rf ${SRC_DIR}/$(@:.very-clean=)/${BUILD_FOLDER}/* + +%.status: + @cd ${SRC_DIR}/$(@:.status=); \ + echo \ + "\033[1;36m------- Folder $(@:.status=) ---------------\033[0m"; \ + git --no-pager -c status.showUntrackedFiles=no status --short --branch;\ + +%.log: + @cd ${SRC_DIR}/$(@:.log=); \ + if [ -f .git/refs/heads/${$(@:.log=)_branch} ]; then \ + echo -n "$(@:.log=): "; \ + cat .git/refs/heads/${$(@:.log=)_branch}; \ + fi + +toppra.configure_nodep:toppra.checkout + mkdir -p ${SRC_DIR}/$(@:.configure_nodep=)/cpp/${BUILD_FOLDER}; \ + cd ${SRC_DIR}/$(@:.configure_nodep=)/cpp/${BUILD_FOLDER}; \ + cmake -DCMAKE_INSTALL_PREFIX=${INSTALL_HPP_DIR} -DCMAKE_INSTALL_LIBDIR=lib -DCMAKE_BU\ +ILD_TYPE=${BUILD_TYPE} \ + -DENFORCE_MINIMAL_CXX_STANDARD=ON \ + -DINSTALL_DOCUMENTATION=${INSTALL_DOCUMENTATION} \ + -DCMAKE_CXX_FLAGS_RELWITHDEBINFO="-g -O3 -DNDEBUG" \ + ${CLANG_FLAGS} \ + ${$(@:.configure_nodep=)_extra_flags} .. + +toppra.install:toppra.configure + ${MAKE} -C ${SRC_DIR}/$(@:.install=)/cpp/${BUILD_FOLDER} install + +toppra.install_nodep:toppra.configure_nodep + ${MAKE} -C ${SRC_DIR}/$(@:.install_nodep=)/cpp/${BUILD_FOLDER} install diff --git a/tutorial_1/README.md b/tutorial_1/README.md new file mode 100644 index 0000000..4fedfd8 --- /dev/null +++ b/tutorial_1/README.md @@ -0,0 +1,69 @@ +# Installing the software + +This tutorial provides a quick procedure to install the software in docker. For general installation instructions, please visit the [project page](https://humanoid-path-planner.github.io/hpp-doc/download.html) + +## Prerequisite + + You need a machine running linux with docker installed. See https://docs.docker.com/engine/install/ubuntu for docker installation instructions. + +## Installing HPP in a docker image + +We will build a minimal docker image containing HPP software from binary packages. Then we will run the image in a container that shares a directory with the host machine. Docker will create a user in the container. In order to avoid file ownership conflicts between this user and your account on your machine, we will make them use the same user id and group. + + 1. Create an empty directory on your machine that will be shared by the container and `cd` into this directory. + 2. Download the Dockerfile to build the docker image. + +``` +wget https://raw.githubusercontent.com/humanoid-path-planner/hpp_tutorial/refs/heads/devel/tutorial_1/Dockerfile +``` + + 3. Download a lightweight web browser + +``` +wget https://github.com/goastian/midori-desktop/releases/download/v11.6/midori_11.6-1_amd64.deb +``` + + 4. Build the docker image + +``` +docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp:tuto -f Dockerfile . +``` + + 5. Create directory `src` + +``` +mkdir src +``` + + 6. get configuration file and compilation file + +``` +wget https://raw.githubusercontent.com/humanoid-path-planner/hpp_tutorial/refs/heads/devel/tutorial_1/config.sh +wget -O src/Makefile https://raw.githubusercontent.com/humanoid-path-planner/hpp_tutorial/refs/heads/devel/tutorial_1/Makefile +``` + + 7. Get script that will run docker + +``` +wget https://raw.githubusercontent.com/humanoid-path-planner/hpp_tutorial/refs/heads/devel/tutorial_1/run_docker.sh +``` + + 8. Run the container + +``` +chmod 775 run_docker.sh +./run_docker.sh +``` + + 9. In the docker container, install a few things. + +``` +cd /home/user/devel/src +make all +``` + +Everything is now installed. You can now exit the container by typing CTRL-D and proceed to [tutorial 2](../tutorial_2/README.md). + +### Note + +The docker container shares the current directory in your host machine with `/home/user/devel`. As a consequence, if you restart it later, you do not need to redo the previous steps. diff --git a/tutorial_1/config.sh b/tutorial_1/config.sh new file mode 100644 index 0000000..dbdc4f2 --- /dev/null +++ b/tutorial_1/config.sh @@ -0,0 +1,17 @@ +export INSTALL_HPP_DIR=$DEVEL_HPP_DIR/install +export INSTALL_PIP_DIR=$HOME/install-pip +export ROBOTPKG=/opt/openrobots + +export PATH=$INSTALL_HPP_DIR/bin:$ROBOTPKG/bin:$PATH +export PKG_CONFIG_PATH=$INSTALL_HPP_DIR/lib/pkgconfig/:$ROBOTPKG/lib/pkgconfig + +export PYTHONPATH=$INSTALL_HPP_DIR/lib/python3.12/site-packages:$INSTALL_PIP_DIR/lib/python3.12/site-packages:$ROBOTPKG/lib/python3.12/site-packages:$PYTHONPATH + +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 + +if [ -f "${INSTALL_HPP_DIR}/etc/hpp-tools/bashrc" ]; then + source "${INSTALL_HPP_DIR}/etc/hpp-tools/bashrc" +fi diff --git a/tutorial_1/run_docker.sh b/tutorial_1/run_docker.sh new file mode 100644 index 0000000..2a4530a --- /dev/null +++ b/tutorial_1/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:tuto diff --git a/tutorial_2/README.md b/tutorial_2/README.md new file mode 100644 index 0000000..2442a44 --- /dev/null +++ b/tutorial_2/README.md @@ -0,0 +1,286 @@ +# Solving a simple pick and place task with HPP + +This tutorial will help you defining and solving a simple pick and place task with a franka robot. + +## Prerequisite + +Having installed HPP by following the steps described in [tutorial 1](../tutorial_1/README.md). + +## Resources + + The main concepts and theory implemented in HPP software are described in the following paper: https://laas.hal.science/hal-02995125v2. + + You can access the C++ online API documentation [here](https://gepetto.github.io/doc/hpp-doc/doxygen-html/index.html) + +## Starting the container and opening new terminals + +To open again the container, in the same directory as in [tutorial 1](../tutorial_1/README.md), simply run the command + +``` +./run_docker.sh +``` +Then in another terminal, type +``` +docker exec -it hpp bash +``` +This will open another bash terminal in the container. You can do this as many times as you want to open new terminals in the container. + +In the new terminal, open `midori` web browser: +``` +midori +``` + +## Building and displaying a robot + +In the first terminal, go into directory `hpp_tutorial/tutorial_2` and run the following python script +``` +cd /home/user/devel/src/hpp_tutorial/tutorial_2 +python -i init.py +``` +You are now in an interactive python terminal. To display the robot that has been loaded by the script, type +``` +v = Viewer(robot) +v.initViewer(open=False, loadModel=True) +v(q) +``` +Then in midori address bar, type `http://localhost:8080`. You should see the panda robot. + +You can have a quick look at the script to see the instructions used to define the robot. + +## Adding an obstacle to the scene + +In the python terminal, copy-paste the following instructions +``` +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()) +v = Viewer(robot) +v.initViewer(open=False, loadModel=True) +v(q) +``` + +## Adding an object to the scene + +In the python terminal, copy-paste the following instructions +``` +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()) +q = neutral(robot.model()) +q[7:9] = [0.035, 0.035] +q[9:12] = [.4, -0.2, 0.025] +v = Viewer(robot) +v.initViewer(open=False, loadModel=True) +v(q) +``` + +### Setting bounds to the object translation + +Latter on, we will uniformly sample random configurations. To make it possible, we need to set bounds to the translation of the object. +``` +robot.setJointBounds("box/root_joint", [-1.5, 1.5, + -1.5, 1.5, + -0.2, 1.5, + -float("Inf"), float("Inf"), + -float("Inf"), float("Inf"), + -float("Inf"), float("Inf"), + -float("Inf"), float("Inf")]) +``` +The first six values are the minimal and maximal values along x, y, z axes. The eight last +values apply to the quaternion coefficients that need not being bounded. + +## Explaining the code + +Let us have a quick look at the code in `init.py`. +``` +robot = Device("tuto") +``` +This line creates an empty robot implementing + - a `pinocchio` model for the kinematic chain, + - a `pinocchio` geometric model for collision, and + - a `pinocchio` geometric model for visual display. + +The following lines populate the kinematic chain with a panda robot: +``` +urdf_filename = "package://example-robot-data/robots/panda_description/urdf/panda.urdf" +srdf_filename = "package://hpp_tutorial/srdf/panda.srdf" + +urdf.loadModel(robot, 0, "panda", "anchor", urdf_filename, srdf_filename, SE3.Identity()) +``` +The arguments of the latter functions are the following: + - `robot`: the kinematic chain to which the new robot will be appended, + - `0`: the index of the frame to which the new robot is appended (here the world frame), + - `"anchor"`: the type of mobility of the new robot `base_link` (here the robot is fixed to the environment), + - `urdf_filename`: URDF file that describes the new robot, + - `srdf_filename`: SRDF file that adds some information about the new robot, like collision pairs, and some information we will explain later on, + - `SE3.Identity()` pose of the new robot in the world frame. + +The following lines of code retrieve the neutral configuration of the robot (all joint set to 0) and open the gripper of the panda. +``` +# Get neutral configuration of robot +q = neutral(robot.model()) +# Open the gripper +q[-2:] = [0.035, 0.035] +``` +Note that adding the ground and a box use the same function. The box is added with "freeflyer" as the mobility type. Therefore, the box is part of the kinematic chain and `neutral` returns a vector of dimension 16 (9 for the panda, 7 for the box). + +## Explaining the SRDF files + +SRDF follows the xml syntax. Some specific tags are parsed by HPP to add information to the robot and object models. Let us now have a look at the srdf files that are used above by function `urdf.loadModel` Note that all srdf files used above can be found in the `srdf` directory of the package. + +In file `panda.srdf`, +``` + + + + + +``` +defines a gripper, that is a frame attached to link `panda_hand_tcp`. The relative pose of the frame with respect to the link is defined by tag `position`. Attribute `xyz` defines the translation, attribute `wxyz` defines the rotation as a unit quaternion with real part in first position. +Attribute `clearance` will be used later on to define pregrasp configurations. + +In file `box.srdf`, +``` + + + + +``` +defines a handle, that is a frame attached to link `base_link`. he relative pose of the frame with respect to the link is defined by tag `position`. Note that the z axis of the handle points downwards when the object is placed on a horizontal contact surface. Tag `approaching_direction` will be used by used later on by the constraint graph factory to generate pregrasp states. +``` + + + -0.025 -0.025 -0.025 -0.025 0.025 -0.025 0.025 0.025 -0.025 0.025 -0.025 -0.025 + + 4 0 1 2 3 + +``` +defines a contact surface used to define stable poses of the object. The surface is a plane polygon defined by the vertices (`point` tag). Here we define 1 polygon with 4 vertices in order `0 1 2 3` (`shape` tag). + +Note that `box.srdf` also defines a contact surface. The box is in stable contact pose when the contact surfaces are coplanar with opposite normal and the center of the box surface is included in the polygonal surface of the ground. + +## A quick tour of the viewer + +In the upper right corner of the viewer, there is the following window to control the view. We call this window the **view controller**. + +![Control of the view](figures/viewer-control.png) + +Move the mouse to the icon representing three vertical sliders in the upper right corner, "Configuration and diagnostics" should pop up. Click on the icon and you will see the scene tree. Clicking again brings you back to the initial widget. Select tab "Display" in the upper part of the view controller and select "Show Visuals", "Show Frames" and "Show Contact Surfaces". Many frames are displayed, as well as the contact surfaces defined in the SRDF files. + +Go again to the scene tree. In the tree, expand "pinocchio" then "frames", then "panda". By clicking on the eye in front of each node, you remove the corresponding frame from the view. Remove them all except "panda/gripper" and "box/handle". You now see the gripper and handle frames that will define how the robot grasps the object as in the picture below. + +![Panda robot and box with gripper and handle](figures/panda-view.png) + +## Defining a manipulation problem + +The manipulation problem is defined through a variable of type `Problem` and through the constraint graph that will be constructed by a factory. Let us define those variables. +``` +from pyhpp.manipulation import (Graph, Problem, ManipulationPlanner) +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory + +problem = Problem(robot) +graph = Graph("robot", robot, problem) +factory = ConstraintGraphFactory(graph) +``` +Later on the graph will solve numerical constraints. We need to set the error threshold and the maximal number of iterations. +``` +graph.maxIterations(40) +graph.errorThreshold(1e-5) +``` +Then, we need to define the set of grippers that will be used. +``` +factory.setGrippers(["panda/gripper"]) +``` +We also define the list of objects and for each of them, the list of handles and the list of contact surfaces. +``` +objects = ["box"] +handlesPerObject = [["box/handle"]] +contactsPerObject = [["box/surface"]] +factory.setObjects(objects, handlesPerObject, contactsPerObject) +``` +Finally, we define the set of environment contact surfaces (surfaces on which objects may be placed). +``` +factory.environmentContacts(["ground/surface"]) +``` +Then we ask the factory to generate the constraint graph corresponding to the manipulation problem. +``` +factory.generate() +``` +Before initializing the graph, we will add a constraint to all states and transitions in order to keep the gripper open all the time. +``` +import numpy as np +from pyhpp.constraints import (ComparisonType, ComparisonTypes, LockedJoint) +cts = ComparisonTypes() +cts[:] = [ComparisonType.EqualToZero] +lockedFingers = list() +for i in range(2): + lj = LockedJoint(robot, f'panda/panda_finger_joint{i+1}', np.array([0.035]), cts) + lockedFingers.append(lj) + +graph.addNumericalConstraintsToGraph(lockedFingers) +graph.initialize() +``` + +### Displaying the constraint graph + +In the python terminal, copy paste the following lines +``` +v.setProblem(problem) +v.setGraph(graph) +``` +In the view controller, click on the button "Show Graph Viewer". The window below should appear. +![Graph Viewer](figures/graph-viewer.png) + +This window displays the constraint graph. + +If you click on show waypoints, you will see **waypoint states** as on the picture below. Those intermediate states ease manipulation planning by forcing the robot to approach the object from above and lift it above the ground before moving it. The distance of the gripper to the object in **pregrasp** state is the sum of the gripper and handle clearances defined in the SRDF files. + +![Graph Viewer](figures/graph-viewer-wp.png) + +By clicking on a state, you can display the constraints of this state. By right clicking on a state, you can select "Generate random config" and figure out what the additional states are meant for. Note that this operation may fail if the numerical solver does not converge and that it does not care for collisions. + +### Back to the manipulation problem + +To define a manipulation problem, we now simply need to set the initial and goal configurations. To do so, we move the robot in a collision-free configuration and change only the pose of the box between those configurations +``` +q_init = np.array([ 0. , 0. , 0. , -0.5 , 0. , 0.5 , 0. , 0.035, + 0.035, 0.4 , -0.2 , 0.0251, 0. , 0. , 0. , 1. ]) +q_goal = np.array([ 0. , 0. , 0. , -0.5 , 0. , 0.5 , 0. , 0.035, + 0.035, 0.4 , 0.2 , 0.0251, 0. , 0. , 0. , 1. ]) +``` +You can display these configurations with +``` +v(q_init) +v(q_goal) +``` + +## Solving the manipulation problem + +To define and solve the problem, +``` +from pyhpp.manipulation import ManipulationPlanner +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) +problem.constraintGraph(graph) +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(500) +p = manipulationPlanner.solve() +``` + +### Displaying the resulting path + +You can display the resulting path in the graphical interface by first loading it +``` +v.loadPath(p) +``` +and then by clicking on "Play" in the view controller. + +## optimizing paths + +Random sampling method usually compute paths that are far from being optimal. These paths can be post-processed to make them shorter. + +``` +from pyhpp.core import RandomShortcut +optimizer = RandomShortcut(problem) +p1 = optimizer.optimize(p) +``` diff --git a/tutorial_2/figures/graph-viewer-wp.png b/tutorial_2/figures/graph-viewer-wp.png new file mode 100644 index 0000000..eedd9aa Binary files /dev/null and b/tutorial_2/figures/graph-viewer-wp.png differ diff --git a/tutorial_2/figures/graph-viewer.png b/tutorial_2/figures/graph-viewer.png new file mode 100644 index 0000000..4c9e38c Binary files /dev/null and b/tutorial_2/figures/graph-viewer.png differ diff --git a/tutorial_2/figures/panda-view.png b/tutorial_2/figures/panda-view.png new file mode 100644 index 0000000..46fd841 Binary files /dev/null and b/tutorial_2/figures/panda-view.png differ diff --git a/tutorial_2/figures/viewer-control.png b/tutorial_2/figures/viewer-control.png new file mode 100644 index 0000000..44b8aa8 Binary files /dev/null and b/tutorial_2/figures/viewer-control.png differ diff --git a/tutorial_2/init.py b/tutorial_2/init.py new file mode 100644 index 0000000..f276734 --- /dev/null +++ b/tutorial_2/init.py @@ -0,0 +1,17 @@ +from pinocchio import SE3, neutral +from pyhpp.manipulation import Device, urdf +from pyhpp_viser import Viewer # noqa: F401 + +robot = Device("tuto") + +urdf_filename = "package://example-robot-data/robots/panda_description/urdf/panda.urdf" +srdf_filename = "package://hpp_tutorial/srdf/panda.srdf" + +urdf.loadModel( + robot, 0, "panda", "anchor", urdf_filename, srdf_filename, SE3.Identity() +) + +# Get neutral configuration of robot +q = neutral(robot.model()) +# Open the gripper +q[-2:] = [0.035, 0.035] diff --git a/tutorial_3/README.md b/tutorial_3/README.md new file mode 100644 index 0000000..1fc763a --- /dev/null +++ b/tutorial_3/README.md @@ -0,0 +1,234 @@ +# How to use HPP in manufacturing. + +## Prerequisite + +Having installed HPP by following the steps described in [tutorial 1](../tutorial_1/README.md). + +## Introduction + +In many manufacturing applications, we often need to bring a tool mounted on a robot end effector to a predefined pose with respect to a part. This tutorial shows how to use the same concepts as in [tutorial 2](../tutorial_2/README.md) to perform a simplified drilling task. + +## Initializing the problem + +In the docker container, cd into `tutorial_3` directory. In a bash terminal, run + +``` +python -i init.py +``` + +to load the models and initialize the path planning problem. The script is similar to tutorial_2 example, except that + + - the robot is a Stäubli TX2-90 holding a drill, + - the object is a vertical square plate, + - a function `display()` creates and initializes the viewer client, + - a handle is added to the object via python and not via the SRDF file. + +The handle is created and added by the following lines +``` +# 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]) +``` +the pose of the handle in the link frame is provided by an object of type `SE3` from `pinocchio` module, initialized by a rotation matrix and a translation vector. Note that the link name and the handle names are prefixed by the name of the object that holds the handle: `"plate/base_link", "plate/hole"`. Parameter `0.1` is the handle clearance. + +The code above is equivalent to the following lines in an SRDF file: +``` + + + + +``` + +## Creating waypoint configurations + +To plan a drilling motion, we will proceed in several steps: + 1. generate a "pregrasp" (qpg) and a "grasp" (qg) configurations, + 2. plan motions between q_init and qpg, then plan a motion between qpg and qg. + +For step 1, first copy paste the following code into the python terminal: +``` +# Generate configuration in pregrasp reachable from q_init and in grasp reachable from pregrasp +transition = graph.getTransition("staubli/tooltip > plate/hole | f_01") +pv = transition.pathValidation() +res, qpg, err = graph.generateTargetConfig(transition, q_init, q_init) # -> failure +if not res: + print ("Failed to project q_init to pregrasp waypoint state") +``` +The above code tries to project configuration `q_init` to the pregrasp waypoint state "staubli/tooltip > plate/hole | f_pregrasp". Method `graph.generateTargetConfig` uses a solver containing the constraints of transition "staubli/tooltip > plate/hole | f_01" and of the target state "staubli/tooltip > plate/hole | f_pregrasp". In this case the constraints are + - Locked joint "plate/root_joint", + - staubli/tooltip pregrasps plate/hole. + +The right hand side of the first one is initialized with `q_init` (second argument). This simply means that the part shoud be in the same pose as in `q_init`. The second one is a constraint of relative pose between the gripper and the handle: both frames should have the same orientation and the position of the center of the handle in the gripper frame is the product of the sum of gripper and handle clearances by the approaching direction vector. As state above, the solver is initialized with `q_init` (third argument). The output of the function is composed of + - `res`: `True` if the solver succeeded, `False` otherwise, + - `qpg`: the result configuration or the last iteration of the solver in case of failure, + - `err`: the norm of the residual. + +Note that projection fails, this does not mean that the constraints have no solution. Initializing the solver with another configuration may succeed. + +Copy paste now the code below in the python terminal. +``` +for i in range(1000): + 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 + res, report = pv.validateConfiguration(qpg) + if not res: continue + transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") + res, qg, err = graph.generateTargetConfig(transition, qpg, qpg) + if res: break + +# Notice that qg is in collision +res, report = pv.validateConfiguration(qg) +print(report) +``` +The code above tries to generate a pregrasp and a grasp configurations by initializing the pregrasp +solver with a random configuration and the grasp solver with the pregrasp configuration. The pregrasp configuration is tested for collision. If a projection fails or a collision occurs, the loop restart with another random configuration. Note that the grasp configuration is produced by initializing the solver (second call to `graph.generateTargetConfig`) with the pregrasp configuration. This is necessary to make sure that the latter is close to the former and that the robot does not need a long reconfiguration motion to reach grasp from pregrasp. + +You can visualize `qpg` and `qg` in the graphical interface as follows. +``` +v = display() +v(q_init) +v(qpg) +v(qg) +``` + +## Using security margins + +In the previous example, the grasp configuration is in collision since the tool is inside the part. We need to deactivate collision checking between the end-effector and the part on the second part of the motion (transition "staubli/tooltip > plate/hole | f_12"). For that, we will use a python class to handle security margins. + +A security margin is a distance below which two objects are considered as colliding. By extension, + - a negative security margin means that penetration is allowed + - -Inf security margin means that the object are never considered as colliding. + +Path and configurations are checked for collisions by `PathValidation` instances. Each transition of the constraint graph owns a PathValidation (`hpp::core::ContinuousValidation`) instance. Security margins can be defined between pairs of joints by a square matrix of size the number of joints. The value in a cell corresponds to the security margin between the joints corresponding to the row and column indices of the cell. the indices are those in the list returned by +``` +robot.getJointNames() +``` +The code below sets security margins between robots (as set of joints). Copy paste the code in the python terminal +``` +# Defining security margins +from pyhpp.manipulation.security_margins import SecurityMargins +sm = SecurityMargins(problem,factory, ["staubli", "plate"], robot) +sm.setSecurityMarginBetween("staubli", "plate", 0.05) +sm.apply() +``` +Then, if you type +``` +M = graph.getSecurityMarginMatrixForTransition(transition) +print(np.array(M)) +``` +you should see +``` +[[0. 0. 0. 0. 0. 0. 0. 0. ] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0. 0. 0. 0. 0. 0. 0.05] + [0. 0.05 0.05 0.05 0.05 0.05 0.05 0. ]] +``` +and notice that between each joint of the robot and the plate, the security margin is 0.05. + +The following code deactivates collision checking between the robot end effector and the plate along a transition. +``` +transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") +graph.setSecurityMarginForTransition(transition, "staubli/joint_6", "plate/root_joint", + float("-inf")) +``` +Copy paste this code and display again the security margin matrix. + +After modifying the graph, we need to reinitialize it. +``` +graph.initialize() +``` +We can now generate collision free pregrasp and grasp configurations. +``` +# Generate configuration in pregrasp reachable from q_init and in grasp reachable from pregrasp +# and test collision for both. +for i in range(1000): + # 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 + # Project pregrasp configuration on grasp state + transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") + res, qg, err = graph.generateTargetConfig(transition, qpg, qpg) + if not res: continue + # Check the configuration for collision + pv = transition.pathValidation() + res, report = pv.validateConfiguration(qg) + if res: break +``` +You can visualize `qpg` and `qg` in the graphical interface. + +## Planning motions between the waypoint configurations + +![Waypoint configurations `q_init`, `qpg`, `qg` in their respective states.](figures/constraint-graph.svg) + +We now need to plan a motion between `q_init` and `qpg` and between `qpg` and `qg`, keeping the relevant constraints satisfied. Here the only relevant constraint is that the part should not move. To do so, we will use the `TransitionPlanner` class. This class plans motions along a transition, keeping the constraints of the transition satisfied. The right hand side of the parameterizable constraints of the transition is initialized with the initial configuration. The code below shows how to use this class. +``` +from pyhpp.manipulation import TransitionPlanner +planner = TransitionPlanner(problem) +planner.maxIterations(1000) +for i in range(10): + # 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 + # Project pregrasp configuration on grasp state + transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") + res, qg, err = graph.generateTargetConfig(transition, qpg, qpg) + if not res: continue + # Check the configuration for collision + pv = transition.pathValidation() + res, report = pv.validateConfiguration(qg) + if not res: continue + # plan motion between qpg and qg + planner.setTransition(transition) + try: + q_goal = np.zeros((1, robot.configSize()), order='F') + q_goal[0,:] = qg + p2 = planner.planPath(qpg, q_goal, True) + except Exception as exc: + print(f"path planning failed between qpg and qg: {exc}") + continue + if p2: + p3 = p2.reverse() + break +``` +You can now load paths `p1`, `p2`, `p3` in the graphical interface and play them. +``` +v.loadPath(p1) +v.loadPath(p2) +v.loadPath(p3) +``` +The drilling path is a little bit naive since the tool trajectory is a straight line but the result of an interpolation in joint space and the velocities of the robot and tool are not controlled. + +[Tutorial 4](../tutorial_4/README.md) shows how to better control a tool trajectory. diff --git a/tutorial_3/figures/constraint-graph.svg b/tutorial_3/figures/constraint-graph.svg new file mode 100644 index 0000000..0e99f60 --- /dev/null +++ b/tutorial_3/figures/constraint-graph.svg @@ -0,0 +1,241 @@ + + + + + + + + + + robot + + %3->%3 + + + + + %7->%7 + + + + + %3->%3 + + + + + %11->%11 + + + + + %7->%7 + + + + + %11->%11 + + + + + staubli/tooltip grasps plate/hole + + staubli/tooltip > plate/hole | f_pregrasp + + free + q_init + qpg + qg + diff --git a/tutorial_3/init.py b/tutorial_3/init.py new file mode 100644 index 0000000..7696f40 --- /dev/null +++ b/tutorial_3/init.py @@ -0,0 +1,83 @@ +import numpy as np +from pinocchio import SE3, neutral +from pyhpp.manipulation import ( + Device, + Graph, + 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 + + +# 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"), + ], +) + +# 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() +graph.initialize() + +shooter = problem.configurationShooter() diff --git a/tutorial_4/README.md b/tutorial_4/README.md new file mode 100644 index 0000000..c5e3758 --- /dev/null +++ b/tutorial_4/README.md @@ -0,0 +1,71 @@ +# How to control the trajectory of a tool. + +## Prerequisite + +Having completed [tutorial 3](../tutorial_3/README.md) + +## Initializing the problem + +In the docker container, cd into `tutorial_4` directory. In a bash terminal, run + +``` +python -i init.py +``` +The script contains the code of [tutorial_3](../tutorial/tutorial_3) up to the computation of +a path to `qpg`. To plan a path between `qpg` and qg, we first generate a desired trajectory +of the tooltip. + +``` +from pyhpp.constraints import (ComparisonType, ComparisonTypes) + +gripper = robot.grippers()["staubli/tooltip"] +trajConstraint = handle.createGrasp(gripper, "drilling") +cts = ComparisonTypes() +cts[:] = 6*[ComparisonType.Equality] +trajConstraint.comparisonType(cts) +init = trajConstraint.function()(qpg) +goal = np.array([0,0,0,0,0,0,1]) +``` +The code above create a relative pose contraint between the gripper (tooltip) and the handle. +The value of the constraint in SE(3) is the pose of the handle in the frame of the gripper. +`init` is the value of the function in `qpg`. As an element of SE(3), it is not a vector. +``` +print(init.space()) +``` +As shown in the python terminal, it is an element of R^3^ x SO(3) (equivalent to SE(3)). +We can access to the vector representation as follows: +``` +print(init.vector()) +``` +As expected, this corresponds to the pose of the handle in the gripper frame in configuration `qpg`: + a translation of 10cm along z. + +See documentation of class `hpp::pinocchio::LiegroupSpace` for more information. + +``` +from pyhpp.manipulation.steering_method import (Cartesian, makePiecewiseLinearTrajectory) + +weights = 50*np.ones(6) +points = np.zeros(14).reshape((2,7)) +points[0:] = init.vector() +points[1:] = goal +rhs = makePiecewiseLinearTrajectory(points, weights) +``` +The above code creates a straight path in SE(3) from the initial pose of the gripper to the +handle. The duration of the path is tuned by a vector of weights as described in the +documentation of method +`hpp::manipulation::steeringMethod::Cartesian::makePiecewiseLinearTrajectory`. +``` +cartesian = Cartesian(planner.innerProblem()) +cartesian.trajectoryConstraint = trajConstraint +cartesian.setRightHandSide(rhs, True) +res, p2 = cartesian.planPath(qpg) +``` +The trajectory constraint as well as the time-varying right hand side are passed to the +`Cartesian` steering method and a path is computed. +``` +transition = graph.getTransition("staubli/tooltip > plate/hole | f_12") +pv = transition.pathValidation() +res, p3, report = pv.validate(p2, False) +``` +The path is then tested for collision after setting the relevant security margins. diff --git a/tutorial_4/init.py b/tutorial_4/init.py new file mode 100644 index 0000000..c2d09e1 --- /dev/null +++ b/tutorial_4/init.py @@ -0,0 +1,123 @@ +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"), + ], +) + +# 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) +for i in range(10): + # 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 + if res: + break diff --git a/tutorial_5/README.md b/tutorial_5/README.md new file mode 100644 index 0000000..5dcc1e4 --- /dev/null +++ b/tutorial_5/README.md @@ -0,0 +1,3 @@ +# How to optimize and time-parameterize paths. + +Work in progress, please be patient. diff --git a/tutorial_6/README.md b/tutorial_6/README.md new file mode 100644 index 0000000..aded22d --- /dev/null +++ b/tutorial_6/README.md @@ -0,0 +1,3 @@ +# How to execute motions on a real robot. + +Work in progress, please be patient. diff --git a/urdf/box.urdf b/urdf/box.urdf index 31b968d..a45ba29 100644 --- a/urdf/box.urdf +++ b/urdf/box.urdf @@ -10,16 +10,16 @@ - + - - + + - + diff --git a/urdf/ground.urdf b/urdf/ground.urdf new file mode 100644 index 0000000..03dbef6 --- /dev/null +++ b/urdf/ground.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/square-plate.urdf b/urdf/square-plate.urdf new file mode 100644 index 0000000..7869028 --- /dev/null +++ b/urdf/square-plate.urdf @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/urdf/staubli-drill.urdf b/urdf/staubli-drill.urdf new file mode 100644 index 0000000..db0424d --- /dev/null +++ b/urdf/staubli-drill.urdf @@ -0,0 +1,254 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +