From faa978b08c747502ea39a896dfd640eb6db04fec Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 2 Jul 2025 08:52:42 +0200 Subject: [PATCH 1/2] Add tutorial_1 variant using hpp-python interface --- CMakeLists.txt | 1 + script/hpp_python_tutorial_1.py | 71 + urdf/kitchen_area_obstacle.urdf | 4729 +++++++++++++++++++++++++++++++ 3 files changed, 4801 insertions(+) create mode 100644 script/hpp_python_tutorial_1.py create mode 100644 urdf/kitchen_area_obstacle.urdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 7f5f786..874c05d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ install( 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 diff --git a/script/hpp_python_tutorial_1.py b/script/hpp_python_tutorial_1.py new file mode 100644 index 0000000..d4fbd41 --- /dev/null +++ b/script/hpp_python_tutorial_1.py @@ -0,0 +1,71 @@ +from pyhpp.gepetto import Viewer +import numpy as np +from pinocchio import SE3 +from pyhpp.pinocchio import Device +from pyhpp.core import Problem, DiffusingPlanner + +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.create("ur2") +viewer = Viewer("tutorial_1", robot) + +viewer.addURDFToScene(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] # [x_lower, x_upper, y_lower, y_upper] +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 + +viewer.addURDFObstacleToScene("package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf", "kitchen") + +viewer.applyConfiguration(q_goal) + +# Setup problem and RRT components +problem = Problem(robot) +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) + +diffusingPlanner = DiffusingPlanner(problem) + +path = diffusingPlanner.solve() +viewer.displayPath(path) diff --git a/urdf/kitchen_area_obstacle.urdf b/urdf/kitchen_area_obstacle.urdf new file mode 100644 index 0000000..2a555d5 --- /dev/null +++ b/urdf/kitchen_area_obstacle.urdf @@ -0,0 +1,4729 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + fixed + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + 0.8 + 0.0000001 + 1000000.0 + 1.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000 + 1000000 + 50000 + 50000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9a6f67b5678b7f81ca490fca5185ebd7c033e5be Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 2 Jul 2025 07:03:14 +0000 Subject: [PATCH 2/2] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- CMakeLists.txt | 2 +- script/hpp_python_tutorial_1.py | 20 ++++++++++++-------- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 874c05d..d71799a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,7 @@ install( urdf/cup.urdf urdf/door.urdf urdf/kitchen_area.urdf - urdf/kitchen_area_obstacle.urdf + urdf/kitchen_area_obstacle.urdf urdf/box_color.urdf urdf/rod.urdf urdf/table.urdf diff --git a/script/hpp_python_tutorial_1.py b/script/hpp_python_tutorial_1.py index d4fbd41..74f8fb4 100644 --- a/script/hpp_python_tutorial_1.py +++ b/script/hpp_python_tutorial_1.py @@ -1,8 +1,8 @@ -from pyhpp.gepetto import Viewer import numpy as np from pinocchio import SE3 +from pyhpp.core import DiffusingPlanner, Problem +from pyhpp.gepetto import Viewer from pyhpp.pinocchio import Device -from pyhpp.core import Problem, DiffusingPlanner urdfFilename = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" srdfFilename = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" @@ -11,7 +11,9 @@ robot = Device.create("ur2") viewer = Viewer("tutorial_1", robot) -viewer.addURDFToScene(0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity()) +viewer.addURDFToScene( + 0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity() +) # Define initial and goal configurations q_init = np.array(robot.currentConfiguration()) @@ -28,9 +30,9 @@ 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 +lowerLimit[iq] = -4 +upperLimit[iq] = -3 +lowerLimit[iq + 1] = -5 upperLimit[iq + 1] = -3 rankInConfiguration = dict() @@ -39,7 +41,7 @@ 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] @@ -56,7 +58,9 @@ rank = rankInConfiguration["r_elbow_flex_joint"] q_goal[rank] = -0.5 -viewer.addURDFObstacleToScene("package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf", "kitchen") +viewer.addURDFObstacleToScene( + "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf", "kitchen" +) viewer.applyConfiguration(q_goal)