diff --git a/CMakeLists.txt b/CMakeLists.txt index 557d885..983f213 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,7 +49,7 @@ 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) @@ -57,35 +57,12 @@ if(NOT FINDPYTHON_ALREADY_CALLED) findpython() endif() -add_subdirectory(src) - set(CATKIN_PACKAGE_SHARE_DESTINATION ${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 @@ -106,19 +83,6 @@ install( 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) - add_library(${PROJECT_NAME} INTERFACE) install( TARGETS ${PROJECT_NAME} diff --git a/README.md b/README.md index 3394cdb..334fb91 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) +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/flake.lock b/flake.lock index a1c93df..ad853d0 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1769996383, - "narHash": "sha256-AnYjnFWgS49RlqX7LrC4uA+sCCDBj0Ry/WOJ5XWAsa0=", + "lastModified": 1772408722, + "narHash": "sha256-rHuJtdcOjK7rAHpHphUb1iCvgkU3GpfvicLMwwnfMT0=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "57928607ea566b5db3ad13af0e57e921e6b12381", + "rev": "f20dc5d9b8027381c474144ecabc9034d6a839a3", "type": "github" }, "original": { @@ -60,11 +60,11 @@ "uv2nix": "uv2nix" }, "locked": { - "lastModified": 1772816781, - "narHash": "sha256-Ac0KEl+8ygy+BnDgczNHgTumw8HpCasp/zJU5Yx3kQs=", + "lastModified": 1773524302, + "narHash": "sha256-0phiMh2h3tZoe3rJGWWKMMJhuLMMHa65hRtgkzbXBJQ=", "owner": "gepetto", "repo": "gazebros2nix", - "rev": "ea8aff2fca6d45fa85fe5e90ef3c71fe0fcc0d12", + "rev": "0eb9151c41ec370e29d4d4ae1640aa4c6c4aa7a7", "type": "github" }, "original": { @@ -81,6 +81,7 @@ "flake-parts" ], "gazebros2nix": "gazebros2nix", + "home-manager": "home-manager", "nix-ros-overlay": [ "gepetto", "gazebros2nix", @@ -107,11 +108,11 @@ ] }, "locked": { - "lastModified": 1772825496, - "narHash": "sha256-ZCgGWufV1suEVlft03k9TGOD190kGRCA3rrO8qsjeQ0=", + "lastModified": 1773834979, + "narHash": "sha256-pxEUfvPZG3lMMiorJxKPt0kjjNE2gQDA3w/cRHvSUX4=", "owner": "gepetto", "repo": "nix", - "rev": "5c1a5edffd02c51e267c42f8dfd36a13c7817950", + "rev": "57036f98001a3b66384cd68ace974d237e3fa78b", "type": "github" }, "original": { @@ -122,11 +123,11 @@ }, "gepetto-lib": { "locked": { - "lastModified": 1770945346, - "narHash": "sha256-L88f+oJbpIkMm9Ln1GP9SFyGztMvnOowbdshQHBeGGs=", + "lastModified": 1772454823, + "narHash": "sha256-jROeQxiRkzpHHAWDAxHJVfW/liA+t6JKDyPMJFqUE9E=", "owner": "Gepetto", "repo": "nix-lib", - "rev": "82ef58cdf50514f6b1fde96b9d5b38fd8d3e83f5", + "rev": "4500b5aa6af20c60011926ad6f42238375779c90", "type": "github" }, "original": { @@ -135,17 +136,39 @@ "type": "github" } }, + "home-manager": { + "inputs": { + "nixpkgs": [ + "gepetto", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1773681845, + "narHash": "sha256-o8hrZrigP0JYcwnglCp8Zi8jQafWsxbDtRRPzuVwFxY=", + "owner": "nix-community", + "repo": "home-manager", + "rev": "0759e0e137305bc9d0c52c204c6d8dffe6f601a6", + "type": "github" + }, + "original": { + "owner": "nix-community", + "ref": "release-25.11", + "repo": "home-manager", + "type": "github" + } + }, "nix-ros-overlay": { "inputs": { "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1771885942, - "narHash": "sha256-TlBFvE4YHNlbhKVkayP/FWBNAAv+yG9APA8vMR+5NBw=", + "lastModified": 1773393994, + "narHash": "sha256-zm24obq1c4ielRR8KlUQ2M5XnNfUIcQjN++9s6CFvxc=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "f891b118c8f4ddb2b6f38d6ce1bfe2f8079552ba", + "rev": "a522c5a050a6d50471a29bbf6a060e2df16abf44", "type": "github" }, "original": { @@ -194,11 +217,11 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1769909678, - "narHash": "sha256-cBEymOf4/o3FD5AZnzC3J9hLbiZ+QDT/KDuyHXVJOpM=", + "lastModified": 1772328832, + "narHash": "sha256-e+/T/pmEkLP6BHhYjx6GmwP5ivonQQn0bJdH9YrRB+Q=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "72716169fe93074c333e8d0173151350670b824c", + "rev": "c185c7a5e5dd8f9add5b2f8ebeff00888b070742", "type": "github" }, "original": { @@ -226,11 +249,11 @@ ] }, "locked": { - "lastModified": 1771423342, - "narHash": "sha256-7uXPiWB0YQ4HNaAqRvVndYL34FEp1ZTwVQHgZmyMtC8=", + "lastModified": 1772555609, + "narHash": "sha256-3BA3HnUvJSbHJAlJj6XSy0Jmu7RyP2gyB/0fL7XuEDo=", "owner": "pyproject-nix", "repo": "build-system-pkgs", - "rev": "04e9c186e01f0830dad3739088070e4c551191a4", + "rev": "c37f66a953535c394244888598947679af231863", "type": "github" }, "original": { @@ -248,11 +271,11 @@ ] }, "locked": { - "lastModified": 1771518446, - "narHash": "sha256-nFJSfD89vWTu92KyuJWDoTQJuoDuddkJV3TlOl1cOic=", + "lastModified": 1773190977, + "narHash": "sha256-fkJvOxz80cJViPz6GVaayC8BVCs5fclJ8qHDaNUpoEA=", "owner": "pyproject-nix", "repo": "pyproject.nix", - "rev": "eb204c6b3335698dec6c7fc1da0ebc3c6df05937", + "rev": "10ebca8a137bf26b7fbd3e94b339bf68cee18693", "type": "github" }, "original": { @@ -369,11 +392,11 @@ ] }, "locked": { - "lastModified": 1770228511, - "narHash": "sha256-wQ6NJSuFqAEmIg2VMnLdCnUc0b7vslUohqqGGD+Fyxk=", + "lastModified": 1773297127, + "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "337a4fe074be1042a35086f15481d763b8ddc0e7", + "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016", "type": "github" }, "original": { @@ -396,11 +419,11 @@ ] }, "locked": { - "lastModified": 1772187362, - "narHash": "sha256-gCojeIlQ/rfWMe3adif3akyHsT95wiMkLURpxTeqmPc=", + "lastModified": 1773359304, + "narHash": "sha256-knv2C6tIk5ysix+9TxWIenPvpB20kFjQ1CH6SJMBNsU=", "owner": "pyproject-nix", "repo": "uv2nix", - "rev": "abe65de114300de41614002fe9dce2152ac2ac23", + "rev": "27b135ea72ab1637fc5845a61c101ea66d6636d6", "type": "github" }, "original": { 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/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..7ff4451 --- /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 + +RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ + 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 diff --git a/tutorial_1/Makefile b/tutorial_1/Makefile new file mode 100644 index 0000000..3ea90f8 --- /dev/null +++ b/tutorial_1/Makefile @@ -0,0 +1,205 @@ +# +# 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 gepetto-gui + +qgv_branch=devel +qgv_repository=${HPP_REPO} +qgv_extra_flags=-DBINDINGS_QT5=ON -DBINDINGS_QT4=OFF + +# }}} +################################## +# {{{ High-level targets + +all: hpp-plot.install hpp_tutorial.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 + +# }}} +################################## +# {{{ 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 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..3677c4b --- /dev/null +++ b/tutorial_1/config.sh @@ -0,0 +1,16 @@ +export INSTALL_HPP_DIR=$DEVEL_HPP_DIR/install +export ROBOTPKG=/opt/openrobots + +export PATH=$INSTALL_HPP_DIR/sbin:$INSTALL_HPP_DIR/bin:$ROBOTPKG/bin:$ROBOTPKG/sbin:$PATH +export PKG_CONFIG_PATH=$INSTALL_HPP_DIR/lib/pkgconfig/:$ROBOTPKG/lib/pkgconfig + +export PYTHONPATH=$INSTALL_HPP_DIR/lib/python3.12/site-packages:$ROBOTPKG/lib/python3.12/site-packages:$PYTHONPATH + +export LD_LIBRARY_PATH=$INSTALL_HPP_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..7e76622 --- /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 midor 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..7b079a5 --- /dev/null +++ b/tutorial_2/init.py @@ -0,0 +1,16 @@ +from pinocchio import SE3, neutral +from pyhpp.manipulation import Device, urdf + +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..761d902 --- /dev/null +++ b/tutorial_3/README.md @@ -0,0 +1,233 @@ +# 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. +``` +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/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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +