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, diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py new file mode 100644 index 0000000..b05f2ac --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -0,0 +1,252 @@ +#!/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 ( + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +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() + +# 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, 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.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_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.0])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.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 = 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}") diff --git a/tests/benchmarks/baxter-manipulation-boxes.py b/tests/benchmarks/baxter-manipulation-boxes.py new file mode 100644 index 0000000..b434bb7 --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes.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.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 ( + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +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() + +# 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, 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.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_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.0])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.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..ee9b225 --- /dev/null +++ b/tests/benchmarks/construction-set-m-rrt.py @@ -0,0 +1,551 @@ +#!/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([-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([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]) + +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.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}") + +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.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}") + +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 g not 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([-0.1 * (i / 2), -0.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 + 0.1 * (i / 2), -0.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}") diff --git a/tests/benchmarks/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py new file mode 100644 index 0000000..f7695eb --- /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}") diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py new file mode 100644 index 0000000..2a9bb92 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -0,0 +1,169 @@ +#!/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 +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_obstacle.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(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/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py new file mode 100644 index 0000000..10d18e7 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -0,0 +1,165 @@ +#!/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 +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_obstacle.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}") 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..495d668 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -0,0 +1,200 @@ +#!/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_obstacle.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}") diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py new file mode 100644 index 0000000..0465e9c --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -0,0 +1,197 @@ +#!/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_obstacle.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}") 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..100535f --- /dev/null +++ b/tests/benchmarks/romeo-placard-spf.py @@ -0,0 +1,305 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +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 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.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 +) + +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() + +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] = [0.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..b5bcfcb --- /dev/null +++ b/tests/benchmarks/romeo-placard.py @@ -0,0 +1,300 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +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 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.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 +) + +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() + +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] = [0.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/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py new file mode 100644 index 0000000..4807938 --- /dev/null +++ b/tests/benchmarks/test_benchmarks.py @@ -0,0 +1,18 @@ +#!/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() 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 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")