From c87d378b69de57dcface633de16b68e88cd50fd9 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 24 Nov 2025 10:25:03 +0100 Subject: [PATCH 1/5] Update pyhpp benchmarks for testing --- .../baxter-manipulation-boxes-spf.py | 251 +++++++++++ tests/benchmarks/baxter-manipulation-boxes.py | 248 +++++++++++ tests/benchmarks/construction-set-m-rrt.py | 417 ++++++++++++++++++ tests/benchmarks/pr2-in-iai-kitchen.py | 88 ++++ .../pr2-manipulation-kitchen-spf.py | 162 +++++++ tests/benchmarks/pr2-manipulation-kitchen.py | 158 +++++++ .../pr2-manipulation-two-hand-spf.py | 184 ++++++++ tests/benchmarks/pr2-manipulation-two-hand.py | 181 ++++++++ tests/benchmarks/pyrene-on-the-ground.py | 2 +- tests/benchmarks/romeo-placard-spf.py | 226 ++++++++++ tests/benchmarks/romeo-placard.py | 221 ++++++++++ tests/benchmarks/ur3-spheres-spf.py | 4 +- tests/benchmarks/ur3-spheres.py | 3 +- 13 files changed, 2139 insertions(+), 6 deletions(-) create mode 100644 tests/benchmarks/baxter-manipulation-boxes-spf.py create mode 100644 tests/benchmarks/baxter-manipulation-boxes.py create mode 100644 tests/benchmarks/construction-set-m-rrt.py create mode 100644 tests/benchmarks/pr2-in-iai-kitchen.py create mode 100644 tests/benchmarks/pr2-manipulation-kitchen-spf.py create mode 100644 tests/benchmarks/pr2-manipulation-kitchen.py create mode 100644 tests/benchmarks/pr2-manipulation-two-hand-spf.py create mode 100644 tests/benchmarks/pr2-manipulation-two-hand.py create mode 100644 tests/benchmarks/romeo-placard-spf.py create mode 100644 tests/benchmarks/romeo-placard.py diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py new file mode 100644 index 0000000..c4c648f --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder + +from pyhpp.manipulation import ( + RandomShortcut as ManipRandomShortcut, + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +from pyhpp.constraints import ( + LockedJoint, +) +from pinocchio import SE3, Quaternion + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +baxter_urdf = "package://example-robot-data/robots/baxter_description/urdf/baxter.urdf" +baxter_srdf = "package://example-robot-data/robots/baxter_description/srdf/baxter_manipulation.srdf" +table_urdf = "package://hpp_tutorial/urdf/table.urdf" +table_srdf = "package://hpp_tutorial/srdf/table.srdf" +box_urdf = "package://hpp_environments/urdf/baxter_benchmark/box.urdf" +box_srdf = "package://hpp_environments/srdf/baxter_benchmark/box.srdf" + +# nbBoxes +K = 2 +nBoxPerLine = 2 +grippers = ["baxter/r_gripper", "baxter/l_gripper"] +# Box i will be at box goal[i] place at the end +goal = [1, 0] + +robot = Device("baxter-manip") + +# Load Baxter robot +baxter_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0.926])) +urdf.loadModel(robot, 0, "baxter", "anchor", baxter_urdf, baxter_srdf, baxter_pose) + +# Load table +table_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "table", "anchor", table_urdf, table_srdf, table_pose) + +# Load boxes +boxes = list() +for i in range(K): + boxes.append("box" + str(i)) + box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + boxes[i], + "freeflyer", + box_urdf, + box_srdf, + box_pose, + ) + robot.setJointBounds( + boxes[i] + '/root_joint', + [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + ) + +model = robot.model() + +problem = Problem(robot) +cg = Graph("graph", robot, problem) + +# Set error threshold and max iterations +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +q_init = robot.currentConfiguration() + +# Calculate box positions +rankB = list() +for i in range(K): + joint_id = robot.model().getJointId(boxes[i] + '/root_joint') + rankB.append (robot.model().idx_qs[joint_id]) + +bb = [0.7, 0.8, 0., 0.1] +c = sqrt(2) / 2 +xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) +nbCols = int(K * 1. / nBoxPerLine + 0.5) +ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) +for i in range(K): + iL = i % nBoxPerLine + iC = (i - iL) / nBoxPerLine + x = bb[0] + xstep * iL + y = bb[2] + xstep * iC + q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + +q_goal = q_init[::].copy() +for i in range(K): + r = rankB[i] + rn = rankB[goal[i]] + q_goal[r:r+7] = q_init[rn:rn+7] + +constraints = dict() +graphConstraints = dict() + +jointNames = dict() +jointNames['all'] = robot.model().names +jointNames['baxterRightSide'] = list() +jointNames['baxterLeftSide'] = list() + +for n in jointNames['all']: + if n.startswith("baxter"): + if n.startswith("baxter/left_"): + jointNames['baxterLeftSide'].append(n) + if n.startswith("baxter/right_"): + jointNames['baxterRightSide'].append(n) +# Lock finger joints +lockFingers = ["r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", + ] +for side in ["r", "l"]: + joint_name = "baxter/" + side + "_gripper_r_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([-0.02])) + constraints[side + "_gripper_r_finger"] = cs + graphConstraints[side + "_gripper_r_finger"] = cs + joint_name = "baxter/" + side + "_gripper_l_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([0.02])) + constraints[side + "_gripper_l_finger"] = cs + graphConstraints[side + "_gripper_l_finger"] = cs + + +# Lock head +lockHead = 'head_pan' +joint_name = 'baxter/head_pan' +joint_id = robot.model().getJointId(joint_name) +cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) +constraints[lockHead] = cs +graphConstraints[lockHead] = cs +for n in jointNames["baxterRightSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +# Define handles and contact surfaces +handlesPerObject = list() +handles = list() +objContactSurfaces = list() +for i in range(K): + handlesPerObject.append([boxes[i] + "/handle2"]) + handles.append(boxes[i] + "/handle2") + objContactSurfaces.append([boxes[i] + "/box_surface"]) + +rules = [Rule([".*"], [".*"], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(['table/pancake_table_table_top']) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() +cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise Exception('Init configuration could not be projected.') + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise Exception('Goal configuration could not be projected.') + +print(q_init) +problem.initConfig(q_init_proj) +print(q_init_proj) +print(cg.displayStateConstraints(cg.getState('free'))) + +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +optimizers = { + 'GraphPartialShortcut': GraphPartialShortcut(problem), + 'GraphRandomShortcut': GraphRandomShortcut(problem), + 'PartialShortcut': PartialShortcut(problem), + 'RandomShortcut': RandomShortcut(problem), + 'SimpleShortcut': SimpleShortcut(problem), +} +optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', + 'RandomShortcut', 'SimpleShortcut'] +iOpt = 0 + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + currentOptimizer = optimizers[optimizerNames[iOpt]] + iOpt += 1 + if iOpt == len(optimizerNames): + iOpt = 0 + + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + path = planner.solve() + optimized_path = currentOptimizer.optimize(path) + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/baxter-manipulation-boxes.py b/tests/benchmarks/baxter-manipulation-boxes.py new file mode 100644 index 0000000..c146ce4 --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.core import Dichotomy +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner + +from pyhpp.manipulation import ( + RandomShortcut as ManipRandomShortcut, + EnforceTransitionSemantic, + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +from pyhpp.constraints import ( + LockedJoint, +) +from pinocchio import SE3, Quaternion + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +baxter_urdf = "package://example-robot-data/robots/baxter_description/urdf/baxter.urdf" +baxter_srdf = "package://example-robot-data/robots/baxter_description/srdf/baxter_manipulation.srdf" +table_urdf = "package://hpp_tutorial/urdf/table.urdf" +table_srdf = "package://hpp_tutorial/srdf/table.srdf" +box_urdf = "package://hpp_environments/urdf/baxter_benchmark/box.urdf" +box_srdf = "package://hpp_environments/srdf/baxter_benchmark/box.srdf" + +# nbBoxes +K = 2 +nBoxPerLine = 2 +grippers = ["baxter/r_gripper", "baxter/l_gripper"] +# Box i will be at box goal[i] place at the end +goal = [1, 0] + +robot = Device("baxter-manip") + +# Load Baxter robot +baxter_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0.926])) +urdf.loadModel(robot, 0, "baxter", "anchor", baxter_urdf, baxter_srdf, baxter_pose) + +# Load table +table_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "table", "anchor", table_urdf, table_srdf, table_pose) + +# Load boxes +boxes = list() +for i in range(K): + boxes.append("box" + str(i)) + box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + boxes[i], + "freeflyer", + box_urdf, + box_srdf, + box_pose, + ) + robot.setJointBounds( + boxes[i] + '/root_joint', + [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + ) + +tmp = boxes[0] +boxes[0] = boxes[1] +boxes[1] = tmp + +model = robot.model() + +problem = Problem(robot) +cg = Graph("graph", robot, problem) + +# Set error threshold and max iterations +cg.errorThreshold(1e-3) +cg.maxIterations(40) +problem.pathValidation = Dichotomy(robot, 0.0) + +q_init = robot.currentConfiguration() + +# Calculate box positions +rankB = list() +for i in range(K): + joint_id = robot.model().getJointId(boxes[i] + '/root_joint') + rankB.append (robot.model().idx_qs[joint_id]) + +bb = [0.7, 0.8, 0., 0.1] +c = sqrt(2) / 2 +xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) +nbCols = int(K * 1. / nBoxPerLine + 0.5) +ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) +for i in range(K): + iL = i % nBoxPerLine + iC = (i - iL) / nBoxPerLine + x = bb[0] + xstep * iL + y = bb[2] + xstep * iC + q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + +q_goal = q_init[::].copy() +for i in range(K): + r = rankB[i] + rn = rankB[goal[i]] + q_goal[r:r+7] = q_init[rn:rn+7] + +constraints = dict() +graphConstraints = dict() + +jointNames = dict() +jointNames['all'] = robot.model().names +jointNames['baxterRightSide'] = list() +jointNames['baxterLeftSide'] = list() + +for n in jointNames['all']: + if n.startswith("baxter"): + if n.startswith("baxter/left_"): + jointNames['baxterLeftSide'].append(n) + if n.startswith("baxter/right_"): + jointNames['baxterRightSide'].append(n) +# Lock finger joints +lockFingers = ["r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", + ] +for side in ["r", "l"]: + joint_name = "baxter/" + side + "_gripper_r_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([-0.02])) + constraints[side + "_gripper_r_finger"] = cs + graphConstraints[side + "_gripper_r_finger"] = cs + joint_name = "baxter/" + side + "_gripper_l_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([0.02])) + constraints[side + "_gripper_l_finger"] = cs + graphConstraints[side + "_gripper_l_finger"] = cs + + +# Lock head +lockHead = 'head_pan' +joint_name = 'baxter/head_pan' +joint_id = robot.model().getJointId(joint_name) +cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) +constraints[lockHead] = cs +graphConstraints[lockHead] = cs +for n in jointNames["baxterRightSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +# Define handles and contact surfaces +handlesPerObject = list() +handles = list() +objContactSurfaces = list() +for i in range(K): + handlesPerObject.append([boxes[i] + "/handle2"]) + handles.append(boxes[i] + "/handle2") + objContactSurfaces.append([boxes[i] + "/box_surface"]) + +rules = [Rule([".*"], [".*"], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(['table/pancake_table_table_top']) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() +cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise Exception('Init configuration could not be projected.') + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise Exception('Goal configuration could not be projected.') + +problem.initConfig(q_init_proj) + +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = ManipulationPlanner(problem) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") +optimizers = { + 'GraphPartialShortcut': GraphPartialShortcut(problem), + 'GraphRandomShortcut': GraphRandomShortcut(problem), + 'PartialShortcut': PartialShortcut(problem), + 'RandomShortcut': RandomShortcut(problem), + 'SimpleShortcut': SimpleShortcut(problem), +} +optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', + 'RandomShortcut', 'SimpleShortcut'] +iOpt = 0 + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + currentOptimizer = optimizers[optimizerNames[iOpt]] + iOpt += 1 + if iOpt == len(optimizerNames): + iOpt = 0 + + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + path = planner.solve() + optimized_path = currentOptimizer.optimize(path) + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/construction-set-m-rrt.py b/tests/benchmarks/construction-set-m-rrt.py new file mode 100644 index 0000000..bf23ba8 --- /dev/null +++ b/tests/benchmarks/construction-set-m-rrt.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import pi, sqrt +import numpy as np +import datetime as dt +import re +import typing as T + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Straight, Progressive, ProgressiveProjector +from pyhpp.constraints import ( + Transformation, + ComparisonTypes, + ComparisonType, + Implicit, + LockedJoint, +) +from pinocchio import SE3, Quaternion + +from pyhpp.manipulation.security_margins import SecurityMargins + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +parser.add_argument('--bigGraph', action='store_true', + help="Whether constraint graph is generated with all the possible states. " + "If unspecified, constraint graph only has a few states traversed by " + "one safe path.") +args = parser.parse_args() + + +class StateName(object): + noGrasp = 'free' + + def __init__(self, grasps): + if isinstance(grasps, set): + self.grasps = grasps.copy() + elif isinstance(grasps, str): + if grasps == self.noGrasp: + self.grasps = set() + else: + g1 = [s.strip(' ') for s in grasps.split(':')] + self.grasps = set([tuple(s.split(' grasps ')) for s in g1]) + else: + raise TypeError('expecting a set of pairs (gripper, handle) or a string') + + def __str__(self): + if self.grasps == set(): + return 'free' + res = "" + for g in self.grasps: + res += g[0] + " grasps " + g[1] + " : " + return res[:-3] + + def __eq__(self, other): + return self.grasps == other.grasps + + def __ne__(self, other): + return not self.__eq__(other) + + +ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" +ur3_srdf = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" +cylinder_08_urdf = "package://hpp_environments/urdf/construction_set/cylinder_08.urdf" +cylinder_08_srdf = "package://hpp_environments/srdf/construction_set/cylinder_08.srdf" +sphere_urdf = "package://hpp_environments/urdf/construction_set/sphere.urdf" +sphere_srdf = "package://hpp_environments/srdf/construction_set/sphere.srdf" +ground_urdf = "package://hpp_environments/urdf/construction_set/ground.urdf" +ground_srdf = "package://hpp_environments/srdf/construction_set/ground.srdf" + +nSphere = 2 +nCylinder = 2 + +robot = Device('2ur5-sphere') + +r0_pose = SE3(rotation=np.identity(3), translation=np.array([-.25, 0, 0])) +urdf.loadModel(robot, 0, "r0", "anchor", ur3_urdf, ur3_srdf, r0_pose) + +r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([.25, 0, 0])) +urdf.loadModel(robot, 0, "r1", "anchor", ur3_urdf, ur3_srdf, r1_pose) + +robot.setJointBounds('r0/shoulder_pan_joint', [-pi, 4]) +robot.setJointBounds('r1/shoulder_pan_joint', [-pi, 4]) +robot.setJointBounds('r0/shoulder_lift_joint', [-pi, 0]) +robot.setJointBounds('r1/shoulder_lift_joint', [-pi, 0]) +robot.setJointBounds('r0/elbow_joint', [-2.6, 2.6]) +robot.setJointBounds('r1/elbow_joint', [-2.6, 2.6]) + +ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "ground", "anchor", ground_urdf, ground_srdf, ground_pose) + +objects = list() +for i in range(nSphere): + sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, f'sphere{i}', "freeflyer", sphere_urdf, sphere_srdf, sphere_pose) + robot.setJointBounds( + f'sphere{i}/root_joint', + [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, + -1.0001, 1.0001, -1.0001, 1.0001] + ) + objects.append(f'sphere{i}') + +for i in range(nCylinder): + cylinder_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, f'cylinder{i}', "freeflyer", cylinder_08_urdf, cylinder_08_srdf, cylinder_pose) + robot.setJointBounds( + f'cylinder{i}/root_joint', + [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, + -1.0001, 1.0001, -1.0001, 1.0001] + ) + objects.append(f'cylinder{i}') + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("assembly", robot, problem) +cg.errorThreshold(1e-4) +cg.maxIterations(40) + +constraints = dict() + +for i in range(nSphere): + placementName = f"place_sphere{i}" + Id = SE3.Identity() + spherePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"sphere{i}/root_joint") + + pc = Transformation( + placementName, robot, joint, Id, spherePlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", robot, joint, Id, spherePlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) + constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, + ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"sphere{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_sphere{i}" + spherePrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, robot, joint, Id, spherePrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +for i in range(nCylinder): + placementName = f"place_cylinder{i}" + Id = SE3.Identity() + cylinderPlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"cylinder{i}/root_joint") + + pc = Transformation( + placementName, robot, joint, Id, cylinderPlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", robot, joint, Id, cylinderPlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) + constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, + ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"cylinder{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_cylinder{i}" + cylinderPrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, robot, joint, Id, cylinderPrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +grippers = [f'cylinder{i}/magnet0' for i in range(nCylinder)] +grippers += [f'cylinder{i}/magnet1' for i in range(nCylinder)] +grippers += [f"r{i}/gripper" for i in range(2)] + +handlesPerObjects = [[f'sphere{i}/handle', f'sphere{i}/magnet'] for i in range(nSphere)] +handlesPerObjects += [[f'cylinder{i}/handle'] for i in range(nCylinder)] + +shapesPerObject = [[] for o in objects] + +allHandles = [handle for objHandles in handlesPerObjects for handle in objHandles] + + +def makeRule(grasps): + _grippers = list() + _handles = list() + for (g, h) in grasps: + _grippers.append(g) + _handles.append(h) + for g in grippers: + if not g in _grippers: + _grippers.append(g) + _handles += (len(_grippers) - len(_handles)) * ['^$'] + return Rule(grippers=_grippers, handles=_handles, link=True) + + +def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: + gRegex = re.compile(g) + hRegex = [re.compile(handle) for handle in h] + forbiddenList: T.List[Rule] = list() + idForbidGrippers = [i for i in range(len(grippers)) if gRegex.match(grippers[i])] + forbidHandles = [handle for handle in allHandles + if not any([handlePattern.match(handle) for handlePattern in hRegex])] + for id in idForbidGrippers: + for handle in forbidHandles: + _handles = [handle if i == id else '.*' for i in range(len(grippers))] + forbiddenList.append(Rule(grippers=grippers, handles=_handles, link=False)) + return forbiddenList + + +q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0] +q0_r1 = q0_r0[::] + +q0_spheres = list() +i = 0 +y = 0.04 +while i < nSphere: + q0_spheres.append([-.1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0_cylinders = list() +i = 0 +y = -0.04 +while i < nCylinder: + q0_cylinders.append([0.45 + .1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0 = np.array(q0_r0 + q0_r1 + sum(q0_spheres, []) + sum(q0_cylinders, [])) +if not args.bigGraph: + nodes = list() + rules = list() + grasps = set() + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r0/gripper', 'sphere0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r1/gripper', 'cylinder0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('cylinder0/magnet0', 'sphere0/magnet')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r0/gripper', 'sphere0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r0/gripper', 'sphere1/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('cylinder0/magnet1', 'sphere1/magnet')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r0/gripper', 'sphere1/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r1/gripper', 'cylinder0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", + "cylinder0", "cylinder1"], robot) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + nodes.append(nodes[-1]) + +else: + rules = list() + + rules.extend(forbidExcept('^r0/gripper$', ['^sphere\\d*/handle$'])) + rules.extend(forbidExcept('^r1/gripper$', ['^cylinder0*/handle$'])) + rules.extend(forbidExcept('^cylinder0/magnet\\d*$', ['^sphere\\d*/magnet$'])) + rules.extend(forbidExcept('^cylinder[1-9][0-9]*.*$', ['^$'])) + + rules.append(Rule(grippers=grippers, handles=[".*"] * len(grippers), link=True)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", + "cylinder0", "cylinder1"], robot) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + +assert (nCylinder == 2 and nSphere == 2) +c = sqrt(2) / 2 +q_goal = np.array(q0_r0 + q0_r1 + [-0.06202136144745322, -0.15, 0.025, c, 0, -c, 0, + 0.06202136144745322, -0.15, 0.025, c, 0, c, 0, + 0, -0.15, 0.025, 0, 0, 0, 1, + 0.5, -0.08, 0.025, 0, 0, 0, 1]) + +problem.initConfig(q0) +problem.addGoalConfig(q_goal) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q0) + problem.addGoalConfig(q_goal) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py new file mode 100644 index 0000000..29ffa92 --- /dev/null +++ b/tests/benchmarks/pr2-in-iai-kitchen.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import numpy as np +import datetime as dt + +from pyhpp.core import Problem, DiffusingPlanner, Dichotomy, Straight +from pyhpp.pinocchio import Device, urdf +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" + +robot = Device('pr2') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +robot.setJointBounds("pr2/root_joint", [-4, -3, -5, -3, -2, 2, -2, 2]) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen", "anchor", kitchen_urdf, "", kitchen_pose) + +model = robot.model() + +q_init = robot.currentConfiguration() +q_goal = q_init.copy() + +q_init[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId('pr2/torso_lift_joint')] +q_init[rank] = 0.2 + +q_goal[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId('pr2/l_shoulder_lift_joint')] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId('pr2/r_shoulder_lift_joint')] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId('pr2/l_elbow_flex_joint')] +q_goal[rank] = -0.5 +rank = model.idx_qs[model.getJointId('pr2/r_elbow_flex_joint')] +q_goal[rank] = -0.5 + +problem = Problem(robot) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.steeringMethod = Straight(problem) + +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) +planner = DiffusingPlanner(problem) +planner.maxIterations(5000) +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py new file mode 100644 index 0000000..51fab7d --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt, pi +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +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_goal = q_init.copy() +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+7] = [-2.5, -4, 0.9, 0, c, 0, c] + +q_goal[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +grippers = ['pr2/l_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle']] +envSurfaces = ['kitchen_area/pancake_table_table_top', + 'kitchen_area/white_counter_top_sink'] +objContactSurfaces = [['box/box_surface']] + +rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), + Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) + +cg.setWeight(cg.getTransition('Loop | f'), 1) +cg.setWeight(cg.getTransition('Loop | 0-0'), 1) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(problem.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py new file mode 100644 index 0000000..d9be872 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt, pi +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +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_goal = q_init.copy() +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+7] = [-2.5, -4, 0.9, 0, c, 0, c] + +q_goal[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +grippers = ['pr2/l_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle']] +envSurfaces = ['kitchen_area/pancake_table_table_top', + 'kitchen_area/white_counter_top_sink'] +objContactSurfaces = [['box/box_surface']] + +rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), + Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) + +cg.setWeight(cg.getTransition('Loop | f'), 1) +cg.setWeight(cg.getTransition('Loop | 0-0'), 1) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py new file mode 100644 index 0000000..d3713b5 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +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]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +q_init[0:4] = [-3.2, -4, 1, 0] + +rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +q_init[rank] = 0.5 +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 +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 + +q_goal = q_init.copy() + +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +lockrhand = ['r_l_finger', 'r_r_finger'] +cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) +constraints['r_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) +constraints['r_r_finger'] = cs + +lockhands = lockrhand + locklhand + +lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] +cs = LockedJoint(robot, 'pr2/head_pan_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) +constraints['head_pan'] = cs +cs = LockedJoint(robot, 'pr2/head_tilt_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) +constraints['head_tilt'] = cs +cs = LockedJoint(robot, 'pr2/torso_lift_joint', + np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) +constraints['torso'] = cs +cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', + np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) +constraints['laser'] = cs + +lockPlanar = ['lockplanar'] +cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) +constraints['lockplanar'] = cs + +lockAll = lockhands + lockHeadAndTorso + lockPlanar + +grippers = ['pr2/l_gripper', 'pr2/r_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle', 'box/handle2']] +objContactSurfaces = [['box/box_surface']] +envSurfaces = ['kitchen_area/pancake_table_table_top'] + +rules = [Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in lockAll]) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.2 +) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py new file mode 100644 index 0000000..dcf14cc --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +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]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +q_init[0:4] = [-3.2, -4, 1, 0] + +rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +q_init[rank] = 0.5 +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 +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 + +q_goal = q_init.copy() + +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +lockrhand = ['r_l_finger', 'r_r_finger'] +cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) +constraints['r_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) +constraints['r_r_finger'] = cs + +lockhands = lockrhand + locklhand + +lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] +cs = LockedJoint(robot, 'pr2/head_pan_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) +constraints['head_pan'] = cs +cs = LockedJoint(robot, 'pr2/head_tilt_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) +constraints['head_tilt'] = cs +cs = LockedJoint(robot, 'pr2/torso_lift_joint', + np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) +constraints['torso'] = cs +cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', + np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) +constraints['laser'] = cs + +lockPlanar = ['lockplanar'] +rank = robot.rankInConfiguration['pr2/root_joint'] +cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) +constraints['lockplanar'] = cs + +lockAll = lockhands + lockHeadAndTorso + lockPlanar + +grippers = ['pr2/l_gripper', 'pr2/r_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle', 'box/handle2']] +objContactSurfaces = [['box/box_surface']] +envSurfaces = ['kitchen_area/pancake_table_table_top'] + +rules = [Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in lockAll]) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.2 +) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 50e56e6..b02d6de 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -19,7 +19,7 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) parser.add_argument("--display", action="store_true") parser.add_argument("--run", action="store_true") args = parser.parse_args() diff --git a/tests/benchmarks/romeo-placard-spf.py b/tests/benchmarks/romeo-placard-spf.py new file mode 100644 index 0000000..72a7262 --- /dev/null +++ b/tests/benchmarks/romeo-placard-spf.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import time +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" +romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +placard_urdf = "package://hpp_environments/urdf/placard.urdf" +placard_srdf = "package://hpp_environments/srdf/placard.srdf" + +robot = Device('romeo-placard') + +# Load Romeo robot +romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) + +robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) + +# Load placard +placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) + +robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) + +model = robot.model() + +problem = Problem(robot) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("graph", robot, problem) +cg.errorThreshold(2e-4) +cg.maxIterations(40) + +constraints = dict() +graphConstraints = dict() + +leftHandOpen = { + "LHand": 1, + "LFinger12": 1.06, + "LFinger13": 1.06, + "LFinger21": 1.06, + "LFinger22": 1.06, + "LFinger23": 1.06, + "LFinger31": 1.06, + "LFinger32": 1.06, + "LFinger33": 1.06, + "LThumb1": 0, + "LThumb2": 1.06, + "LThumb3": 1.06, +} + +rightHandOpen = { + "RHand": 1, + "RFinger12": 1.06, + "RFinger13": 1.06, + "RFinger21": 1.06, + "RFinger22": 1.06, + "RFinger23": 1.06, + "RFinger31": 1.06, + "RFinger32": 1.06, + "RFinger33": 1.06, + "RThumb1": 0, + "RThumb2": 1.06, + "RThumb3": 1.06, +} + + +# Lock left hand +locklhand = list() +for j, v in leftHandOpen.items(): + joint_name = 'romeo/' + j + locklhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +# Lock right hand +lockrhand = list() +for j, v in rightHandOpen.items(): + joint_name = 'romeo/' + j + lockrhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +lockHands = lockrhand + locklhand + +# Create static stability constraint +q = robot.currentConfiguration() +placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] +q[placard_rank:placard_rank+3] = [.4, 0, 1.2] + +problem.addPartialCom("romeo", ["romeo/root_joint"]) + +leftAnkle = 'romeo/LAnkleRoll' +rightAnkle = 'romeo/RAnkleRoll' + +factory = StaticStabilityConstraintsFactory(problem, robot) +balanceConstraintsDict = factory.createStaticStabilityConstraint( + "balance/", "romeo", leftAnkle, rightAnkle, q +) + +balanceConstraints = [ + balanceConstraintsDict.get('balance/pose-left-foot'), + balanceConstraintsDict.get('balance/pose-right-foot'), + balanceConstraintsDict.get('balance/relative-com'), +] +balanceConstraints = [c for c in balanceConstraints if c is not None] + +for name, constraint in balanceConstraintsDict.items(): + if constraint not in balanceConstraints: + constraints[name] = constraint + graphConstraints[name] = constraint + +# Build graph +grippers = ['romeo/r_hand', 'romeo/l_hand'] +handlesPerObjects = [['placard/low', 'placard/high']] + +rules = [ + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["", "placard/high"], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", "placard/high"], True), +] + +factory_cg = ConstraintGraphFactory(cg, constraints) +factory_cg.setGrippers(grippers) +factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setRules(rules) +factory_cg.generate() + +# Add balance constraints and locked hands to graph +all_graph_constraints = list(graphConstraints.values()) + balanceConstraints +cg.addNumericalConstraintsToGraph(all_graph_constraints) +cg.initialize() + +# Define initial and final configurations +q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_init = q_goal.copy() +q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] + +n = 'romeo/l_hand grasps placard/low' +state = cg.getState(n) +res, q_init_proj, err = cg.applyStateConstraints(state, q_init) +if not res: + raise RuntimeError("Failed to project initial configuration.") +res, q_goal_proj, err = cg.applyStateConstraints(state, q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration.") + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 +) + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) +# Run benchmark +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") + + diff --git a/tests/benchmarks/romeo-placard.py b/tests/benchmarks/romeo-placard.py new file mode 100644 index 0000000..8d7aed4 --- /dev/null +++ b/tests/benchmarks/romeo-placard.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import time +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" +romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +placard_urdf = "package://hpp_environments/urdf/placard.urdf" +placard_srdf = "package://hpp_environments/srdf/placard.srdf" + +robot = Device('romeo-placard') + +# Load Romeo robot +romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) + +robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) + +# Load placard +placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) + +robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) + +model = robot.model() + +problem = Problem(robot) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("graph", robot, problem) +cg.errorThreshold(2e-4) +cg.maxIterations(40) + +constraints = dict() +graphConstraints = dict() + +leftHandOpen = { + "LHand": 1, + "LFinger12": 1.06, + "LFinger13": 1.06, + "LFinger21": 1.06, + "LFinger22": 1.06, + "LFinger23": 1.06, + "LFinger31": 1.06, + "LFinger32": 1.06, + "LFinger33": 1.06, + "LThumb1": 0, + "LThumb2": 1.06, + "LThumb3": 1.06, +} + +rightHandOpen = { + "RHand": 1, + "RFinger12": 1.06, + "RFinger13": 1.06, + "RFinger21": 1.06, + "RFinger22": 1.06, + "RFinger23": 1.06, + "RFinger31": 1.06, + "RFinger32": 1.06, + "RFinger33": 1.06, + "RThumb1": 0, + "RThumb2": 1.06, + "RThumb3": 1.06, +} + + +# Lock left hand +locklhand = list() +for j, v in leftHandOpen.items(): + joint_name = 'romeo/' + j + locklhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +# Lock right hand +lockrhand = list() +for j, v in rightHandOpen.items(): + joint_name = 'romeo/' + j + lockrhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +lockHands = lockrhand + locklhand + +# Create static stability constraint +q = robot.currentConfiguration() +placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] +q[placard_rank:placard_rank+3] = [.4, 0, 1.2] + +problem.addPartialCom("romeo", ["romeo/root_joint"]) + +leftAnkle = 'romeo/LAnkleRoll' +rightAnkle = 'romeo/RAnkleRoll' + +factory = StaticStabilityConstraintsFactory(problem, robot) +balanceConstraintsDict = factory.createStaticStabilityConstraint( + "balance/", "romeo", leftAnkle, rightAnkle, q +) + +balanceConstraints = [ + balanceConstraintsDict.get('balance/pose-left-foot'), + balanceConstraintsDict.get('balance/pose-right-foot'), + balanceConstraintsDict.get('balance/relative-com'), +] +balanceConstraints = [c for c in balanceConstraints if c is not None] + +for name, constraint in balanceConstraintsDict.items(): + if constraint not in balanceConstraints: + constraints[name] = constraint + graphConstraints[name] = constraint + +# Build graph +grippers = ['romeo/r_hand', 'romeo/l_hand'] +handlesPerObjects = [['placard/low', 'placard/high']] + +rules = [ + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["", "placard/high"], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", "placard/high"], True), +] + +factory_cg = ConstraintGraphFactory(cg, constraints) +factory_cg.setGrippers(grippers) +factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setRules(rules) +factory_cg.generate() + +# Add balance constraints and locked hands to graph +all_graph_constraints = list(graphConstraints.values()) + balanceConstraints +cg.addNumericalConstraintsToGraph(all_graph_constraints) +cg.initialize() + +# Define initial and final configurations +q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_init = q_goal.copy() +q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] + +n = 'romeo/l_hand grasps placard/low' +state = cg.getState(n) +res, q_init_proj, err = cg.applyStateConstraints(state, q_init) +if not res: + raise RuntimeError("Failed to project initial configuration.") +res, q_goal_proj, err = cg.applyStateConstraints(state, q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration.") + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 +) + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +# Run benchmark +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") + + diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/benchmarks/ur3-spheres-spf.py index 0586f64..07ea8f8 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/benchmarks/ur3-spheres-spf.py @@ -31,7 +31,7 @@ from pinocchio import SE3, Quaternion parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) args = parser.parse_args() # Robot and environment file paths @@ -309,8 +309,6 @@ for i in range(args.N): try: planner.roadmap().clear() - # TODO: Verify if resetGoalConfigs equivalent is needed - # In old API: ps.resetGoalConfigs() t1 = dt.datetime.now() planner.solve() t2 = dt.datetime.now() diff --git a/tests/benchmarks/ur3-spheres.py b/tests/benchmarks/ur3-spheres.py index 3b2591f..debb270 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/benchmarks/ur3-spheres.py @@ -26,7 +26,7 @@ from argparse import ArgumentParser parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) args = parser.parse_args() # Robot and environment file paths ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" @@ -289,7 +289,6 @@ t2 = dt.datetime.now() except Exception as e: print(f"Failed to plan path: {e}") - break else: success += 1 totalTime += t2 - t1 From 1a2a1e3602379cb9fdb3b3a7af12a4861fee027e Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:13:17 +0100 Subject: [PATCH 2/5] Fix test files referencing bad urdf file --- tests/benchmarks/baxter-manipulation-boxes-spf.py | 3 --- tests/benchmarks/pr2-manipulation-kitchen-spf.py | 4 ++-- tests/benchmarks/pr2-manipulation-kitchen.py | 2 +- tests/benchmarks/pr2-manipulation-two-hand-spf.py | 2 +- tests/benchmarks/pr2-manipulation-two-hand.py | 2 +- tests/graph_factory.py | 2 +- 6 files changed, 6 insertions(+), 9 deletions(-) diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py index c4c648f..f635221 100644 --- a/tests/benchmarks/baxter-manipulation-boxes-spf.py +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -183,10 +183,7 @@ if not res: raise Exception('Goal configuration could not be projected.') -print(q_init) problem.initConfig(q_init_proj) -print(q_init_proj) -print(cg.displayStateConstraints(cg.getState('free'))) problem.addGoalConfig(q_goal_proj) problem.constraintGraph(cg) diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py index 51fab7d..f7a4531 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen-spf.py +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') @@ -148,7 +148,7 @@ success += 1 totalTime += t2 - t1 print(t2 - t1) - n = len(problem.roadmap().nodes()) + n = len(planner.roadmap().nodes()) totalNumberNodes += n print("Number nodes: " + str(n)) diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py index d9be872..50685a8 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen.py +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py index d3713b5..85c2a85 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand-spf.py +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py index dcf14cc..b4fad23 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand.py +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/graph_factory.py b/tests/graph_factory.py index 215addb..c580b35 100644 --- a/tests/graph_factory.py +++ b/tests/graph_factory.py @@ -13,7 +13,7 @@ ) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device("pr2-box") From 9a68fa0e4effe1b68d55235c6cb45dccf1391587 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:14:41 +0100 Subject: [PATCH 3/5] Add getGraphCapsule to allow external modules to extract cpp pointer --- src/pyhpp/manipulation/graph.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index a362afa..b7ffc82 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -1212,6 +1212,23 @@ ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, ImplicitPtr_t constraint = h->createPreGrasp(g, c, name); return constraint; } + +static void graphCapsuleDestructor(PyObject* capsule) { + auto* ptr = static_cast( + PyCapsule_GetPointer(capsule, "hpp.manipulation.graph.GraphPtr")); + if (ptr) { + delete ptr; + } +} + +/// Create a PyCapsule containing a copy of the GraphPtr_t shared_ptr +/// This allows external modules to extract the native C++ pointer +PyObject* getGraphCapsule(const PyWGraph& self) { + auto* ptr = new GraphPtr_t(self.obj); + return PyCapsule_New(ptr, "hpp.manipulation.graph.GraphPtr", + graphCapsuleDestructor); +} + // ============================================================================= // Boost.Python bindings // ============================================================================= @@ -1230,6 +1247,7 @@ void exposeGraph() { "Graph", init()) + .def("_get_native_graph", &getGraphCapsule) .def_readwrite("robot", &PyWGraph::robot) // Configuration methods .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, From 892c0bb92712a822b952cdb09d7ab5af7c169b03 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:27:53 +0100 Subject: [PATCH 4/5] Add file to test benchmarks examples easily --- tests/benchmarks/test_benchmarks.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 tests/benchmarks/test_benchmarks.py diff --git a/tests/benchmarks/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py new file mode 100644 index 0000000..582f773 --- /dev/null +++ b/tests/benchmarks/test_benchmarks.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 +import subprocess +import sys +from pathlib import Path + +def main(): + folder = Path(sys.argv[1]) if len(sys.argv) > 1 else Path(".") + + for script in sorted(folder.glob("*.py")): + if script.name == Path(__file__).name: + continue + print(f"\n{'='*60}\nRunning: {script.name}\n{'='*60}") + subprocess.run([sys.executable, str(script), "-N", "1"]) + +if __name__ == "__main__": + main() \ No newline at end of file From 22d8edeecefc13d096e36962d3ae0c36d97b0593 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 7 Jan 2026 08:33:17 +0000 Subject: [PATCH 5/5] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../baxter-manipulation-boxes-spf.py | 90 +++-- tests/benchmarks/baxter-manipulation-boxes.py | 89 +++-- tests/benchmarks/construction-set-m-rrt.py | 366 ++++++++++++------ tests/benchmarks/pr2-in-iai-kitchen.py | 20 +- .../pr2-manipulation-kitchen-spf.py | 89 +++-- tests/benchmarks/pr2-manipulation-kitchen.py | 89 +++-- .../pr2-manipulation-two-hand-spf.py | 118 +++--- tests/benchmarks/pr2-manipulation-two-hand.py | 120 +++--- tests/benchmarks/romeo-placard-spf.py | 135 +++++-- tests/benchmarks/romeo-placard.py | 135 +++++-- tests/benchmarks/test_benchmarks.py | 8 +- 11 files changed, 803 insertions(+), 456 deletions(-) diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py index f635221..b05f2ac 100644 --- a/tests/benchmarks/baxter-manipulation-boxes-spf.py +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -9,7 +9,6 @@ from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder from pyhpp.manipulation import ( - RandomShortcut as ManipRandomShortcut, GraphRandomShortcut, GraphPartialShortcut, ) @@ -22,12 +21,12 @@ from pyhpp.constraints import ( LockedJoint, ) -from pinocchio import SE3, Quaternion +from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths @@ -70,8 +69,7 @@ box_pose, ) robot.setJointBounds( - boxes[i] + '/root_joint', - [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] ) model = robot.model() @@ -88,47 +86,48 @@ # Calculate box positions rankB = list() for i in range(K): - joint_id = robot.model().getJointId(boxes[i] + '/root_joint') - rankB.append (robot.model().idx_qs[joint_id]) + joint_id = robot.model().getJointId(boxes[i] + "/root_joint") + rankB.append(robot.model().idx_qs[joint_id]) -bb = [0.7, 0.8, 0., 0.1] +bb = [0.7, 0.8, 0.0, 0.1] c = sqrt(2) / 2 xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) -nbCols = int(K * 1. / nBoxPerLine + 0.5) +nbCols = int(K * 1.0 / nBoxPerLine + 0.5) ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) for i in range(K): iL = i % nBoxPerLine iC = (i - iL) / nBoxPerLine x = bb[0] + xstep * iL y = bb[2] + xstep * iC - q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c] q_goal = q_init[::].copy() for i in range(K): r = rankB[i] rn = rankB[goal[i]] - q_goal[r:r+7] = q_init[rn:rn+7] + q_goal[r : r + 7] = q_init[rn : rn + 7] constraints = dict() graphConstraints = dict() jointNames = dict() -jointNames['all'] = robot.model().names -jointNames['baxterRightSide'] = list() -jointNames['baxterLeftSide'] = list() +jointNames["all"] = robot.model().names +jointNames["baxterRightSide"] = list() +jointNames["baxterLeftSide"] = list() -for n in jointNames['all']: +for n in jointNames["all"]: if n.startswith("baxter"): if n.startswith("baxter/left_"): - jointNames['baxterLeftSide'].append(n) + jointNames["baxterLeftSide"].append(n) if n.startswith("baxter/right_"): - jointNames['baxterRightSide'].append(n) + jointNames["baxterRightSide"].append(n) # Lock finger joints -lockFingers = ["r_gripper_l_finger", - "r_gripper_r_finger", - "l_gripper_l_finger", - "l_gripper_r_finger", - ] +lockFingers = [ + "r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", +] for side in ["r", "l"]: joint_name = "baxter/" + side + "_gripper_r_finger_joint" cs = LockedJoint(robot, joint_name, np.array([-0.02])) @@ -141,18 +140,18 @@ # Lock head -lockHead = 'head_pan' -joint_name = 'baxter/head_pan' +lockHead = "head_pan" +joint_name = "baxter/head_pan" joint_id = robot.model().getJointId(joint_name) cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) constraints[lockHead] = cs graphConstraints[lockHead] = cs for n in jointNames["baxterRightSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs for n in jointNames["baxterLeftSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs # Define handles and contact surfaces @@ -168,20 +167,20 @@ factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) -factory.environmentContacts(['table/pancake_table_table_top']) +factory.environmentContacts(["table/pancake_table_table_top"]) factory.setObjects(boxes, handlesPerObject, objContactSurfaces) factory.setRules(rules) factory.generate() -cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.addNumericalConstraintsToGraph(list(graphConstraints.values())) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: - raise Exception('Init configuration could not be projected.') + raise Exception("Init configuration could not be projected.") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: - raise Exception('Goal configuration could not be projected.') + raise Exception("Goal configuration could not be projected.") problem.initConfig(q_init_proj) @@ -200,14 +199,19 @@ problem.addConfigValidation("CollisionValidation") optimizers = { - 'GraphPartialShortcut': GraphPartialShortcut(problem), - 'GraphRandomShortcut': GraphRandomShortcut(problem), - 'PartialShortcut': PartialShortcut(problem), - 'RandomShortcut': RandomShortcut(problem), - 'SimpleShortcut': SimpleShortcut(problem), + "GraphPartialShortcut": GraphPartialShortcut(problem), + "GraphRandomShortcut": GraphRandomShortcut(problem), + "PartialShortcut": PartialShortcut(problem), + "RandomShortcut": RandomShortcut(problem), + "SimpleShortcut": SimpleShortcut(problem), } -optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', - 'RandomShortcut', 'SimpleShortcut'] +optimizerNames = [ + "GraphPartialShortcut", + "GraphRandomShortcut", + "PartialShortcut", + "RandomShortcut", + "SimpleShortcut", +] iOpt = 0 totalTime = dt.timedelta(0) @@ -219,7 +223,7 @@ iOpt += 1 if iOpt == len(optimizerNames): iOpt = 0 - + try: planner.roadmap().clear() problem.resetGoalConfigs() @@ -245,4 +249,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/baxter-manipulation-boxes.py b/tests/benchmarks/baxter-manipulation-boxes.py index c146ce4..b434bb7 100644 --- a/tests/benchmarks/baxter-manipulation-boxes.py +++ b/tests/benchmarks/baxter-manipulation-boxes.py @@ -10,8 +10,6 @@ from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner from pyhpp.manipulation import ( - RandomShortcut as ManipRandomShortcut, - EnforceTransitionSemantic, GraphRandomShortcut, GraphPartialShortcut, ) @@ -24,12 +22,12 @@ from pyhpp.constraints import ( LockedJoint, ) -from pinocchio import SE3, Quaternion +from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths @@ -72,8 +70,7 @@ box_pose, ) robot.setJointBounds( - boxes[i] + '/root_joint', - [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] ) tmp = boxes[0] @@ -95,47 +92,48 @@ # Calculate box positions rankB = list() for i in range(K): - joint_id = robot.model().getJointId(boxes[i] + '/root_joint') - rankB.append (robot.model().idx_qs[joint_id]) + joint_id = robot.model().getJointId(boxes[i] + "/root_joint") + rankB.append(robot.model().idx_qs[joint_id]) -bb = [0.7, 0.8, 0., 0.1] +bb = [0.7, 0.8, 0.0, 0.1] c = sqrt(2) / 2 xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) -nbCols = int(K * 1. / nBoxPerLine + 0.5) +nbCols = int(K * 1.0 / nBoxPerLine + 0.5) ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) for i in range(K): iL = i % nBoxPerLine iC = (i - iL) / nBoxPerLine x = bb[0] + xstep * iL y = bb[2] + xstep * iC - q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c] q_goal = q_init[::].copy() for i in range(K): r = rankB[i] rn = rankB[goal[i]] - q_goal[r:r+7] = q_init[rn:rn+7] + q_goal[r : r + 7] = q_init[rn : rn + 7] constraints = dict() graphConstraints = dict() jointNames = dict() -jointNames['all'] = robot.model().names -jointNames['baxterRightSide'] = list() -jointNames['baxterLeftSide'] = list() +jointNames["all"] = robot.model().names +jointNames["baxterRightSide"] = list() +jointNames["baxterLeftSide"] = list() -for n in jointNames['all']: +for n in jointNames["all"]: if n.startswith("baxter"): if n.startswith("baxter/left_"): - jointNames['baxterLeftSide'].append(n) + jointNames["baxterLeftSide"].append(n) if n.startswith("baxter/right_"): - jointNames['baxterRightSide'].append(n) + jointNames["baxterRightSide"].append(n) # Lock finger joints -lockFingers = ["r_gripper_l_finger", - "r_gripper_r_finger", - "l_gripper_l_finger", - "l_gripper_r_finger", - ] +lockFingers = [ + "r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", +] for side in ["r", "l"]: joint_name = "baxter/" + side + "_gripper_r_finger_joint" cs = LockedJoint(robot, joint_name, np.array([-0.02])) @@ -148,18 +146,18 @@ # Lock head -lockHead = 'head_pan' -joint_name = 'baxter/head_pan' +lockHead = "head_pan" +joint_name = "baxter/head_pan" joint_id = robot.model().getJointId(joint_name) cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) constraints[lockHead] = cs graphConstraints[lockHead] = cs for n in jointNames["baxterRightSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs for n in jointNames["baxterLeftSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs # Define handles and contact surfaces @@ -175,20 +173,20 @@ factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) -factory.environmentContacts(['table/pancake_table_table_top']) +factory.environmentContacts(["table/pancake_table_table_top"]) factory.setObjects(boxes, handlesPerObject, objContactSurfaces) factory.setRules(rules) factory.generate() -cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.addNumericalConstraintsToGraph(list(graphConstraints.values())) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: - raise Exception('Init configuration could not be projected.') + raise Exception("Init configuration could not be projected.") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: - raise Exception('Goal configuration could not be projected.') + raise Exception("Goal configuration could not be projected.") problem.initConfig(q_init_proj) @@ -200,14 +198,19 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") optimizers = { - 'GraphPartialShortcut': GraphPartialShortcut(problem), - 'GraphRandomShortcut': GraphRandomShortcut(problem), - 'PartialShortcut': PartialShortcut(problem), - 'RandomShortcut': RandomShortcut(problem), - 'SimpleShortcut': SimpleShortcut(problem), + "GraphPartialShortcut": GraphPartialShortcut(problem), + "GraphRandomShortcut": GraphRandomShortcut(problem), + "PartialShortcut": PartialShortcut(problem), + "RandomShortcut": RandomShortcut(problem), + "SimpleShortcut": SimpleShortcut(problem), } -optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', - 'RandomShortcut', 'SimpleShortcut'] +optimizerNames = [ + "GraphPartialShortcut", + "GraphRandomShortcut", + "PartialShortcut", + "RandomShortcut", + "SimpleShortcut", +] iOpt = 0 totalTime = dt.timedelta(0) @@ -219,7 +222,7 @@ iOpt += 1 if iOpt == len(optimizerNames): iOpt = 0 - + try: planner.roadmap().clear() problem.resetGoalConfigs() diff --git a/tests/benchmarks/construction-set-m-rrt.py b/tests/benchmarks/construction-set-m-rrt.py index bf23ba8..ee9b225 100644 --- a/tests/benchmarks/construction-set-m-rrt.py +++ b/tests/benchmarks/construction-set-m-rrt.py @@ -22,19 +22,22 @@ from pyhpp.manipulation.security_margins import SecurityMargins parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') -parser.add_argument('--bigGraph', action='store_true', +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") +parser.add_argument( + "--bigGraph", + action="store_true", help="Whether constraint graph is generated with all the possible states. " - "If unspecified, constraint graph only has a few states traversed by " - "one safe path.") + "If unspecified, constraint graph only has a few states traversed by " + "one safe path.", +) args = parser.parse_args() class StateName(object): - noGrasp = 'free' - + noGrasp = "free" + def __init__(self, grasps): if isinstance(grasps, set): self.grasps = grasps.copy() @@ -42,22 +45,22 @@ def __init__(self, grasps): if grasps == self.noGrasp: self.grasps = set() else: - g1 = [s.strip(' ') for s in grasps.split(':')] - self.grasps = set([tuple(s.split(' grasps ')) for s in g1]) + g1 = [s.strip(" ") for s in grasps.split(":")] + self.grasps = set([tuple(s.split(" grasps ")) for s in g1]) else: - raise TypeError('expecting a set of pairs (gripper, handle) or a string') - + raise TypeError("expecting a set of pairs (gripper, handle) or a string") + def __str__(self): if self.grasps == set(): - return 'free' + return "free" res = "" for g in self.grasps: res += g[0] + " grasps " + g[1] + " : " return res[:-3] - + def __eq__(self, other): return self.grasps == other.grasps - + def __ne__(self, other): return not self.__eq__(other) @@ -74,20 +77,20 @@ def __ne__(self, other): nSphere = 2 nCylinder = 2 -robot = Device('2ur5-sphere') +robot = Device("2ur5-sphere") -r0_pose = SE3(rotation=np.identity(3), translation=np.array([-.25, 0, 0])) +r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) urdf.loadModel(robot, 0, "r0", "anchor", ur3_urdf, ur3_srdf, r0_pose) -r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([.25, 0, 0])) +r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([0.25, 0, 0])) urdf.loadModel(robot, 0, "r1", "anchor", ur3_urdf, ur3_srdf, r1_pose) -robot.setJointBounds('r0/shoulder_pan_joint', [-pi, 4]) -robot.setJointBounds('r1/shoulder_pan_joint', [-pi, 4]) -robot.setJointBounds('r0/shoulder_lift_joint', [-pi, 0]) -robot.setJointBounds('r1/shoulder_lift_joint', [-pi, 0]) -robot.setJointBounds('r0/elbow_joint', [-2.6, 2.6]) -robot.setJointBounds('r1/elbow_joint', [-2.6, 2.6]) +robot.setJointBounds("r0/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r1/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r0/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r1/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r0/elbow_joint", [-2.6, 2.6]) +robot.setJointBounds("r1/elbow_joint", [-2.6, 2.6]) ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "ground", "anchor", ground_urdf, ground_srdf, ground_pose) @@ -95,23 +98,61 @@ def __ne__(self, other): objects = list() for i in range(nSphere): sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) - urdf.loadModel(robot, 0, f'sphere{i}', "freeflyer", sphere_urdf, sphere_srdf, sphere_pose) + urdf.loadModel( + robot, 0, f"sphere{i}", "freeflyer", sphere_urdf, sphere_srdf, sphere_pose + ) robot.setJointBounds( - f'sphere{i}/root_joint', - [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, - -1.0001, 1.0001, -1.0001, 1.0001] + f"sphere{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], ) - objects.append(f'sphere{i}') + objects.append(f"sphere{i}") for i in range(nCylinder): cylinder_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) - urdf.loadModel(robot, 0, f'cylinder{i}', "freeflyer", cylinder_08_urdf, cylinder_08_srdf, cylinder_pose) + urdf.loadModel( + robot, + 0, + f"cylinder{i}", + "freeflyer", + cylinder_08_urdf, + cylinder_08_srdf, + cylinder_pose, + ) robot.setJointBounds( - f'cylinder{i}/root_joint', - [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, - -1.0001, 1.0001, -1.0001, 1.0001] + f"cylinder{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], ) - objects.append(f'cylinder{i}') + objects.append(f"cylinder{i}") model = robot.model() @@ -130,42 +171,68 @@ def __ne__(self, other): Id = SE3.Identity() spherePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) joint = model.getJointId(f"sphere{i}/root_joint") - + pc = Transformation( - placementName, robot, joint, Id, spherePlacement, + placementName, + robot, + joint, + Id, + spherePlacement, [False, False, True, True, True, False], ) cts = ComparisonTypes() - cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) constraints[placementName] = Implicit(pc, cts, [True, True, True]) - + pc_complement = Transformation( - placementName + "/complement", robot, joint, Id, spherePlacement, + placementName + "/complement", + robot, + joint, + Id, + spherePlacement, [True, True, False, False, False, True], ) cts_complement = ComparisonTypes() - cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) - constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) - + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + cts_hold = ComparisonTypes() cts_hold[:] = ( - ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, - ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, ) constraints[placementName + "/hold"] = LockedJoint( robot, f"sphere{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold ) - + cg.registerConstraints( constraints[placementName], constraints[placementName + "/complement"], constraints[placementName + "/hold"], ) - + preplacementName = f"preplace_sphere{i}" spherePrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) pc_pre = Transformation( - preplacementName, robot, joint, Id, spherePrePlacement, + preplacementName, + robot, + joint, + Id, + spherePrePlacement, [False, False, True, True, True, False], ) constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) @@ -175,52 +242,78 @@ def __ne__(self, other): Id = SE3.Identity() cylinderPlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) joint = model.getJointId(f"cylinder{i}/root_joint") - + pc = Transformation( - placementName, robot, joint, Id, cylinderPlacement, + placementName, + robot, + joint, + Id, + cylinderPlacement, [False, False, True, True, True, False], ) cts = ComparisonTypes() - cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) constraints[placementName] = Implicit(pc, cts, [True, True, True]) - + pc_complement = Transformation( - placementName + "/complement", robot, joint, Id, cylinderPlacement, + placementName + "/complement", + robot, + joint, + Id, + cylinderPlacement, [True, True, False, False, False, True], ) cts_complement = ComparisonTypes() - cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) - constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) - + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + cts_hold = ComparisonTypes() cts_hold[:] = ( - ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, - ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, ) constraints[placementName + "/hold"] = LockedJoint( robot, f"cylinder{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold ) - + cg.registerConstraints( constraints[placementName], constraints[placementName + "/complement"], constraints[placementName + "/hold"], ) - + preplacementName = f"preplace_cylinder{i}" cylinderPrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) pc_pre = Transformation( - preplacementName, robot, joint, Id, cylinderPrePlacement, + preplacementName, + robot, + joint, + Id, + cylinderPrePlacement, [False, False, True, True, True, False], ) constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) -grippers = [f'cylinder{i}/magnet0' for i in range(nCylinder)] -grippers += [f'cylinder{i}/magnet1' for i in range(nCylinder)] +grippers = [f"cylinder{i}/magnet0" for i in range(nCylinder)] +grippers += [f"cylinder{i}/magnet1" for i in range(nCylinder)] grippers += [f"r{i}/gripper" for i in range(2)] -handlesPerObjects = [[f'sphere{i}/handle', f'sphere{i}/magnet'] for i in range(nSphere)] -handlesPerObjects += [[f'cylinder{i}/handle'] for i in range(nCylinder)] +handlesPerObjects = [[f"sphere{i}/handle", f"sphere{i}/magnet"] for i in range(nSphere)] +handlesPerObjects += [[f"cylinder{i}/handle"] for i in range(nCylinder)] shapesPerObject = [[] for o in objects] @@ -230,13 +323,13 @@ def __ne__(self, other): def makeRule(grasps): _grippers = list() _handles = list() - for (g, h) in grasps: + for g, h in grasps: _grippers.append(g) _handles.append(h) for g in grippers: - if not g in _grippers: + if g not in _grippers: _grippers.append(g) - _handles += (len(_grippers) - len(_handles)) * ['^$'] + _handles += (len(_grippers) - len(_handles)) * ["^$"] return Rule(grippers=_grippers, handles=_handles, link=True) @@ -245,23 +338,26 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: hRegex = [re.compile(handle) for handle in h] forbiddenList: T.List[Rule] = list() idForbidGrippers = [i for i in range(len(grippers)) if gRegex.match(grippers[i])] - forbidHandles = [handle for handle in allHandles - if not any([handlePattern.match(handle) for handlePattern in hRegex])] + forbidHandles = [ + handle + for handle in allHandles + if not any([handlePattern.match(handle) for handlePattern in hRegex]) + ] for id in idForbidGrippers: for handle in forbidHandles: - _handles = [handle if i == id else '.*' for i in range(len(grippers))] + _handles = [handle if i == id else ".*" for i in range(len(grippers))] forbiddenList.append(Rule(grippers=grippers, handles=_handles, link=False)) return forbiddenList -q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0] +q0_r0 = [pi / 6, -pi / 2, pi / 2, 0, 0, 0] q0_r1 = q0_r0[::] q0_spheres = list() i = 0 y = 0.04 while i < nSphere: - q0_spheres.append([-.1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + q0_spheres.append([-0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -269,7 +365,7 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: i = 0 y = -0.04 while i < nCylinder: - q0_cylinders.append([0.45 + .1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + q0_cylinders.append([0.45 + 0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -280,103 +376,141 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: grasps = set() nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r0/gripper', 'sphere0/handle')) + + grasps.add(("r0/gripper", "sphere0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r1/gripper', 'cylinder0/handle')) + + grasps.add(("r1/gripper", "cylinder0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('cylinder0/magnet0', 'sphere0/magnet')) + + grasps.add(("cylinder0/magnet0", "sphere0/magnet")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r0/gripper', 'sphere0/handle')) + + grasps.remove(("r0/gripper", "sphere0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r0/gripper', 'sphere1/handle')) + + grasps.add(("r0/gripper", "sphere1/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('cylinder0/magnet1', 'sphere1/magnet')) + + grasps.add(("cylinder0/magnet1", "sphere1/magnet")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r0/gripper', 'sphere1/handle')) + + grasps.remove(("r0/gripper", "sphere1/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r1/gripper', 'cylinder0/handle')) + + grasps.remove(("r1/gripper", "cylinder0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - + problem.steeringMethod = Straight(problem) problem.pathValidation = Progressive(robot, 0.02) problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, shapesPerObject) factory.setRules(rules) factory.generate() - - sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", - "cylinder0", "cylinder1"], robot) + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) sm.defaultMargin = 0.02 sm.apply() - + cg.initialize() - + problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + nodes.append(nodes[-1]) else: rules = list() - - rules.extend(forbidExcept('^r0/gripper$', ['^sphere\\d*/handle$'])) - rules.extend(forbidExcept('^r1/gripper$', ['^cylinder0*/handle$'])) - rules.extend(forbidExcept('^cylinder0/magnet\\d*$', ['^sphere\\d*/magnet$'])) - rules.extend(forbidExcept('^cylinder[1-9][0-9]*.*$', ['^$'])) - + + rules.extend(forbidExcept("^r0/gripper$", ["^sphere\\d*/handle$"])) + rules.extend(forbidExcept("^r1/gripper$", ["^cylinder0*/handle$"])) + rules.extend(forbidExcept("^cylinder0/magnet\\d*$", ["^sphere\\d*/magnet$"])) + rules.extend(forbidExcept("^cylinder[1-9][0-9]*.*$", ["^$"])) + rules.append(Rule(grippers=grippers, handles=[".*"] * len(grippers), link=True)) - + problem.steeringMethod = Straight(problem) problem.pathValidation = Progressive(robot, 0.02) problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, shapesPerObject) factory.setRules(rules) factory.generate() - - sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", - "cylinder0", "cylinder1"], robot) + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) sm.defaultMargin = 0.02 sm.apply() - + cg.initialize() - + problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) -assert (nCylinder == 2 and nSphere == 2) +assert nCylinder == 2 and nSphere == 2 c = sqrt(2) / 2 -q_goal = np.array(q0_r0 + q0_r1 + [-0.06202136144745322, -0.15, 0.025, c, 0, -c, 0, - 0.06202136144745322, -0.15, 0.025, c, 0, c, 0, - 0, -0.15, 0.025, 0, 0, 0, 1, - 0.5, -0.08, 0.025, 0, 0, 0, 1]) +q_goal = np.array( + q0_r0 + + q0_r1 + + [ + -0.06202136144745322, + -0.15, + 0.025, + c, + 0, + -c, + 0, + 0.06202136144745322, + -0.15, + 0.025, + c, + 0, + c, + 0, + 0, + -0.15, + 0.025, + 0, + 0, + 0, + 1, + 0.5, + -0.08, + 0.025, + 0, + 0, + 0, + 1, + ] +) problem.initConfig(q0) problem.addGoalConfig(q_goal) @@ -414,4 +548,4 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py index 29ffa92..f7695eb 100644 --- a/tests/benchmarks/pr2-in-iai-kitchen.py +++ b/tests/benchmarks/pr2-in-iai-kitchen.py @@ -9,16 +9,16 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" -robot = Device('pr2') +robot = Device("pr2") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -34,17 +34,17 @@ q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = model.idx_qs[model.getJointId('pr2/torso_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/torso_lift_joint")] q_init[rank] = 0.2 q_goal[0:2] = [-3.2, -4] -rank = model.idx_qs[model.getJointId('pr2/l_shoulder_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/l_shoulder_lift_joint")] q_goal[rank] = 0.5 -rank = model.idx_qs[model.getJointId('pr2/r_shoulder_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/r_shoulder_lift_joint")] q_goal[rank] = 0.5 -rank = model.idx_qs[model.getJointId('pr2/l_elbow_flex_joint')] +rank = model.idx_qs[model.getJointId("pr2/l_elbow_flex_joint")] q_goal[rank] = -0.5 -rank = model.idx_qs[model.getJointId('pr2/r_elbow_flex_joint')] +rank = model.idx_qs[model.getJointId("pr2/r_elbow_flex_joint")] q_goal[rank] = -0.5 problem = Problem(robot) @@ -85,4 +85,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py index f7a4531..2a9bb92 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen-spf.py +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -1,7 +1,7 @@ #!/usr/bin/env python from argparse import ArgumentParser -from math import sqrt, pi +from math import sqrt import numpy as np import datetime as dt @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,10 +35,14 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) +robot.setJointBounds( + "box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2] +) model = robot.model() @@ -44,7 +50,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -53,45 +59,46 @@ c = sqrt(2) / 2 q_init = robot.currentConfiguration() -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -4, 0.9, 0, c, 0, c] q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/l_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/l_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/r_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/r_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] - -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs - -grippers = ['pr2/l_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle']] -envSurfaces = ['kitchen_area/pancake_table_table_top', - 'kitchen_area/white_counter_top_sink'] -objContactSurfaces = [['box/box_surface']] - -rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), - Rule([""], [""], True)] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs + +grippers = ["pr2/l_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle"]] +envSurfaces = [ + "kitchen_area/pancake_table_table_top", + "kitchen_area/white_counter_top_sink", +] +objContactSurfaces = [["box/box_surface"]] + +rules = [Rule(["pr2/l_gripper"], ["box/handle"], True), Rule([""], [""], True)] factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) @@ -102,19 +109,19 @@ cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) -cg.setWeight(cg.getTransition('Loop | f'), 1) -cg.setWeight(cg.getTransition('Loop | 0-0'), 1) +cg.setWeight(cg.getTransition("Loop | f"), 1) +cg.setWeight(cg.getTransition("Loop | 0-0"), 1) problem.steeringMethod = Straight(problem) problem.pathValidation = Dichotomy(robot, 0.0) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -159,4 +166,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py index 50685a8..10d18e7 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen.py +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -1,7 +1,7 @@ #!/usr/bin/env python from argparse import ArgumentParser -from math import sqrt, pi +from math import sqrt import numpy as np import datetime as dt @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,10 +35,14 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) +robot.setJointBounds( + "box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2] +) model = robot.model() @@ -44,7 +50,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -53,45 +59,46 @@ c = sqrt(2) / 2 q_init = robot.currentConfiguration() -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -4, 0.9, 0, c, 0, c] q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/l_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/l_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/r_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/r_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] - -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs - -grippers = ['pr2/l_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle']] -envSurfaces = ['kitchen_area/pancake_table_table_top', - 'kitchen_area/white_counter_top_sink'] -objContactSurfaces = [['box/box_surface']] - -rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), - Rule([""], [""], True)] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs + +grippers = ["pr2/l_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle"]] +envSurfaces = [ + "kitchen_area/pancake_table_table_top", + "kitchen_area/white_counter_top_sink", +] +objContactSurfaces = [["box/box_surface"]] + +rules = [Rule(["pr2/l_gripper"], ["box/handle"], True), Rule([""], [""], True)] factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) @@ -102,19 +109,19 @@ cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) -cg.setWeight(cg.getTransition('Loop | f'), 1) -cg.setWeight(cg.getTransition('Loop | 0-0'), 1) +cg.setWeight(cg.getTransition("Loop | f"), 1) +cg.setWeight(cg.getTransition("Loop | 0-0"), 1) problem.steeringMethod = Straight(problem) problem.pathValidation = Dichotomy(robot, 0.0) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -155,4 +162,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py index 85c2a85..495d668 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand-spf.py +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,7 +35,9 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) 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]) @@ -44,7 +48,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -55,63 +59,75 @@ q_init = robot.currentConfiguration() q_init[0:4] = [-3.2, -4, 1, 0] -rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 q_goal = q_init.copy() -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs -lockrhand = ['r_l_finger', 'r_r_finger'] -cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) -constraints['r_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) -constraints['r_r_finger'] = cs +lockrhand = ["r_l_finger", "r_r_finger"] +cs = LockedJoint(robot, "pr2/r_gripper_l_finger_joint", np.array([0.5])) +constraints["r_l_finger"] = cs +cs = LockedJoint(robot, "pr2/r_gripper_r_finger_joint", np.array([0.5])) +constraints["r_r_finger"] = cs lockhands = lockrhand + locklhand -lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] -cs = LockedJoint(robot, 'pr2/head_pan_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) -constraints['head_pan'] = cs -cs = LockedJoint(robot, 'pr2/head_tilt_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) -constraints['head_tilt'] = cs -cs = LockedJoint(robot, 'pr2/torso_lift_joint', - np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) -constraints['torso'] = cs -cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', - np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) -constraints['laser'] = cs - -lockPlanar = ['lockplanar'] -cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) -constraints['lockplanar'] = cs +lockHeadAndTorso = ["head_pan", "head_tilt", "torso", "laser"] +cs = LockedJoint( + robot, + "pr2/head_pan_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_pan_joint"]]]), +) +constraints["head_pan"] = cs +cs = LockedJoint( + robot, + "pr2/head_tilt_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_tilt_joint"]]]), +) +constraints["head_tilt"] = cs +cs = LockedJoint( + robot, + "pr2/torso_lift_joint", + np.array([q_init[robot.rankInConfiguration["pr2/torso_lift_joint"]]]), +) +constraints["torso"] = cs +cs = LockedJoint( + robot, + "pr2/laser_tilt_mount_joint", + np.array([q_init[robot.rankInConfiguration["pr2/laser_tilt_mount_joint"]]]), +) +constraints["laser"] = cs + +lockPlanar = ["lockplanar"] +cs = LockedJoint(robot, "pr2/root_joint", np.array([-3.2, -4, 1, 0])) +constraints["lockplanar"] = cs lockAll = lockhands + lockHeadAndTorso + lockPlanar -grippers = ['pr2/l_gripper', 'pr2/r_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle', 'box/handle2']] -objContactSurfaces = [['box/box_surface']] -envSurfaces = ['kitchen_area/pancake_table_table_top'] +grippers = ["pr2/l_gripper", "pr2/r_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle", "box/handle2"]] +objContactSurfaces = [["box/box_surface"]] +envSurfaces = ["kitchen_area/pancake_table_table_top"] rules = [Rule([""], [""], True)] @@ -132,11 +148,11 @@ cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -181,4 +197,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py index b4fad23..0465e9c 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand.py +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,7 +35,9 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) 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]) @@ -44,7 +48,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -55,64 +59,76 @@ q_init = robot.currentConfiguration() q_init[0:4] = [-3.2, -4, 1, 0] -rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 q_goal = q_init.copy() -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs -lockrhand = ['r_l_finger', 'r_r_finger'] -cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) -constraints['r_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) -constraints['r_r_finger'] = cs +lockrhand = ["r_l_finger", "r_r_finger"] +cs = LockedJoint(robot, "pr2/r_gripper_l_finger_joint", np.array([0.5])) +constraints["r_l_finger"] = cs +cs = LockedJoint(robot, "pr2/r_gripper_r_finger_joint", np.array([0.5])) +constraints["r_r_finger"] = cs lockhands = lockrhand + locklhand -lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] -cs = LockedJoint(robot, 'pr2/head_pan_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) -constraints['head_pan'] = cs -cs = LockedJoint(robot, 'pr2/head_tilt_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) -constraints['head_tilt'] = cs -cs = LockedJoint(robot, 'pr2/torso_lift_joint', - np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) -constraints['torso'] = cs -cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', - np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) -constraints['laser'] = cs - -lockPlanar = ['lockplanar'] -rank = robot.rankInConfiguration['pr2/root_joint'] -cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) -constraints['lockplanar'] = cs +lockHeadAndTorso = ["head_pan", "head_tilt", "torso", "laser"] +cs = LockedJoint( + robot, + "pr2/head_pan_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_pan_joint"]]]), +) +constraints["head_pan"] = cs +cs = LockedJoint( + robot, + "pr2/head_tilt_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_tilt_joint"]]]), +) +constraints["head_tilt"] = cs +cs = LockedJoint( + robot, + "pr2/torso_lift_joint", + np.array([q_init[robot.rankInConfiguration["pr2/torso_lift_joint"]]]), +) +constraints["torso"] = cs +cs = LockedJoint( + robot, + "pr2/laser_tilt_mount_joint", + np.array([q_init[robot.rankInConfiguration["pr2/laser_tilt_mount_joint"]]]), +) +constraints["laser"] = cs + +lockPlanar = ["lockplanar"] +rank = robot.rankInConfiguration["pr2/root_joint"] +cs = LockedJoint(robot, "pr2/root_joint", np.array([-3.2, -4, 1, 0])) +constraints["lockplanar"] = cs lockAll = lockhands + lockHeadAndTorso + lockPlanar -grippers = ['pr2/l_gripper', 'pr2/r_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle', 'box/handle2']] -objContactSurfaces = [['box/box_surface']] -envSurfaces = ['kitchen_area/pancake_table_table_top'] +grippers = ["pr2/l_gripper", "pr2/r_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle", "box/handle2"]] +objContactSurfaces = [["box/box_surface"]] +envSurfaces = ["kitchen_area/pancake_table_table_top"] rules = [Rule([""], [""], True)] @@ -133,11 +149,11 @@ cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -178,4 +194,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/romeo-placard-spf.py b/tests/benchmarks/romeo-placard-spf.py index 72a7262..100535f 100644 --- a/tests/benchmarks/romeo-placard-spf.py +++ b/tests/benchmarks/romeo-placard-spf.py @@ -1,44 +1,53 @@ #!/usr/bin/env python from argparse import ArgumentParser -import time import numpy as np import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder from pyhpp.core import Dichotomy, Straight, ProgressiveProjector -from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.constraints import LockedJoint from pyhpp.core.static_stability_constraint_factory import ( StaticStabilityConstraintsFactory, ) from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" -romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +romeo_srdf_moveit = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +) placard_urdf = "package://hpp_environments/urdf/placard.urdf" placard_srdf = "package://hpp_environments/srdf/placard.srdf" -robot = Device('romeo-placard') +robot = Device("romeo-placard") # Load Romeo robot romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) +urdf.loadModel( + robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose +) -robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "romeo/root_joint", [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) # Load placard placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) +urdf.loadModel( + robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose +) -robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "placard/root_joint", [-1, 1, -1, 1, 0, 1.5, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) model = robot.model() @@ -88,7 +97,7 @@ # Lock left hand locklhand = list() for j, v in leftHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j locklhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -101,7 +110,7 @@ # Lock right hand lockrhand = list() for j, v in rightHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j lockrhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -115,13 +124,13 @@ # Create static stability constraint q = robot.currentConfiguration() -placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] -q[placard_rank:placard_rank+3] = [.4, 0, 1.2] +placard_rank = model.idx_qs[model.getJointId("placard/root_joint")] +q[placard_rank : placard_rank + 3] = [0.4, 0, 1.2] problem.addPartialCom("romeo", ["romeo/root_joint"]) -leftAnkle = 'romeo/LAnkleRoll' -rightAnkle = 'romeo/RAnkleRoll' +leftAnkle = "romeo/LAnkleRoll" +rightAnkle = "romeo/RAnkleRoll" factory = StaticStabilityConstraintsFactory(problem, robot) balanceConstraintsDict = factory.createStaticStabilityConstraint( @@ -129,9 +138,9 @@ ) balanceConstraints = [ - balanceConstraintsDict.get('balance/pose-left-foot'), - balanceConstraintsDict.get('balance/pose-right-foot'), - balanceConstraintsDict.get('balance/relative-com'), + balanceConstraintsDict.get("balance/pose-left-foot"), + balanceConstraintsDict.get("balance/pose-right-foot"), + balanceConstraintsDict.get("balance/relative-com"), ] balanceConstraints = [c for c in balanceConstraints if c is not None] @@ -141,8 +150,8 @@ graphConstraints[name] = constraint # Build graph -grippers = ['romeo/r_hand', 'romeo/l_hand'] -handlesPerObjects = [['placard/low', 'placard/high']] +grippers = ["romeo/r_hand", "romeo/l_hand"] +handlesPerObjects = [["placard/low", "placard/high"]] rules = [ Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), @@ -152,7 +161,7 @@ factory_cg = ConstraintGraphFactory(cg, constraints) factory_cg.setGrippers(grippers) -factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setObjects(["placard"], handlesPerObjects, [[]]) factory_cg.setRules(rules) factory_cg.generate() @@ -162,11 +171,83 @@ cg.initialize() # Define initial and final configurations -q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_goal = np.array( + [ + -0.003429678026293006, + 7.761615492429529e-05, + 0.8333148411182841, + -0.08000440760954532, + 0.06905332841243099, + -0.09070086400314036, + 0.9902546570793265, + 0.2097693637044623, + 0.19739743868699455, + -0.6079135018296973, + 0.8508704420155889, + -0.39897628829947995, + -0.05274298289004072, + 0.20772797293264825, + 0.1846394290733244, + -0.49824886682709824, + 0.5042013065348324, + -0.16158420369261683, + -0.039828502509861335, + -0.3827070014985058, + -0.24118425356319423, + 1.0157846623463191, + 0.5637424355124602, + -1.3378817283780955, + -1.3151786907256797, + -0.392409481224193, + 0.11332560818107676, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.35936687035487364, + -0.32595302056157444, + -0.33115291290191723, + 0.20387672048126043, + 0.9007626913161502, + -0.39038645767349395, + 0.31725226129015516, + 1.5475253831101246, + -0.0104572058777634, + 0.32681856374063933, + 0.24476959944940427, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.412075621240969, + 0.020809907186176854, + 1.056724788359247, + 0.0, + 0.0, + 0.0, + 1.0, + ] +) q_init = q_goal.copy() -q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] +q_init[placard_rank + 3 : placard_rank + 7] = [0, 0, 1, 0] -n = 'romeo/l_hand grasps placard/low' +n = "romeo/l_hand grasps placard/low" state = cg.getState(n) res, q_init_proj, err = cg.applyStateConstraints(state, q_init) if not res: @@ -222,5 +303,3 @@ if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") print(f"Average number nodes per success: {totalNumberNodes / success}") - - diff --git a/tests/benchmarks/romeo-placard.py b/tests/benchmarks/romeo-placard.py index 8d7aed4..b5bcfcb 100644 --- a/tests/benchmarks/romeo-placard.py +++ b/tests/benchmarks/romeo-placard.py @@ -1,44 +1,53 @@ #!/usr/bin/env python from argparse import ArgumentParser -import time import numpy as np import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner from pyhpp.core import Dichotomy, Straight, ProgressiveProjector -from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.constraints import LockedJoint from pyhpp.core.static_stability_constraint_factory import ( StaticStabilityConstraintsFactory, ) from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" -romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +romeo_srdf_moveit = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +) placard_urdf = "package://hpp_environments/urdf/placard.urdf" placard_srdf = "package://hpp_environments/srdf/placard.srdf" -robot = Device('romeo-placard') +robot = Device("romeo-placard") # Load Romeo robot romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) +urdf.loadModel( + robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose +) -robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "romeo/root_joint", [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) # Load placard placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) +urdf.loadModel( + robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose +) -robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "placard/root_joint", [-1, 1, -1, 1, 0, 1.5, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) model = robot.model() @@ -88,7 +97,7 @@ # Lock left hand locklhand = list() for j, v in leftHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j locklhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -101,7 +110,7 @@ # Lock right hand lockrhand = list() for j, v in rightHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j lockrhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -115,13 +124,13 @@ # Create static stability constraint q = robot.currentConfiguration() -placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] -q[placard_rank:placard_rank+3] = [.4, 0, 1.2] +placard_rank = model.idx_qs[model.getJointId("placard/root_joint")] +q[placard_rank : placard_rank + 3] = [0.4, 0, 1.2] problem.addPartialCom("romeo", ["romeo/root_joint"]) -leftAnkle = 'romeo/LAnkleRoll' -rightAnkle = 'romeo/RAnkleRoll' +leftAnkle = "romeo/LAnkleRoll" +rightAnkle = "romeo/RAnkleRoll" factory = StaticStabilityConstraintsFactory(problem, robot) balanceConstraintsDict = factory.createStaticStabilityConstraint( @@ -129,9 +138,9 @@ ) balanceConstraints = [ - balanceConstraintsDict.get('balance/pose-left-foot'), - balanceConstraintsDict.get('balance/pose-right-foot'), - balanceConstraintsDict.get('balance/relative-com'), + balanceConstraintsDict.get("balance/pose-left-foot"), + balanceConstraintsDict.get("balance/pose-right-foot"), + balanceConstraintsDict.get("balance/relative-com"), ] balanceConstraints = [c for c in balanceConstraints if c is not None] @@ -141,8 +150,8 @@ graphConstraints[name] = constraint # Build graph -grippers = ['romeo/r_hand', 'romeo/l_hand'] -handlesPerObjects = [['placard/low', 'placard/high']] +grippers = ["romeo/r_hand", "romeo/l_hand"] +handlesPerObjects = [["placard/low", "placard/high"]] rules = [ Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), @@ -152,7 +161,7 @@ factory_cg = ConstraintGraphFactory(cg, constraints) factory_cg.setGrippers(grippers) -factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setObjects(["placard"], handlesPerObjects, [[]]) factory_cg.setRules(rules) factory_cg.generate() @@ -162,11 +171,83 @@ cg.initialize() # Define initial and final configurations -q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_goal = np.array( + [ + -0.003429678026293006, + 7.761615492429529e-05, + 0.8333148411182841, + -0.08000440760954532, + 0.06905332841243099, + -0.09070086400314036, + 0.9902546570793265, + 0.2097693637044623, + 0.19739743868699455, + -0.6079135018296973, + 0.8508704420155889, + -0.39897628829947995, + -0.05274298289004072, + 0.20772797293264825, + 0.1846394290733244, + -0.49824886682709824, + 0.5042013065348324, + -0.16158420369261683, + -0.039828502509861335, + -0.3827070014985058, + -0.24118425356319423, + 1.0157846623463191, + 0.5637424355124602, + -1.3378817283780955, + -1.3151786907256797, + -0.392409481224193, + 0.11332560818107676, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.35936687035487364, + -0.32595302056157444, + -0.33115291290191723, + 0.20387672048126043, + 0.9007626913161502, + -0.39038645767349395, + 0.31725226129015516, + 1.5475253831101246, + -0.0104572058777634, + 0.32681856374063933, + 0.24476959944940427, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.412075621240969, + 0.020809907186176854, + 1.056724788359247, + 0.0, + 0.0, + 0.0, + 1.0, + ] +) q_init = q_goal.copy() -q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] +q_init[placard_rank + 3 : placard_rank + 7] = [0, 0, 1, 0] -n = 'romeo/l_hand grasps placard/low' +n = "romeo/l_hand grasps placard/low" state = cg.getState(n) res, q_init_proj, err = cg.applyStateConstraints(state, q_init) if not res: @@ -217,5 +298,3 @@ if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") print(f"Average number nodes per success: {totalNumberNodes / success}") - - diff --git a/tests/benchmarks/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py index 582f773..4807938 100644 --- a/tests/benchmarks/test_benchmarks.py +++ b/tests/benchmarks/test_benchmarks.py @@ -3,14 +3,16 @@ import sys from pathlib import Path + def main(): folder = Path(sys.argv[1]) if len(sys.argv) > 1 else Path(".") - + for script in sorted(folder.glob("*.py")): if script.name == Path(__file__).name: continue - print(f"\n{'='*60}\nRunning: {script.name}\n{'='*60}") + print(f"\n{'=' * 60}\nRunning: {script.name}\n{'=' * 60}") subprocess.run([sys.executable, str(script), "-N", "1"]) + if __name__ == "__main__": - main() \ No newline at end of file + main()