From 14a02d554c31cb179e85a763c141a97edbb5eb71 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 9 Jan 2026 07:35:02 +0100 Subject: [PATCH 1/6] Fix utility function that would incorrectly truncate values --- src/pyhpp/manipulation/graph.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index b7ffc82..0337bcb 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -234,16 +234,16 @@ namespace manipulation { // Utility functions // ============================================================================= -std::vector> matrixToVectorVector( +std::vector> matrixToVectorVector( const Eigen::Ref>& input) { - std::vector> result; + std::vector> result; result.reserve(input.rows()); for (int i = 0; i < input.rows(); ++i) { - std::vector row; + std::vector row; row.reserve(input.cols()); for (int j = 0; j < input.cols(); ++j) { - row.push_back(static_cast(input(i, j))); + row.push_back(input(i, j)); } result.push_back(std::move(row)); } @@ -817,7 +817,7 @@ void PyWGraph::addLevelSetFoliation(PyWEdgePtr_t edge, boost::python::list PyWGraph::getSecurityMarginMatrixForTransition( PyWEdgePtr_t edge) { try { - std::vector> matrix = + std::vector> matrix = matrixToVectorVector(edge->obj->securityMargins()); boost::python::list result; From 62c2a0213864bd365e91bf7799811ee590453964 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 09:41:34 +0100 Subject: [PATCH 2/6] fix constraint creation argument bad conversion --- .../core/static_stability_constraint_factory.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 0f8bf5d..cc9010b 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -28,6 +28,7 @@ # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH # DAMAGE. +import numpy as np from hpp import Transform from pinocchio import SE3 @@ -39,12 +40,10 @@ def __init__(self, problem, robot): self.robot = robot def _getCOM(self, com): - from numpy import array - if com == "": - return array(self.robot.getCenterOfMass()) + return np.array(self.robot.getCenterOfMass()) else: - return array(self.problem.getPartialCom(com)) + return np.array(self.problem.getPartialCom(com)) # # Create static stability constraints where the robot slides on the ground, # # \param prefix prefix of the names of the constraint @@ -171,14 +170,13 @@ def createAlignedCOMStabilityConstraint( # COM between feet result.append(prefix + "com-between-feet") - # TODO: verify createComBetweenFeet parameters conversion constraint = self.problem.createComBetweenFeet( result[-1], comName, leftAnkle, rightAnkle, - [0, 0, 0], - [0, 0, 0], + np.zeros(3), + np.zeros(3), "", x, [True] * 4, From f619b56ca9daadb0122df8e7afcda95ac3615b25 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 09:45:07 +0100 Subject: [PATCH 3/6] Add more tests and homogeinize testing process --- src/pyhpp/constraints/by-substitution.cc | 6 + .../static_stability_constraint_factory.py | 2 +- tests/CMakeLists.txt | 42 +- .../baxter-manipulation-boxes-spf.py | 252 ----------- tests/benchmarks/baxter-manipulation-boxes.py | 251 ----------- tests/benchmarks/pr2-in-iai-kitchen.py | 88 ---- .../pr2-manipulation-kitchen-spf.py | 169 -------- tests/benchmarks/pr2-manipulation-kitchen.py | 165 ------- .../pr2-manipulation-two-hand-spf.py | 200 --------- tests/benchmarks/pr2-manipulation-two-hand.py | 197 --------- tests/benchmarks/pyrene-on-the-ground.py | 299 ------------- tests/benchmarks/romeo-placard-spf.py | 305 ------------- tests/benchmarks/test_benchmarks.py | 18 - tests/constraint_graph.py | 164 ------- tests/construction_set.py | 96 ---- tests/corbaserver-client.py | 4 - tests/corbaserver.py | 12 - tests/differentiable-function.py | 35 -- tests/graph_factory.py | 101 ----- tests/graph_factory2.py | 234 ---------- tests/imports.py | 0 tests/integration/benchmark_utils.py | 232 ++++++++++ .../construction-set-m-rrt.py | 410 ++++++------------ tests/integration/pr2-in-iai-kitchen.py | 64 +++ .../romeo-placard.py | 178 ++------ tests/integration/test_benchmarks.py | 55 +++ .../ur3-spheres-spf.py | 52 +-- .../ur3-spheres.py | 52 +-- tests/liegroup.py | 25 -- tests/load_ur3.py | 40 -- tests/motion_planner.py | 67 --- tests/non_linear_spline_gradient_based.py | 313 ------------- tests/path-optimizer.py | 60 --- tests/path-planner.py | 102 ----- tests/path.py | 118 ----- tests/problem-solver.py | 35 -- tests/rrt.py | 100 ----- tests/solver.py | 66 --- tests/test_non_linear_optimization.py | 14 - tests/unit/__init__.py | 1 + tests/unit/conftest.py | 263 +++++++++++ tests/unit/test_configuration_shooter.py | 51 +++ tests/unit/test_constraint_factory.py | 151 +++++++ tests/unit/test_constraint_graph_factory.py | 107 +++++ tests/unit/test_device.py | 190 ++++++++ tests/unit/test_differentiable_function.py | 83 ++++ tests/unit/test_handles_grippers.py | 109 +++++ tests/unit/test_liegroup.py | 97 +++++ tests/unit/test_path.py | 224 ++++++++++ tests/unit/test_path_optimizer.py | 146 +++++++ tests/unit/test_path_planner.py | 127 ++++++ tests/unit/test_path_projector.py | 115 +++++ tests/unit/test_path_validation.py | 107 +++++ tests/unit/test_position_constraint.py | 164 +++++++ tests/unit/test_problem.py | 149 +++++++ tests/unit/test_roadmap.py | 130 ++++++ tests/unit/test_security_margins.py | 221 ++++++++++ tests/unit/test_steering_method.py | 51 +++ 58 files changed, 3074 insertions(+), 4035 deletions(-) delete mode 100644 tests/benchmarks/baxter-manipulation-boxes-spf.py delete mode 100644 tests/benchmarks/baxter-manipulation-boxes.py delete mode 100644 tests/benchmarks/pr2-in-iai-kitchen.py delete mode 100644 tests/benchmarks/pr2-manipulation-kitchen-spf.py delete mode 100644 tests/benchmarks/pr2-manipulation-kitchen.py delete mode 100644 tests/benchmarks/pr2-manipulation-two-hand-spf.py delete mode 100644 tests/benchmarks/pr2-manipulation-two-hand.py delete mode 100755 tests/benchmarks/pyrene-on-the-ground.py delete mode 100644 tests/benchmarks/romeo-placard-spf.py delete mode 100644 tests/benchmarks/test_benchmarks.py delete mode 100644 tests/constraint_graph.py delete mode 100644 tests/construction_set.py delete mode 100644 tests/corbaserver-client.py delete mode 100644 tests/corbaserver.py delete mode 100644 tests/differentiable-function.py delete mode 100644 tests/graph_factory.py delete mode 100644 tests/graph_factory2.py delete mode 100644 tests/imports.py create mode 100644 tests/integration/benchmark_utils.py rename tests/{benchmarks => integration}/construction-set-m-rrt.py (54%) create mode 100644 tests/integration/pr2-in-iai-kitchen.py rename tests/{benchmarks => integration}/romeo-placard.py (50%) create mode 100644 tests/integration/test_benchmarks.py rename tests/{benchmarks => integration}/ur3-spheres-spf.py (87%) rename tests/{benchmarks => integration}/ur3-spheres.py (85%) delete mode 100644 tests/liegroup.py delete mode 100644 tests/load_ur3.py delete mode 100644 tests/motion_planner.py delete mode 100644 tests/non_linear_spline_gradient_based.py delete mode 100644 tests/path-optimizer.py delete mode 100644 tests/path-planner.py delete mode 100644 tests/path.py delete mode 100644 tests/problem-solver.py delete mode 100644 tests/rrt.py delete mode 100644 tests/solver.py delete mode 100644 tests/test_non_linear_optimization.py create mode 100644 tests/unit/__init__.py create mode 100644 tests/unit/conftest.py create mode 100644 tests/unit/test_configuration_shooter.py create mode 100644 tests/unit/test_constraint_factory.py create mode 100644 tests/unit/test_constraint_graph_factory.py create mode 100644 tests/unit/test_device.py create mode 100644 tests/unit/test_differentiable_function.py create mode 100644 tests/unit/test_handles_grippers.py create mode 100644 tests/unit/test_liegroup.py create mode 100644 tests/unit/test_path.py create mode 100644 tests/unit/test_path_optimizer.py create mode 100644 tests/unit/test_path_planner.py create mode 100644 tests/unit/test_path_projector.py create mode 100644 tests/unit/test_path_validation.py create mode 100644 tests/unit/test_position_constraint.py create mode 100644 tests/unit/test_problem.py create mode 100644 tests/unit/test_roadmap.py create mode 100644 tests/unit/test_security_margins.py create mode 100644 tests/unit/test_steering_method.py diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index c2b3862..11616a3 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -45,6 +45,12 @@ tuple BySubstitution_solve(const BySubstitution& hs, const vector_t& q) { } void exposeBySubstitution() { + enum_("SolverStatus") + .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED) + .value("MAX_ITERATION_REACHED", HierarchicalIterative::MAX_ITERATION_REACHED) + .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) + .value("SUCCESS", HierarchicalIterative::SUCCESS); + class_ >( "BySubstitution", init()) .def("explicitConstraintSetHasChanged", diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index cc9010b..95c8ace 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -178,7 +178,7 @@ def createAlignedCOMStabilityConstraint( np.zeros(3), np.zeros(3), "", - x, + x,git [True] * 4, ) created_constraints[result[-1]] = constraint diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index be168d4..bd0d927 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,16 +12,9 @@ # details. You should have received a copy of the GNU Lesser General Public # License along with hpp-python If not, see . -set(PYTHON_UNIT_TESTS - imports - liegroup - load_ur3 - differentiable-function - path-planner - rrt - path - constraint_graph - graph_factory2) +# ============================================================================= +# Existing Python unit tests (run as simple scripts) +# ============================================================================= foreach(UNIT_TEST ${PYTHON_UNIT_TESTS}) add_python_unit_test(${UNIT_TEST} tests/${UNIT_TEST}.py src) @@ -33,3 +26,32 @@ set_tests_properties( ENVIRONMENT_MODIFICATION "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;" ) + +# ============================================================================= +# unittest-based unit tests (uses Python's built-in unittest framework) +# ============================================================================= + +# Compute Python path for unit tests +compute_pythonpath(UNITTEST_ENV_VARIABLES src) + +# Discover and add individual test files +file(GLOB UNITTEST_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/unit/test_*.py") +foreach(TEST_FILE ${UNITTEST_TEST_FILES}) + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + add_test( + NAME unittest_${TEST_NAME} + COMMAND ${PYTHON_EXECUTABLE} -m unittest discover + -s ${CMAKE_CURRENT_SOURCE_DIR} + -p "${TEST_NAME}.py" + -v + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + ) + set_tests_properties( + unittest_${TEST_NAME} + PROPERTIES + ENVIRONMENT "${UNITTEST_ENV_VARIABLES}" + ENVIRONMENT_MODIFICATION + "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;" + LABELS "python;unit" + ) +endforeach() diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py deleted file mode 100644 index b05f2ac..0000000 --- a/tests/benchmarks/baxter-manipulation-boxes-spf.py +++ /dev/null @@ -1,252 +0,0 @@ -#!/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 deleted file mode 100644 index b434bb7..0000000 --- a/tests/benchmarks/baxter-manipulation-boxes.py +++ /dev/null @@ -1,251 +0,0 @@ -#!/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/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py deleted file mode 100644 index f7695eb..0000000 --- a/tests/benchmarks/pr2-in-iai-kitchen.py +++ /dev/null @@ -1,88 +0,0 @@ -#!/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 deleted file mode 100644 index 2a9bb92..0000000 --- a/tests/benchmarks/pr2-manipulation-kitchen-spf.py +++ /dev/null @@ -1,169 +0,0 @@ -#!/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 deleted file mode 100644 index 10d18e7..0000000 --- a/tests/benchmarks/pr2-manipulation-kitchen.py +++ /dev/null @@ -1,165 +0,0 @@ -#!/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 deleted file mode 100644 index 495d668..0000000 --- a/tests/benchmarks/pr2-manipulation-two-hand-spf.py +++ /dev/null @@ -1,200 +0,0 @@ -#!/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 deleted file mode 100644 index 0465e9c..0000000 --- a/tests/benchmarks/pr2-manipulation-two-hand.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/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 deleted file mode 100755 index b02d6de..0000000 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ /dev/null @@ -1,299 +0,0 @@ -#!/usr/bin/env python - -from argparse import ArgumentParser -import numpy as np -import datetime as dt - -from pyhpp.core.static_stability_constraint_factory import ( - StaticStabilityConstraintsFactory, -) -from pyhpp.core import ( - Problem, - DiffusingPlanner, - Dichotomy, - ProgressiveProjector, - Straight, -) -from pyhpp.constraints import LockedJoint -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() - -# Robot file paths -talos_urdf = "package://example-robot-data/robots/talos_data/robots/talos_full_v2.urdf" -talos_srdf = "package://example-robot-data/robots/talos_data/srdf/talos.srdf" - -robot = Device("pyrene") - -# Load Talos robot -talos_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "pyrene", "freeflyer", talos_urdf, talos_srdf, talos_pose) -robot.setJointBounds( - "pyrene/root_joint", [-3, 3, -3, 3, 0, 1, -1, 1, -1, 1, -1, 1, -1, 1] -) - -q0 = np.array( - [ - 0, - 0, - 1.0192720229567027, - 0, - 0, - 0, - 1, - 0.0, - 0.0, - -0.411354, - 0.859395, - -0.448041, - -0.001708, - 0.0, - 0.0, - -0.411354, - 0.859395, - -0.448041, - -0.001708, - 0, - 0.006761, - 0.25847, - 0.173046, - -0.0002, - -0.525366, - 0, - 0, - 0.1, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - -0.25847, - -0.173046, - 0.0002, - -0.525366, - 0, - 0, - 0.1, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - 0, - ] -) - -problem = Problem(robot) - -# Remove joint bound validation -problem.clearConfigValidations() -problem.addConfigValidation("CollisionValidation") - -problem.addPartialCom("pyrene", ["pyrene/root_joint"]) - -constraints = dict() - -leftAnkle = "pyrene/leg_left_6_joint" -rightAnkle = "pyrene/leg_right_6_joint" -factory = StaticStabilityConstraintsFactory(problem, robot) -constraints = factory.createSlidingStabilityConstraint( - "balance/", "pyrene", leftAnkle, rightAnkle, q0 -) -constraints.pop("balance/pose-left-foot-complement") - -# Lock gripper joints in closed position -model = robot.model() -jointNames = model.names - -gripperJoints = [j for j in jointNames if j.startswith("gripper_")] -for j in gripperJoints: - ljName = "locked_" + j - value = q0[robot.rankInConfiguration[j]] - cs = LockedJoint(robot, j, np.array([value])) - constraints[ljName] = cs - -# Lock torso joints -for j in ["pyrene/torso_1_joint", "pyrene/torso_2_joint"]: - ljName = "locked_" + j - value = q0[robot.rankInConfiguration[j]] - cs = LockedJoint(robot, j, np.array([value])) - constraints[ljName] = cs - -problem.addNumericalConstraintsToConfigProjector( - "balance-proj", list(constraints.values()), [0] * len(constraints) -) -# Set up path validation and projection -problem.pathValidation = Dichotomy(robot, 0) -problem.steeringMethod = Straight(problem) -problem.pathProjector = ProgressiveProjector( - problem.distance(), problem.steeringMethod, 0.2 -) - -q1 = np.array( - [ - 0.46186525119743765, - 0.7691484390667176, - 1.0, - 0.044318662659833724, - -0.0108631325758057, - -0.0005624014939687202, - 0.9989582234484302, - 0.007182038754728065, - -0.07804157609345573, - -0.45119414082769843, - 0.9175221606997885, - -0.44402665063685365, - -0.012200787668632173, - 0.007200628661317587, - -0.0788724231291963, - -0.4956000845048934, - 1.009916799073695, - -0.49201388832345117, - -0.011369733964913645, - 0.0, - 0.006761, - 0.2408808670823526, - 0.28558871367875255, - 0.021347338765649856, - -0.5979935578118766, - -0.0014717027925961507, - 0.006759032911476202, - 0.08832103581416396, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 1.0392843760345567, - -0.10575191252250002, - -0.05668798069441503, - -1.7498341362736458, - 0.0022744473854138473, - 0.0015716871843022243, - 0.07078184761729372, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.00020052684970875304, - 0.00019305414086825983, - ] -) - -q2 = np.array( - [ - -0.03792613133823603, - 0.24583989035169654, - 1.0, - 0.008581421388221004, - 0.044373915255123464, - -0.0006050481369481731, - 0.9989779520933587, - 0.0011692308149178052, - -0.011583002652064677, - -0.5522315456639073, - 0.9525259684676938, - -0.4890594525896807, - -0.007366718494771048, - 0.0011679806161439602, - -0.01159704912053673, - -0.5610095845869443, - 0.9704046648090222, - -0.49816012449736746, - -0.007352616506901346, - 0.0, - 0.006761, - 0.25575424894162485, - 0.21391256924828497, - 0.006460912367916318, - -0.5673886888192637, - -0.0007964566272850148, - 0.0027266557203091918, - 0.09323792816834059, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.5020992312243198, - -0.770681876188843, - 2.42600766027, - -1.8794064100743089, - 0.0019251455338804122, - 0.007445905986286772, - 0.06939811636044525, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - -0.0013061717130372818, - 4.856617592856522e-05, - ] -) - -result, q1proj, err = problem.applyConstraints(q1) -assert result -valid = problem.isConfigValid(q1proj) -assert valid[0] -result, q2proj, err = problem.applyConstraints(q2) -assert result -assert problem.isConfigValid(q2proj) - -problem.initConfig(q1proj) -problem.addGoalConfig(q2proj) - -planner = DiffusingPlanner(problem) -planner.maxIterations(5000) - -# Run benchmark -totalTime = dt.timedelta(0) -totalNumberNodes = 0 -success = 0 - -for i in range(args.N): - try: - planner.roadmap().clear() - problem.resetGoalConfigs() - problem.initConfig(q1proj) - problem.addGoalConfig(q2proj) - 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-spf.py b/tests/benchmarks/romeo-placard-spf.py deleted file mode 100644 index 100535f..0000000 --- a/tests/benchmarks/romeo-placard-spf.py +++ /dev/null @@ -1,305 +0,0 @@ -#!/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/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py deleted file mode 100644 index 4807938..0000000 --- a/tests/benchmarks/test_benchmarks.py +++ /dev/null @@ -1,18 +0,0 @@ -#!/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/constraint_graph.py b/tests/constraint_graph.py deleted file mode 100644 index d9d1b09..0000000 --- a/tests/constraint_graph.py +++ /dev/null @@ -1,164 +0,0 @@ -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.core import ConfigurationShooter # noqa: F401 -import numpy as np -from pinocchio import SE3, StdVec_Bool as Mask, Quaternion - - -from pyhpp.constraints import ( - RelativeTransformation, - Transformation, - ComparisonTypes, - ComparisonType, - BySubstitution, - Implicit, -) - -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur5_gripper.srdf" -) - -urdfFilenameBall = "package://hpp_environments/urdf/ur_benchmark/pokeball.urdf" -srdfFilenameBall = "package://hpp_environments/srdf/ur_benchmark/pokeball.srdf" - -r0_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -r1_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) - -robot = Device("bot") - -urdf.loadModel(robot, 0, "ur5", "anchor", urdfFilename, srdfFilename, r0_pose) -urdf.loadModel( - robot, 0, "pokeball", "freeflyer", urdfFilenameBall, srdfFilenameBall, r1_pose -) - - -robot.setJointBounds( - "pokeball/root_joint", - [ - -0.4, - 0.4, - -0.4, - 0.4, - -0.1, - 1.0, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - ], -) - -problem = Problem(robot) - -graph = Graph("graph", robot, problem) - -# Create states -state_placement = graph.createState("placement", False, 0) -state_grasp_placement = graph.createState("grasp-placement", False, 0) -state_gripper_above_ball = graph.createState("gripper-above-ball", False, 0) -state_ball_above_ground = graph.createState("ball-above-ground", False, 0) -state_grasp = graph.createState("grasp", False, 0) - -# Create transitions -transitions_transit = graph.createTransition( - state_placement, state_placement, "transit", 1, state_placement -) - -transitions_approach_ball = graph.createTransition( - state_placement, state_gripper_above_ball, "approach-ball", 1, state_placement -) -transitions_move_gripper_away = graph.createTransition( - state_gripper_above_ball, state_placement, "move-gripper-away", 1, state_placement -) -transitions_grasp_ball = graph.createTransition( - state_gripper_above_ball, state_grasp_placement, "grasp-ball", 1, state_placement -) -transitions_move_gripper_up = graph.createTransition( - state_grasp_placement, - state_gripper_above_ball, - "move-gripper-up", - 1, - state_placement, -) -transitions_take_ball_up = graph.createTransition( - state_grasp_placement, state_ball_above_ground, "take-ball-up", 1, state_grasp -) -transitions_put_ball_down = graph.createTransition( - state_ball_above_ground, state_grasp_placement, "put-ball-down", 1, state_grasp -) -transitions_take_ball_away = graph.createTransition( - state_ball_above_ground, state_grasp, "take-ball-away", 1, state_grasp -) -transitions_approach_ground = graph.createTransition( - state_grasp, state_ball_above_ground, "approach-ground", 1, state_grasp -) -transitions_transfer = graph.createTransition( - state_grasp, state_grasp, "transfer", 1, state_grasp -) - -joint2 = robot.model().getJointId("pokeball/root_joint") -joint1 = robot.model().getJointId("ur5/wrist_3_joint") -Id = SE3.Identity() - -m = [ - False, - False, - True, - True, - True, - False, -] -q = Quaternion(0, 0, 0, 1) -ballGround = SE3(q, np.array([0, 0, 0.15])) -pc = Transformation("placementConstraint", robot, joint2, Id, ballGround, m) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, -) -implicit_mask = [True, True, True] -implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) - - -q = Quaternion(0.5, 0.5, -0.5, 0.5) -ballInGripper = SE3(q, np.array([0, 0.137, 0])) -m = Mask() -m[:] = (True,) * 6 -pc = RelativeTransformation("grasp", robot, joint1, joint2, ballInGripper, Id, m) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, -) -implicitGraspConstraint = Implicit(pc, cts, m) - - -# Set constraints of nodes and edges -graph.addNumericalConstraint(state_placement, implicitPlacementConstraint) -graph.addNumericalConstraint(state_grasp, implicitGraspConstraint) - -graph.maxIterations(100) -graph.errorThreshold(0.00001) - -graph.initialize() - -configurationShooter = problem.configurationShooter() -solver = BySubstitution(robot.configSpace()) - -for i in range(100): - q = configurationShooter.shoot() - res, config, err = graph.applyStateConstraints(state_placement, q) - if res: - print("after applying constraints: ", config) - break diff --git a/tests/construction_set.py b/tests/construction_set.py deleted file mode 100644 index 3546304..0000000 --- a/tests/construction_set.py +++ /dev/null @@ -1,96 +0,0 @@ -from math import pi -import numpy as np -from pinocchio import SE3 -from pyhpp.manipulation import Device, urdf - -# Load two UR3 robots -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" -) - -r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) -r1_pose = SE3(rotation=np.identity(3), translation=np.array([0.25, 0, 0])) -robot = Device("construction-set") -urdf.loadModel(robot, 0, "r0", "anchor", urdfFilename, srdfFilename, r0_pose) -urdf.loadModel(robot, 0, "r1", "anchor", urdfFilename, srdfFilename, r1_pose) - -# Load environment -urdfFilename = "package://hpp_environments/urdf/construction_set/ground.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/ground.srdf" -urdf.loadModel(robot, 0, "ground", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - -# Load spheres -nSphere = 2 -nCylinder = 2 - -objects = list() -urdfFilename = "package://hpp_environments/urdf/construction_set/sphere.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/sphere.srdf" -for i in range(nSphere): - urdf.loadModel( - robot, 0, f"sphere{i}", "freeflyer", urdfFilename, srdfFilename, SE3.Identity() - ) - objects.append(f"sphere{i}") - -# Load cylinders -urdfFilename = "package://hpp_environments/urdf/construction_set/cylinder_08.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/cylinder_08.srdf" -for i in range(nCylinder): - urdf.loadModel( - robot, - 0, - f"cylinder{i}", - "freeflyer", - urdfFilename, - srdfFilename, - SE3.Identity(), - ) - objects.append(f"cylinder{i}") - -# Set joint bounds -# Set robot joint bounds -jointBounds = { - "r0/shoulder_pan_joint": [-pi, 4], - "r1/shoulder_pan_joint": [-pi, 4], - "r0/shoulder_lift_joint": [-pi, 0], - "r1/shoulder_lift_joint": [-pi, 0], - "r0/elbow_joint": [-2.6, 2.6], - "r1/elbow_joint": [-2.6, 2.6], -} -m = robot.model() -lowerLimit = m.lowerPositionLimit -upperLimit = m.upperPositionLimit -for jn, [lower, upper] in jointBounds.items(): - lowerLimit[m.getJointId(jn)] = lower - upperLimit[m.getJointId(jn)] = upper - -for i in range(nSphere): - ij = m.getJointId(f"sphere{i}/root_joint") - iq = m.idx_qs[ij] - nq = m.nqs[ij] - lowerLimit[iq : iq + nq] = [-1.0, -1.0, -0.1, -1.0001, -1.0001, -1.0001, -1.0001] - upperLimit[iq : iq + nq] = [1.0, 1.0, 0.1, 1.0001, 1.0001, 1.0001, 1.0001] - -for i in range(nCylinder): - ij = m.getJointId(f"cylinder{i}/root_joint") - iq = m.idx_qs[ij] - nq = m.nqs[ij] - lowerLimit[iq : iq + nq] = [-1.0, -1.0, -0.1, -1.0001, -1.0001, -1.0001, -1.0001] - upperLimit[iq : iq + nq] = [1.0, 1.0, 0.1, 1.0001, 1.0001, 1.0001, 1.0001] - -m.lowerPositionLimit = lowerLimit -m.upperPositionLimit = upperLimit - -grippers = robot.grippers() -grippers["r0/gripper"].localPosition - -handles = robot.handles() -handles["sphere0/magnet"].localPosition - -h = handles["sphere1/magnet"] -g = grippers["cylinder0/magnet1"] - -c = h.createGrasp(g, "sphere1/magnet grasps cylinder0/magnet1") diff --git a/tests/corbaserver-client.py b/tests/corbaserver-client.py deleted file mode 100644 index c9ad45d..0000000 --- a/tests/corbaserver-client.py +++ /dev/null @@ -1,4 +0,0 @@ -from hpp.corbaserver import Client - -cl = Client() -cl._tools.shutdown() diff --git a/tests/corbaserver.py b/tests/corbaserver.py deleted file mode 100644 index a6190d1..0000000 --- a/tests/corbaserver.py +++ /dev/null @@ -1,12 +0,0 @@ -from pyhpp.core import ProblemSolver -from pyhpp.corbaserver import Server - -ps = ProblemSolver.create() - -for i in range(3): - print(i) - server = Server(ps, False) - server.initialize() - server.startCorbaServer() - server.processRequest(True) - del server diff --git a/tests/differentiable-function.py b/tests/differentiable-function.py deleted file mode 100644 index f76c53a..0000000 --- a/tests/differentiable-function.py +++ /dev/null @@ -1,35 +0,0 @@ -from pyhpp.pinocchio import LiegroupElement - -from pyhpp.constraints import DifferentiableFunction -import numpy as np - - -class Function(DifferentiableFunction): - def __init__(self): - super(Function, self).__init__(2, 2, 2, "Test") - - def impl_compute(self, res, arg): - res.v = 2 * arg - - def impl_jacobian(self, res, arg): - res = 2 * np.eye(2) - return res - - -function = Function() -print(function) - -q = np.ones((function.ni, 1)) - -# C++ API -v = LiegroupElement(function.outputSpace()) -function.value(v, q) -print(v) - -J = np.zeros((function.ndo, function.ndi)) -function.jacobian(J, q) -print(J) - -# Pythonic API -v = function(q) -J = function.J(q) diff --git a/tests/graph_factory.py b/tests/graph_factory.py deleted file mode 100644 index c580b35..0000000 --- a/tests/graph_factory.py +++ /dev/null @@ -1,101 +0,0 @@ -from math import sqrt -import numpy as np - -from pyhpp.manipulation.constraint_graph_factory import Rule, ConstraintGraphFactory -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.constraints import LockedJoint -from pinocchio import SE3 - -# Robot and environment file paths -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") - -# Load PR2 robot -pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "pr2", "freeflyer", pr2_urdf, pr2_srdf, pr2_pose) - -# Load box to be manipulated -box_pose = SE3(rotation=np.identity(3), translation=np.array([-2.5, -4, 0.746])) -urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) - -# Load kitchen environment -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 -) - -model = robot.model() - -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]) - -# Create problem -problem = Problem(robot) -print(model.names) - -# Generate initial and goal configuration. -rankInConfiguration = dict() -current_rank = 0 -for joint_id in range(1, model.njoints): - joint_name = model.names[joint_id] - joint = model.joints[joint_id] - rankInConfiguration[joint_name[0:]] = current_rank - current_rank += joint.nq - - -q_init = robot.currentConfiguration() -rank = rankInConfiguration["pr2/l_gripper_l_finger_joint"] -q_init[rank] = 0.5 -rank = rankInConfiguration["pr2/l_gripper_r_finger_joint"] -q_init[rank] = 0.5 -q_init[0:2] = [-3.2, -4] -rank = rankInConfiguration["pr2/torso_lift_joint"] -q_init[rank] = 0.2 -rank = rankInConfiguration["box/root_joint"] -q_init[rank : rank + 3] = [-2.5, -4, 0.746] -q_init[rank + 3 : rank + 7] = [0, -sqrt(2) / 2, 0, sqrt(2) / 2] - -q_goal = q_init[::] -q_goal[0:2] = [-3.2, -4] -rank = rankInConfiguration["box/root_joint"] -q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746] - -# Create constraints -ll = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) -lr = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) - -# Create the constraint graph -grippers = ["pr2/l_gripper"] -objects = ["box"] -handlesPerObject = [["box/handle2"]] -contactSurfacesPerObject = [["box/box_surface"]] -envContactSurfaces = ["kitchen_area/pancake_table_table_top"] -rules = [Rule([".*"], [".*"], True)] - -cg = Graph("graph", robot, problem) -cg.maxIterations(100) -cg.errorThreshold(0.0001) -factory = ConstraintGraphFactory(cg) - -factory.setGrippers(grippers) -factory.environmentContacts(envContactSurfaces) -factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject) -factory.setRules(rules) -factory.generate() - -cg.addNumericalConstraintsToGraph([ll, lr]) - -cg.initialize() - -problem.initConfig(q_init) -problem.addGoalConfig(q_goal) - -print("Script completed!") diff --git a/tests/graph_factory2.py b/tests/graph_factory2.py deleted file mode 100644 index 26f1cbe..0000000 --- a/tests/graph_factory2.py +++ /dev/null @@ -1,234 +0,0 @@ -from math import pi -import numpy as np - -from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.constraints import ( - Transformation, - ComparisonTypes, - ComparisonType, - Implicit, - LockedJoint, -) -from pinocchio import SE3, Quaternion - -# based on /hpp_benchmark/2025/04-01/ur3-spheres/script.py - -# Robot and environment file paths -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" -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" - -robot = Device("ur3-spheres") - -# Load UR3 robot -ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "ur3", "anchor", ur3_urdf, ur3_srdf, ur3_pose) - -# Load sphere to be manipulated -objects = list() -nSphere = 2 -sphere_pose = SE3(rotation=np.identity(3), translation=np.array([-2.5, -4, 0.746])) -for i in range(nSphere): - urdf.loadModel( - robot, - 0, - "sphere{0}".format(i), - "freeflyer", - sphere_urdf, - sphere_srdf, - sphere_pose, - ) - robot.setJointBounds( - "sphere{0}/root_joint".format(i), - [ - -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("sphere{0}".format(i)) - -# Load kitchen environment -kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel( - robot, 0, "kitchen_area", "anchor", ground_urdf, ground_srdf, kitchen_pose -) - -model = robot.model() -robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) -robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) -robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) - -problem = Problem(robot) -cg = Graph("graph", robot, problem) - -constraints = dict() - -for i in range(nSphere): - o = objects[i] - h = robot.handles()[o + "/handle"] - h.mask = [True, True, True, False, True, True] - # placement constraint - placementName = "place_sphere{0}".format(i) - Id = SE3.Identity() - q = Quaternion(0, 0, 0, 1) - ballPlacement = SE3(q, np.array([0, 0, 0.02])) - joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation( - placementName, - robot, - joint, - ballPlacement, - Id, - [True, True, False, False, False, True], - ) - cts = ComparisonTypes() - cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ) - implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) - constraints[placementName] = implicitPlacementConstraint - # placement complement constraint - pc = Transformation( - placementName + "/complement", - robot, - joint, - ballPlacement, - Id, - [False, False, True, True, True, False], - ) - cts[:] = ( - ComparisonType.Equality, - ComparisonType.Equality, - ComparisonType.Equality, - ) - implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) - constraints[placementName + "/complement"] = implicitPlacementComplementConstraint - - # combination of placement and complement - cts[:] = ( - ComparisonType.Equality, - ComparisonType.Equality, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.Equality, - ) - ll = LockedJoint( - robot, - "sphere{0}/root_joint".format(i), - np.array([0, 0, 0.02, 0, 0, 0, 1]), - cts, - ) - constraints[placementName + "/hold"] = ll - cg.registerConstraints( - constraints[placementName], - constraints[placementName + "/complement"], - constraints[placementName + "/hold"], - ) - - preplacementName = "preplace_sphere{0}".format(i) - Id = SE3.Identity() - q = Quaternion(0, 0, 0, 1) - ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) - joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation( - preplacementName, - robot, - joint, - ballPrePlacement, - Id, - [False, False, True, True, True, False], - ) - cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ) - implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) - constraints[preplacementName] = implicitPrePlacementConstraint - -q_init = [ - pi / 6, - -pi / 2, - pi / 2, - 0, - 0, - 0, - 0.2, - 0, - 0.02, - 0, - 0, - 0, - 1, - 0.3, - 0, - 0.02, - 0, - 0, - 0, - 1, -] -q_goal = [ - pi / 6, - -pi / 2, - pi / 2, - 0, - 0, - 0, - 0.3, - 0, - 0.02, - 0, - 0, - 0, - 1, - 0.2, - 0, - 0.02, - 0, - 0, - 0, - 1, -] - - -grippers = ["ur3/gripper"] -handlesPerObject = [["sphere{0}/handle".format(i)] for i in range(nSphere)] -contactsPerObject = [[] for i in range(nSphere)] - -cg.maxIterations(100) -cg.errorThreshold(0.0001) -factory = ConstraintGraphFactory(cg, constraints) - -factory.setGrippers(grippers) -factory.setObjects(objects, handlesPerObject, contactsPerObject) -factory.generate() - -cg.initialize() -# cg.display("./temp.dot") -# problem.initConfig(q_init) -# problem.addGoalConfig(q_goal) - -print("Script completed!") diff --git a/tests/imports.py b/tests/imports.py deleted file mode 100644 index e69de29..0000000 diff --git a/tests/integration/benchmark_utils.py b/tests/integration/benchmark_utils.py new file mode 100644 index 0000000..b331d72 --- /dev/null +++ b/tests/integration/benchmark_utils.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Shared utilities for benchmark scripts. + +import datetime as dt +from argparse import ArgumentParser +from dataclasses import dataclass, field +from typing import Callable, Optional, Any + + +def create_benchmark_parser(description: str = "HPP Benchmark") -> ArgumentParser: + """Create a standard argument parser for benchmarks.""" + parser = ArgumentParser(description=description) + parser.add_argument( + "-N", + default=0, + type=int, + help="Number of benchmark iterations to run", + ) + parser.add_argument( + "--display", + action="store_true", + help="Display visualization (if available)", + ) + parser.add_argument( + "--run", + action="store_true", + help="Run path playback after solving", + ) + return parser + + +@dataclass +class BenchmarkResult: + """Results from a single benchmark iteration.""" + success: bool + time_elapsed: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) + num_nodes: int = 0 + path_length: float = 0.0 + error_message: str = "" + + +@dataclass +class BenchmarkStats: + """Aggregated statistics from benchmark runs.""" + total_iterations: int = 0 + num_successes: int = 0 + total_time: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) + total_nodes: int = 0 + total_path_length: float = 0.0 + + @property + def success_rate(self) -> float: + if self.total_iterations == 0: + return 0.0 + return self.num_successes / self.total_iterations * 100 + + @property + def avg_time_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_time.total_seconds() / self.num_successes + + @property + def avg_nodes_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_nodes / self.num_successes + + @property + def avg_path_length_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_path_length / self.num_successes + + +class BenchmarkRunner: + """ + Runs benchmark iterations and collects statistics. + + Usage: + runner = BenchmarkRunner( + solve_func=lambda: planner.solve(), + reset_func=lambda: planner.roadmap().clear(), + get_nodes_func=lambda: len(planner.roadmap().nodes()), + ) + stats = runner.run(num_iterations=10) + runner.print_stats() + """ + + def __init__( + self, + solve_func: Callable[[], Any], + reset_func: Optional[Callable[[], None]] = None, + get_nodes_func: Optional[Callable[[], int]] = None, + get_path_length_func: Optional[Callable[[], float]] = None, + ): + self.solve_func = solve_func + self.reset_func = reset_func + self.get_nodes_func = get_nodes_func + self.get_path_length_func = get_path_length_func + self.stats = BenchmarkStats() + self.results: list = [] + + def _run_single(self) -> BenchmarkResult: + """Run a single benchmark iteration.""" + result = BenchmarkResult(success=False) + + if self.reset_func: + self.reset_func() + + try: + t1 = dt.datetime.now() + path = self.solve_func() + t2 = dt.datetime.now() + + result.success = True + result.time_elapsed = t2 - t1 + + if self.get_nodes_func: + result.num_nodes = self.get_nodes_func() + + if self.get_path_length_func and path is not None: + result.path_length = self.get_path_length_func() + + except Exception as e: + result.error_message = str(e) + print(f"Failed to plan path: {e}") + + return result + + def run(self, num_iterations: int) -> BenchmarkStats: + """Run the benchmark for num_iterations.""" + self.stats = BenchmarkStats(total_iterations=num_iterations) + self.results = [] + + for i in range(num_iterations): + result = self._run_single() + self.results.append(result) + + if result.success: + self.stats.num_successes += 1 + self.stats.total_time += result.time_elapsed + self.stats.total_nodes += result.num_nodes + self.stats.total_path_length += result.path_length + print(result.time_elapsed) + if result.num_nodes > 0: + print(f"Number nodes: {result.num_nodes}") + + return self.stats + + def print_stats(self) -> None: + """Print benchmark statistics.""" + if self.stats.total_iterations == 0: + return + + print("#" * 20) + print(f"Number of rounds: {self.stats.total_iterations}") + print(f"Number of successes: {self.stats.num_successes}") + print(f"Success rate: {self.stats.success_rate}%") + + if self.stats.num_successes > 0: + print(f"Average time per success: {self.stats.avg_time_per_success:.4f}s") + print(f"Average number nodes per success: {self.stats.avg_nodes_per_success:.1f}") + if self.stats.total_path_length > 0: + print(f"Average path length per success: {self.stats.avg_path_length_per_success:.4f}") + + def verify_results(self) -> bool: + """ + Verify that benchmark produced valid results. + Returns True if all assertions pass. + """ + if self.stats.total_iterations == 0: + return True + + success = True + + if self.stats.num_successes > 0: + for result in self.results: + if result.success: + if result.time_elapsed.total_seconds() <= 0: + print("ASSERTION FAILED: time_elapsed should be positive") + success = False + + if self.get_nodes_func and result.num_nodes <= 0: + print("ASSERTION FAILED: num_nodes should be positive") + success = False + + return success + + +def run_benchmark_main( + planner: Any, + problem: Any, + q_init: Any, + q_goal: Any, + num_iterations: int, +) -> BenchmarkStats: + """ + Convenience function to run a standard benchmark. + + Args: + planner: The planner object (must have solve() and roadmap().clear()) + problem: The problem object (must have initConfig(), addGoalConfig(), resetGoalConfigs()) + q_init: Initial configuration + q_goal: Goal configuration + num_iterations: Number of iterations to run + + Returns: + BenchmarkStats with results + """ + def reset(): + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + runner = BenchmarkRunner( + solve_func=planner.solve, + reset_func=reset, + get_nodes_func=lambda: len(planner.roadmap().nodes()), + ) + + stats = runner.run(num_iterations) + runner.print_stats() + + assert runner.verify_results(), "Benchmark verification failed!" + + return stats diff --git a/tests/benchmarks/construction-set-m-rrt.py b/tests/integration/construction-set-m-rrt.py similarity index 54% rename from tests/benchmarks/construction-set-m-rrt.py rename to tests/integration/construction-set-m-rrt.py index ee9b225..789b551 100644 --- a/tests/benchmarks/construction-set-m-rrt.py +++ b/tests/integration/construction-set-m-rrt.py @@ -1,9 +1,7 @@ #!/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 @@ -21,23 +19,19 @@ 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", +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("Construction Set M-RRT Benchmark") +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() @@ -45,22 +39,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) @@ -77,20 +71,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([-0.25, 0, 0])) +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([0.25, 0, 0])) +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]) +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) @@ -98,61 +92,23 @@ 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.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, - ], + 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}") + 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.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, - ], + 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}") + objects.append(f'cylinder{i}') model = robot.model() @@ -171,68 +127,42 @@ 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]) @@ -242,78 +172,52 @@ 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] @@ -323,13 +227,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 g not in _grippers: + if not g in _grippers: _grippers.append(g) - _handles += (len(_grippers) - len(_handles)) * ["^$"] + _handles += (len(_grippers) - len(_handles)) * ['^$'] return Rule(grippers=_grippers, handles=_handles, link=True) @@ -338,26 +242,23 @@ 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([-0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) + q0_spheres.append([-.1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -365,7 +266,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 + 0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) + q0_cylinders.append([0.45 + .1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -376,176 +277,121 @@ 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) problem.constraintGraph(cg) +if args.display: + from pyhpp_plot import show_graph + show_graph(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}") +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q0, + q_goal=q_goal, + num_iterations=args.N, + ) \ No newline at end of file diff --git a/tests/integration/pr2-in-iai-kitchen.py b/tests/integration/pr2-in-iai-kitchen.py new file mode 100644 index 0000000..b4508b8 --- /dev/null +++ b/tests/integration/pr2-in-iai-kitchen.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python + +import numpy as np + +from pyhpp.core import Problem, DiffusingPlanner, Dichotomy, Straight +from pyhpp.pinocchio import Device, urdf +from pinocchio import SE3 + +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("PR2 Navigation in IAI Kitchen Benchmark") +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) + +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=planner, + problem=problem, + q_init=q_init, + q_goal=q_goal, + num_iterations=args.N, + ) diff --git a/tests/benchmarks/romeo-placard.py b/tests/integration/romeo-placard.py similarity index 50% rename from tests/benchmarks/romeo-placard.py rename to tests/integration/romeo-placard.py index b5bcfcb..9e18d5c 100644 --- a/tests/benchmarks/romeo-placard.py +++ b/tests/integration/romeo-placard.py @@ -1,53 +1,40 @@ #!/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.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") +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("Romeo Placard Humanoid Manipulation Benchmark") 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.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] -) +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 -) +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] -) +robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) model = robot.model() @@ -97,7 +84,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]) @@ -110,7 +97,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]) @@ -124,13 +111,13 @@ # 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] +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" +leftAnkle = 'romeo/LAnkleRoll' +rightAnkle = 'romeo/RAnkleRoll' factory = StaticStabilityConstraintsFactory(problem, robot) balanceConstraintsDict = factory.createStaticStabilityConstraint( @@ -138,9 +125,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] @@ -150,8 +137,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), @@ -161,7 +148,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() @@ -171,83 +158,11 @@ 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: @@ -267,34 +182,15 @@ 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}") + +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q_init_proj, + q_goal=q_goal_proj, + num_iterations=args.N, + ) + + diff --git a/tests/integration/test_benchmarks.py b/tests/integration/test_benchmarks.py new file mode 100644 index 0000000..8122c5b --- /dev/null +++ b/tests/integration/test_benchmarks.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Benchmark test runner - runs all benchmarks with N=1 to verify they work. + +import subprocess +import sys +from pathlib import Path + + +def main(): + folder = Path(__file__).parent + + # List of benchmark files to run (excluding this file and utilities) + excluded = {Path(__file__).name, "benchmark_utils.py", "__init__.py"} + benchmarks = sorted( + script for script in folder.glob("*.py") + if script.name not in excluded + ) + + print(f"Running {len(benchmarks)} benchmarks...") + + failed = [] + for script in benchmarks: + print(f"\n{'='*60}") + print(f"Running: {script.name}") + print("=" * 60) + + result = subprocess.run( + [sys.executable, str(script), "-N", "1"], + cwd=str(folder), + ) + + if result.returncode != 0: + failed.append(script.name) + print(f"FAILED: {script.name} (exit code {result.returncode})") + + print(f"\n{'='*60}") + print("SUMMARY") + print("=" * 60) + print(f"Total benchmarks: {len(benchmarks)}") + print(f"Passed: {len(benchmarks) - len(failed)}") + print(f"Failed: {len(failed)}") + + if failed: + print(f"\nFailed benchmarks: {', '.join(failed)}") + sys.exit(1) + else: + print("\nAll benchmarks passed!") + + +if __name__ == "__main__": + main() diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/integration/ur3-spheres-spf.py similarity index 87% rename from tests/benchmarks/ur3-spheres-spf.py rename to tests/integration/ur3-spheres-spf.py index 07ea8f8..1df2220 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/integration/ur3-spheres-spf.py @@ -7,8 +7,6 @@ from math import pi import numpy as np -import datetime as dt -from argparse import ArgumentParser from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp.manipulation import ( @@ -30,8 +28,9 @@ ) from pinocchio import SE3, Quaternion -parser = ArgumentParser() -parser.add_argument("-N", default=0, type=int) +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("UR3 Spheres StatesPathFinder Benchmark") args = parser.parse_args() # Robot and environment file paths @@ -290,8 +289,11 @@ cg.initialize() -problem.initConfig(np.array(q_init)) -problem.addGoalConfig(np.array(q_goal)) +q_init_arr = np.array(q_init) +q_goal_arr = np.array(q_goal) + +problem.initConfig(q_init_arr) +problem.addGoalConfig(q_goal_arr) problem.constraintGraph(cg) planner = StatesPathFinder(problem) @@ -301,32 +303,12 @@ 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() - 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}") +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=planner, + problem=problem, + q_init=q_init_arr, + q_goal=q_goal_arr, + num_iterations=args.N, + ) diff --git a/tests/benchmarks/ur3-spheres.py b/tests/integration/ur3-spheres.py similarity index 85% rename from tests/benchmarks/ur3-spheres.py rename to tests/integration/ur3-spheres.py index debb270..9efa9ec 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/integration/ur3-spheres.py @@ -1,6 +1,5 @@ from math import pi import numpy as np -import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp.manipulation import ( @@ -22,11 +21,9 @@ ) from pinocchio import SE3, Quaternion -# based on /hpp_benchmark/2025/04-01/ur3-spheres/script.py -from argparse import ArgumentParser +from benchmark_utils import create_benchmark_parser, run_benchmark_main -parser = ArgumentParser() -parser.add_argument("-N", default=0, type=int) +parser = create_benchmark_parser("UR3 Spheres Manipulation Benchmark") args = parser.parse_args() # Robot and environment file paths ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" @@ -269,38 +266,21 @@ cg.initialize() -problem.initConfig(np.array(q_init)) -problem.addGoalConfig(np.array(q_goal)) +q_init_arr = np.array(q_init) +q_goal_arr = np.array(q_goal) + +problem.initConfig(q_init_arr) +problem.addGoalConfig(q_goal_arr) problem.constraintGraph(cg) manipulationPlanner = ManipulationPlanner(problem) manipulationPlanner.maxIterations(5000) -# Run benchmark -# - -totalTime = dt.timedelta(0) -totalNumberNodes = 0 -success = 0 -for i in range(args.N): - try: - manipulationPlanner.roadmap().clear() - 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}") +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q_init_arr, + q_goal=q_goal_arr, + num_iterations=args.N, + ) diff --git a/tests/liegroup.py b/tests/liegroup.py deleted file mode 100644 index 1a0875a..0000000 --- a/tests/liegroup.py +++ /dev/null @@ -1,25 +0,0 @@ -from pyhpp.pinocchio import LiegroupSpace, LiegroupElement -import numpy as np - -space = LiegroupSpace.R2() -space *= LiegroupSpace.R1(False) -assert str(space) == "R^3" - -space2 = space * space -space2.mergeVectorSpaces() -assert str(space2) == "R^6" - -el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) -el2 = LiegroupElement(np.array([1.0, 2.0, 3.0]), space) -print(f"el1\n->{el1}") -print(f"el2\n->{el2}") -v = el2 - el1 -print(f"v = el2 - el1\n->{v.T}") - -el3 = el1 + v -print(f"el3 = el1 + v\n->{el3}") - -# __eq__ for LiegroupElement not defined -assert el2.space() == el3.space() -assert not el2.space() != el3.space() -assert all(el2.v == el3.v) diff --git a/tests/load_ur3.py b/tests/load_ur3.py deleted file mode 100644 index 9cda9a7..0000000 --- a/tests/load_ur3.py +++ /dev/null @@ -1,40 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pinocchio import StdVec_Bool as Mask -from pyhpp.constraints import Position -from pyhpp.pinocchio import Device, LiegroupElement, urdf - -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" -) - -robot = Device("ur3") -urdf.loadModel(robot, 0, "", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - -q = robot.currentConfiguration() -robot.currentConfiguration(q) - - -m = Mask() -m[:] = (True,) * 3 -Id = SE3.Identity() -pc = Position("position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m) -print(pc) - -qa = np.zeros((pc.ni, 1)) - -# C++ API -v = LiegroupElement(pc.outputSpace()) -pc.value(v, qa) -print(f"{v.space().name()}: {v.vector().T}") - -J = np.zeros((pc.ndo, pc.ndi)) -pc.jacobian(J, q) -print(J[:, 0:4]) - -# Pythonic API -v = pc(qa) -J = pc.J(q) diff --git a/tests/motion_planner.py b/tests/motion_planner.py deleted file mode 100644 index 4e88bf0..0000000 --- a/tests/motion_planner.py +++ /dev/null @@ -1,67 +0,0 @@ -class MotionPlanner: - def __init__(self, robot, ps): - self.robot = robot - self.ps = ps - - def solveBiRRT(self, maxIter=float("inf")): - self.ps.prepareSolveStepByStep() - finished = False - - # In the framework of the course, - # we restrict ourselves to 2 connected components. - nbCC = self.ps.numberConnectedComponents() - if nbCC != 2: - raise Exception("There should be 2 connected components.") - - iter = 0 - ps = self.ps - robot = self.robot - while True: - # RRT begin - # Extend - newNodes = list() - newEdges = list() - q_rand = robot.shootRandomConfig() - for i in range(ps.numberConnectedComponents()): - q_near, d = ps.getNearestConfig(q_rand, i) - res, pid, msg = ps.directPath(q_near, q_rand, True) - if res: - q_new = q_rand - else: - q_new = ps.configAtParam(pid, ps.pathLength(pid)) - newNodes.append(q_new) - newEdges.append((q_near, q_new, pid)) - for q in newNodes: - ps.addConfigToRoadmap(q) - for q1, q2, pid in newEdges: - ps.addEdgeToRoadmap(q1, q2, pid, True) - # connect - for q_new in newNodes: - for i in range(ps.numberConnectedComponents()): - q_near, d = ps.getNearestConfig(q_new, i) - # if q_near == q_new, q_new is in this connected component - if q_near != q_new: - res, pid, msg = ps.directPath(q_new, q_near, True) - if res: - ps.addEdgeToRoadmap(q_new, q_near, pid, True) - print("finished") - break - # RRT end - # Check if the problem is solved. - nbCC = self.ps.numberConnectedComponents() - if nbCC == 1: - # Problem solved - finished = True - break - iter = iter + 1 - if iter > maxIter: - break - if finished: - self.ps.finishSolveStepByStep() - return self.ps.numberPaths() - 1 - - def solvePRM(self): - self.ps.prepareSolveStepByStep() - # PRM begin - # PRM end - self.ps.finishSolveStepByStep() diff --git a/tests/non_linear_spline_gradient_based.py b/tests/non_linear_spline_gradient_based.py deleted file mode 100644 index 929b63b..0000000 --- a/tests/non_linear_spline_gradient_based.py +++ /dev/null @@ -1,313 +0,0 @@ -import numpy as np -from pyhpp.constraints import ComparisonType -from pyhpp.core.path import SplineB3 as Spline -from pyhpp.core.path_optimization import LinearConstraint, SplineGradientBasedAbstractB3 - - -class CostFunction: - def __init__(self, robot, order): - self.order = order - self.robot = robot - - def value(self, splines): - return sum(s.squaredNormIntegral(self.order) for s in splines) - - def jacobian(self, splines): - r = splines[0].NbCoeffs * self.robot.numberDof() - J = np.empty((splines.size() * r, 1)) - n = 0 - for s in splines: - s.squaredNormIntegralDerivative(self.order, J[n : n + r, :]) - n += r - return J.T - - def hessian(self, splines): - r = splines[0].NbCoeffs * self.robot.numberDof() - Ic = np.empty((splines[0].NbCoeffs, splines[0].NbCoeffs)) - H = np.zeros((r * splines.size(), r * splines.size())) - - for k, s in enumerate(splines): - s.squaredNormBasisFunctionIntegral(self.order, Ic) - Ic *= 2 - paramSize = s.parameterSize() - shift = k * s.NbCoeffs * paramSize - for i in range(s.NbCoeffs): - ic = shift + i * paramSize - for j in range(s.NbCoeffs): - jc = shift + j * paramSize - H[ic : ic + paramSize, jc : jc + paramSize] = Ic[i, j] * np.eye( - paramSize - ) - return H - - -class NonLinearSplineGradientBasedB3(SplineGradientBasedAbstractB3): - def __init__(self, problem): - super(NonLinearSplineGradientBasedB3, self).__init__(problem) - self.problem = problem - - def optimize(self, path): - print("NonLinearSplineGradientBasedB3::optimize") - checkJointBound = True - - robot = self.problem.robot() - rDof = robot.numberDof() - - # 1 - splines = SplineGradientBasedAbstractB3.Splines() - self.appendEquivalentSpline(path, splines) - if splines.size() == 1: - return self.buildPathVector(splines) - - self.initializePathValidation(splines) - - # 2 - nParameters = len(splines) * Spline.NbCoeffs - # Order 1 -> max continuity: 0 - # Order 3 -> max continuity: 1 - SplineOrder = 3 - MaxContinuityOrder = int((SplineOrder - 1) / 2) - # For the sake of the example, use C^0 continuity, not C^1 - orderContinuity = MaxContinuityOrder - # orderContinuity = 0 - - constraint = LinearConstraint(nParameters * rDof, 0) - solvers = self.SplineOptimizationDatas( - splines.size(), self.SplineOptimizationData(rDof) - ) - - # TODO - # addProblemConstraints (input, splines, constraint, solvers); - self.addContinuityConstraints(splines, orderContinuity, solvers, constraint) - - validations, parameterizations = self.createConstraints(splines) - value, jacobian = self.initValueAndJacobian( - splines, validations, parameterizations - ) - linearizedConstraint = LinearConstraint(jacobian.shape[1], jacobian.shape[0]) - lcReduced = LinearConstraint(0, 0) - # self.computeValue (value, splines, validations, parameterizations) - # self.computeJacobian (jacobian, splines, validations, parameterizations) - # self.computeLinearizedConstraint (linearizedConstraint, value, jacobian, splines, validations, parameterizations) - - # 3 - collision = LinearConstraint(nParameters * rDof, 0) - # collisionFunctions = CollisionFunctions (); - - # 4 - # TODO: Initialize cost function. - cost = CostFunction(robot, 1) - print("Current {cost.value(splines)}") - - # 5 - _feasible = constraint.decompose(True, True) - - self.checkConstraint(splines, constraint, msg="continuity") - - collisionReduced = LinearConstraint(constraint.PK.shape[0], 0) - constraint.reduceConstraint(collision, collisionReduced) - - boundConstraint = LinearConstraint(nParameters * rDof, 0) - if checkJointBound: - self.jointBoundConstraint(splines, boundConstraint) - self.checkConstraint(splines, boundConstraint, msg="bounds", ineq=True) - # if (!this->validateBounds(splines, boundConstraint).empty()) - # throw std::invalid_argument("Input path does not satisfy joint bounds"); - boundConstraintReduced = LinearConstraint(boundConstraint.PK.shape[0], 0) - constraint.reduceConstraint(boundConstraint, boundConstraintReduced, False) - - costLinearized = LinearConstraint(nParameters * rDof, 1) - clReduced = LinearConstraint(0, 1) - constraint.reduceConstraint(costLinearized, clReduced) - - finalLinearProblem = LinearConstraint(0, 1) - ok = False - iter = 0 - while not ok: - v = cost.value(splines) - print("Iter {iter}, cost: {v}") - - # Compute linearized cost - parameters = np.empty((nParameters * rDof, 1)) - self.updateParameters(parameters, splines) - costLinearized.J = cost.jacobian(splines) - costLinearized.v = costLinearized.J.dot(parameters) - np.matrix([[v]]) - constraint.reduceConstraint(costLinearized, clReduced) - - # Compute decomposition. - self.computeLinearizedConstraint( - linearizedConstraint, - value, - jacobian, - splines, - validations, - parameterizations, - ) - print("Linearized constraints") - constraint.reduceConstraint(linearizedConstraint, lcReduced, True) - print("reduction") - lcReduced.decompose(True, True) - print("decompose") - - lcReduced.reduceConstraint(clReduced, finalLinearProblem, False) - print("reduction") - finalLinearProblem.decompose(True, True) # TODO need only xStar, not PK - - # Compute descent direction. - lcReduced.computeSolution(finalLinearProblem.xStar) - print("computeSolution") - constraint.computeSolution(lcReduced.xSol) - - self.integrate(splines, constraint.xSol) - - iter += 1 - if iter == 5: - ok = True - - return self.buildPathVector(splines) - - def integrate(self, splines, dir): - alpha = 0.5 - row = 0 - for s in splines: - n = s.parameterSize() * s.NbCoeffs - s.parameterIntegrate(alpha * dir[row : row + n]) - row += n - - def createConstraints(self, splines): - # Functions of the form f(v_k) == 0 - validations = [[] for s in splines] - # Functions of the form g(v_k) == g(v_{k+1}) - parameterizations = [[] for s in splines] - for k, s in enumerate(splines): - # Extend function to - c = s.constraints() - if c: - cp = c.configProjector() - if cp: - ncs = cp.numericalConstraints() - ljs = cp.lockedJoints() - for nc in ncs: - if ComparisonType.Equality in nc.comparisonType(): - parameterizations[k].append(nc) - else: - validations[k].append(nc) - for nc in ljs: - if ComparisonType.Equality in nc.comparisonType(): - parameterizations[k].append(nc) - else: - validations[k].append(nc) - else: - print(f"{k} no config projector") - else: - print(f"{k} no constraints") - return validations, parameterizations - - def initValueAndJacobian(self, splines, validations, parameterizations): - vRows = 0 - JRows = JCols = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - for f in v: - vRows += f.function().outputSize() * 2 - JRows += f.function().outputDerivativeSize() * 2 - for f in p: - vRows += f.function().outputDerivativeSize() - JRows += f.function().outputDerivativeSize() - - JCols += s.parameterSize() * s.NbCoeffs - - return np.zeros((vRows, 1)), np.zeros((JRows, JCols)) - - def computeValue(self, value, splines, validations, parameterizations): - i = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - qinit = s.initial() - qend = s.end() - - for f in v: - e = i + f.function().outputSize() - value[i:e] = f.function()(qinit).vector() - i = e - - e = i + f.function().outputSize() - value[i:e] = f.function()(qend).vector() - i = e - - for f in p: - e = i + f.function().outputDerivativeSize() - value[i:e] = f.function()(qinit) - f.function()(qend) - i = e - - def computeJacobian(self, J, splines, validations, parameterizations): - ir = ic = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - qinit = s.initial() - qend = s.end() - - ps = s.parameterSize() - _ec = ic + s.NbCoeffs * ps - ics = [ic + i * ps for i in range(s.NbCoeffs + 1)] - - # Only the endpoints and not the other points ? - for f in v: - er = ir + f.function().outputDerivativeSize() - Jfunction = f.function().J(qinit) - paramDerInit = s.parameterDerivativeCoefficients(s.paramRange().first) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = Jfunction * paramDerInit[c, 0] - ir = er - - er = ir + f.function().outputDerivativeSize() - Jfunction = f.function().J(qend) - paramDerEnd = s.parameterDerivativeCoefficients(s.paramRange().second) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = Jfunction * paramDerEnd[c, 0] - ir = er - - for f in p: - er = ir + f.function().outputDerivativeSize() - JfunctionInit = f.function().J(qinit) - JfunctionEnd = f.function().J(qend) - paramDerInit = s.parameterDerivativeCoefficients(s.paramRange().first) - paramDerEnd = s.parameterDerivativeCoefficients(s.paramRange().second) - # J[ir:er, ics[0]:ics[1]] = f.function().J(s.end()) * s.parameterDerivativeCoefficients(s.paramRange().second) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = ( - JfunctionInit * paramDerInit[c, 0] - - JfunctionEnd * paramDerEnd[c, 0] - ) - ir = er - - return J - - def computeLinearizedConstraint( - self, constraint, value, J, splines, validations, parameterizations - ): - self.computeValue(value, splines, validations, parameterizations) - self.computeJacobian(J, splines, validations, parameterizations) - - s = splines[0] - parameters = np.empty((splines.size() * s.parameterSize() * s.NbCoeffs, 1)) - self.updateParameters(parameters, splines) - - constraint.b = J.dot(parameters) - value - constraint.J = J - constraint.decompose(False, False) - - def checkConstraint(self, splines, constraints, ineq=False, msg=""): - x = np.empty((0)) - for s in splines: - x = np.concatenate((x, s.rowParameters())) - res = constraints.J.dot(x) - constraints.b - if ineq: - ok = all(res >= 0) - else: - ok = np.linalg.norm(res) < 1e-3 - if not ok: - print(f"Constraint {msg} not satisfied: {res.T}") - return ok, res - - @staticmethod - def create(problem): - print("NonLinearSplineGradientBasedB3.create called!") - return NonLinearSplineGradientBasedB3(problem) diff --git a/tests/path-optimizer.py b/tests/path-optimizer.py deleted file mode 100644 index 58cca6f..0000000 --- a/tests/path-optimizer.py +++ /dev/null @@ -1,60 +0,0 @@ -from pyhpp.pinocchio import urdf -from pyhpp.core import ProblemSolver -import pyhpp.core.path -import numpy as np - -from non_linear_spline_gradient_based import NonLinearSplineGradientBasedB3 - -ps = ProblemSolver.create() -robot = ps.createRobot("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) -ps.robot(robot) - -ps.pathOptimizers.add( - "NonLinearSplineGradientBasedB3", NonLinearSplineGradientBasedB3.create -) -ps.addPathOptimizer("NonLinearSplineGradientBasedB3") -ps.createPathOptimizers() -ps.clearPathOptimizers() - -qinit = np.zeros((robot.model().nq, 1)) -qgoal = qinit.copy() -qgoal[0, 0] = 1 - -ps.initConfig(qinit) -ps.addGoalConfig(qgoal) -ps.solve() - -paths = ps.paths() -path = paths[0] - -problem = ps.problem() -po = NonLinearSplineGradientBasedB3(problem) -optPath = po.optimize(path) - -steer = problem.steeringMethod() -shooter = problem.configurationShooter() -for _ in range(10): - qs = shooter.shoot() # start - qi = shooter.shoot() # intermediate - qe = shooter.shoot() # end - p1 = steer(qs, qi) - p2 = steer(qi, qe) - # valid = problem.pathValidation().validate (path, False, validPart, report) - valid1, validPart, report = problem.pathValidation().validate(p1, False) - valid2, validPart, report = problem.pathValidation().validate(p2, False) - if not valid1 or not valid2: - pathVector = pyhpp.core.path.Vector.create( - robot.configSize(), robot.numberDof() - ) - pathVector.appendPath(p1) - pathVector.appendPath(p2) - print(pathVector.numberPaths()) - po.optimize(pathVector) diff --git a/tests/path-planner.py b/tests/path-planner.py deleted file mode 100644 index 287f18a..0000000 --- a/tests/path-planner.py +++ /dev/null @@ -1,102 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pyhpp.pinocchio import Device, urdf -from pyhpp.core import ( - Problem, - DiffusingPlanner, - BiRRTPlanner, - VisibilityPrmPlanner, - BiRrtStar, - kPrmStar, -) - -urdfFilename = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -srdfFilename = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" -rootJointType = "planar" - -# Initialize robot and viewer -robot = Device("ur2") -urdf.loadModel( - robot, 0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity() -) - -# Define initial and goal configurations -q_init = np.array(robot.currentConfiguration()) -q_goal = np.array(q_init[::].copy()) -model = robot.model() - -# Set root_joint bounds only -lowerLimit = model.lowerPositionLimit -upperLimit = model.upperPositionLimit - -# Set root_joint bounds specifically -root_joint_bounds = [-4, -3, -5, -3] # [x_lower, x_upper, y_lower, y_upper] -ij = model.getJointId("r0/root_joint") -iq = model.idx_qs[ij] - -# Apply bounds (assuming first 2 DOF are x,y position) -lowerLimit[iq] = -4 -upperLimit[iq] = -3 -lowerLimit[iq + 1] = -5 -upperLimit[iq + 1] = -3 - -rankInConfiguration = dict() -current_rank = 0 -for joint_id in range(1, model.njoints): - joint_name = model.names[joint_id] - joint = model.joints[joint_id] - rankInConfiguration[joint_name[3:]] = current_rank - - current_rank += joint.nq - -q_init[0:2] = [-3.2, -4] - -rank = rankInConfiguration["torso_lift_joint"] -q_init[rank] = 0.2 -q_goal[0:2] = [-3.2, -4] -rank = rankInConfiguration["l_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["l_elbow_flex_joint"] -q_goal[rank] = -0.5 -rank = rankInConfiguration["r_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["r_elbow_flex_joint"] -q_goal[rank] = -0.5 - -# urdf.loadModel(robot, 0, "kitchen", "anchor", "package://hpp_environments/urdf/kitchen_area_obstacle.urdf", "", SE3.Identity()) - -problem = Problem(robot) -problem.initConfig(q_init) -problem.addGoalConfig(q_goal) - -diffusingPlanner = DiffusingPlanner(problem) -biRRTPlanner = BiRRTPlanner(problem) -visibilityPrmPlanner = VisibilityPrmPlanner(problem) -biRrtStar = BiRrtStar(problem) -kPrmStar_inst = kPrmStar(problem) - -print(diffusingPlanner.maxIterations()) - -path = biRRTPlanner.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = diffusingPlanner.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = visibilityPrmPlanner.solve() infinite search -# viewer.displayPath(path, 0.001, 50) - -# path = biRrtStar.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = kPrmStar_inst.solve() infinite search -# viewer.displayPath(path, 0.001, 50) - -# roadmap = Roadmap(problem.distance(), robot) -# roadmap.initNode(q_init) -# roadmap.addGoalNode(q_goal) -# searchInRoadmap = SearchInRoadmap(problem, roadmap) -# searchInRoadmap.solve() SearchInRoadmap: no goal configuration in the connected componentof initial configuration. A* fails - -# planAndOptimize = PlanAndOptimize(diffusingPlanner) -# viewer.displayPath(planAndOptimize.solve(), 0.001, 50) diff --git a/tests/path.py b/tests/path.py deleted file mode 100644 index c5b465d..0000000 --- a/tests/path.py +++ /dev/null @@ -1,118 +0,0 @@ -import unittest - -import numpy as np -import pyhpp.core - - -class PythonStraightPath(pyhpp.core.PathWrap): - def __init__(self, a: float, b: float, v: float): - duration = (b - a) / v - super().__init__(pyhpp.core.interval(0, duration), 1, 1) - self.a = a - self.b = b - self.v = v - self.initPtr(self) - self._counters = {} - - def _inc_counter(self, key): - self._counters[key] = self._counters.get(key, 0) + 1 - - def copy(self): - self._inc_counter("copy") - p = PythonStraightPath(self.a, self.b, self.v) - return p - - def initial(self): - self._inc_counter("initial") - return self.a - - def end(self): - self._inc_counter("end") - return self.b - - def impl_compute(self, param): - self._inc_counter("impl_compute") - return ( - np.array( - [ - self.a + param * self.v, - ] - ), - True, - ) - - def impl_derivative(self, param, order): - self._inc_counter("impl_derivative") - if order == 1: - return np.array( - [ - self.v, - ] - ) - else: - return np.array( - [ - 0.0, - ] - ) - - def clear_counter(self, key=None): - if key is None: - self._counters.clear() - else: - self._counters["key"] = 0 - - def assert_called(self, key): - assert self._counters.get(key, 0) > 0 - - -class TestPath(unittest.TestCase): - def test_path_api(self): - space = pyhpp.pinocchio.LiegroupSpace.R1(False) - path = pyhpp.core.StraightPath.create( - space, - np.array((0.0,)), - np.array((1.0,)), - pyhpp.core.interval(1.0, 2.0), - None, - ) - self.assertEqual(path.initial(), 0.0) - self.assertEqual(path.end(), 1.0) - self.assertEqual(path.length(), 1.0) - - self.assertEqual(path.eval(1.5), (0.5, True)) - self.assertEqual(path.derivative(1.5, 1), 1.0) - self.assertEqual(path.derivative(1.5, 2), 0.0) - - extracted_path = path.extract(1.5, 2.0) - self.assertTrue(isinstance(extracted_path, pyhpp.core.StraightPath)) - self.assertEqual(extracted_path.length(), 0.5) - - def test_path_inheritance(self): - path = PythonStraightPath(2.0, 4.0, 1.0) - - # path.initial() - res, ok = path.eval(1.0) - self.assertTrue(ok) - self.assertEqual(res, 3.0) - path.assert_called("impl_compute") - - res = path.derivative(1.0, 1) - self.assertEqual(res, 1.0) - path.assert_called("impl_derivative") - - copy = path.copy() - self.assertEqual(copy.initial(), path.initial()) - self.assertEqual(copy.end(), path.end()) - - # Check that it can be used as a path - pv = pyhpp.core.path.Vector.create(1, 1) - pv.appendPath(path) - - path.clear_counter("copy") - _path_0 = pv.pathAtRank(0) - path.assert_called("copy") - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/problem-solver.py b/tests/problem-solver.py deleted file mode 100644 index 6bf6171..0000000 --- a/tests/problem-solver.py +++ /dev/null @@ -1,35 +0,0 @@ -from pyhpp.pinocchio import urdf -from pyhpp.core import ProblemSolver -import numpy as np - -ps = ProblemSolver.create() -robot = ps.createRobot("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) -ps.robot(robot) - -qinit = np.zeros((robot.model().nq, 1)) -qgoal = qinit.copy() -qgoal[0, 0] = 1 - -ps.initConfig(qinit) -ps.addGoalConfig(qgoal) -ps.solve() - -paths = ps.paths() -path = paths[0] - -assert (path.initial() == qinit).all() - -q, success = path(0.3) -assert success - -qq = np.zeros_like(qinit) -success = path(qq, path.length()) -assert success diff --git a/tests/rrt.py b/tests/rrt.py deleted file mode 100644 index 081b766..0000000 --- a/tests/rrt.py +++ /dev/null @@ -1,100 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pyhpp.pinocchio import Device, urdf -from pyhpp.core import Problem, Roadmap, WeighedDistance - -# Robot configuration -urdfFilename = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" -srdfFilename = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" - -# Initialize robot and viewer -robot = Device("ur5") - -# Add robot and obstacles to scene -urdf.loadModel(robot, 0, "r0", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - - -# urdf.loadModel(robot, 0, "table", "anchor", "package://hpp_environments/urdf/ur_benchmark/table.urdf", "", SE3.Identity()) -# urdf.loadModel(robot, 0, "wall", "anchor", "package://hpp_environments/urdf/ur_benchmark/wall.urdf", "", SE3.Identity()) -# urdf.loadModel(robot, 0, "obstacles", "anchor", "package://hpp_environments/urdf/ur_benchmark/obstacles.urdf", "", SE3.Identity()) - - -# Define initial and goal configurations -qInit = np.array([0.2, -1.57, -1.8, 0, 0.8, 0]) -qGoal = np.array([1.57, -1.57, -1.8, 0, 0.8, 0]) - -# Setup problem and RRT components -problem = Problem(robot) -configurationShooter = problem.configurationShooter() -steer = problem.steeringMethod() -weighedDistance = WeighedDistance(robot) - -# Initialize roadmap -roadmap = Roadmap(weighedDistance, robot) -roadmap.initNode(qInit) -roadmap.addGoalNode(qGoal) - -# RRT algorithm parameters -finished = False -iter = 0 -maxIter = 1000 - -# Main RRT loop -while not finished and iter < maxIter: - # Extend phase - newNodes = [] - newEdges = [] - q_rand = configurationShooter.shoot() - iter += 1 - - # Try to extend from each connected component - for i in range(roadmap.numberConnectedComponents()): - cc = roadmap.getConnectedComponent(i) - q_near, d = roadmap.nearestNode(q_rand, cc) - directpath = steer(q_near, q_rand) - res, validPart, report = problem.pathValidation().validate(directpath, False) - - if res: - q_new = q_rand - else: - q_new = validPart.end() - - newNodes.append(q_new) - newEdges.append((q_near, q_new, validPart)) - - # Add new nodes to roadmap - for q in newNodes: - roadmap.addNode(q) - - # Add new edges to roadmap - for q1, q2, path in newEdges: - roadmap.addEdge(q1, q2, path) - - # Connect phase - for q_new in newNodes: - for i in range(roadmap.numberConnectedComponents()): - cc = roadmap.getConnectedComponent(i) - q_near, d = roadmap.nearestNode(q_new, cc) - - if (q_near != q_new).all(): - directpath = steer(q_new, q_near) - res, validPart, report = problem.pathValidation().validate( - directpath, False - ) - - if res: - roadmap.addEdge(q_new, q_near, validPart) - break - - # Check if problem is solved - nbCC = roadmap.numberConnectedComponents() - if nbCC == 1: - print("Problem solved!") - finished = True - -# Compute and display final path -if finished: - path = problem.target().computePath(roadmap) - print(path) -else: - print(f"Maximum iterations ({maxIter}) reached without finding solution") diff --git a/tests/solver.py b/tests/solver.py deleted file mode 100644 index a280fb0..0000000 --- a/tests/solver.py +++ /dev/null @@ -1,66 +0,0 @@ -from pyhpp.pinocchio import Device, urdf - -from pyhpp.constraints import ( - Position, - ComparisonTypes, - ComparisonType, - BySubstitution, - segment, - Implicit, - Explicit, -) -from pinocchio import SE3, StdVec_Bool as Mask - -robot = Device("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) - -m = Mask() -m[:] = (True,) * 3 -Id = SE3.Identity() -pc = Position.create( - "position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m -) - -solver = BySubstitution(robot.configSpace()) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.Equality, -) -solver.add(Implicit(pc, cts, [True, True, True]), 0) - -print(solver) - -# This only tests the call to add. -# The inputs are not valid so the solver -# will not be able to solve anything. -solver.explicitConstraintSet().add( - Explicit.create( - robot.configSpace(), - pc, - [ - segment(0, 2), - ], - [ - segment(2, 4), - ], - [ - segment(0, 2), - ], - [ - segment(2, 4), - ], - cts, - ) -) -solver.explicitConstraintSetHasChanged() - -print(solver) diff --git a/tests/test_non_linear_optimization.py b/tests/test_non_linear_optimization.py deleted file mode 100644 index 70d1178..0000000 --- a/tests/test_non_linear_optimization.py +++ /dev/null @@ -1,14 +0,0 @@ -from pyhpp.core import ProblemSolver -from pyhpp.corbaserver import Server - -from non_linear_spline_gradient_based import NonLinearSplineGradientBasedB3 - -ps = ProblemSolver.create() -ps.pathOptimizers.add( - "NonLinearSplineGradientBasedB3", NonLinearSplineGradientBasedB3.create -) - -server = Server(ps, False) -server.startCorbaServer() -server.processRequest(True) -del server diff --git a/tests/unit/__init__.py b/tests/unit/__init__.py new file mode 100644 index 0000000..b9e59ae --- /dev/null +++ b/tests/unit/__init__.py @@ -0,0 +1 @@ +# Unit tests for hpp-python diff --git a/tests/unit/conftest.py b/tests/unit/conftest.py new file mode 100644 index 0000000..293535c --- /dev/null +++ b/tests/unit/conftest.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Test fixtures for hpp-python unit tests. +# Provides factory functions that create test objects. + +import numpy as np +from math import pi +from pinocchio import SE3, Quaternion + +from pyhpp.pinocchio import Device as CoreDevice, urdf as core_urdf +from pyhpp.core import Problem as CoreProblem +from pyhpp.manipulation import Device, urdf, Graph, Problem +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory +from pyhpp.manipulation.security_margins import SecurityMargins +from pyhpp.constraints import ( + Transformation, + ComparisonTypes, + ComparisonType, + Implicit, + LockedJoint, +) + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_problem(): + """Create a UR5 robot and core Problem for non-manipulation tests.""" + robot = CoreDevice("ur5") + core_urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + problem = CoreProblem(robot) + return problem, robot + + +# Robot URDF/SRDF paths +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" +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" + + +def create_ur3_robot(): + """Load a basic UR3 robot without manipulation objects.""" + robot = Device("ur3-test") + ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, ur3_pose) + robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) + robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) + robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) + return robot + + +def _create_placement_constraints(robot, objects): + """Helper to create placement constraints for objects.""" + constraints = {} + model = robot.model() + + for obj in objects: + placement_name = f"place_{obj}" + Id = SE3.Identity() + q = Quaternion(1, 0, 0, 0) + ball_placement = SE3(q, np.array([0, 0, 0.02])) + joint = model.getJointId(f"{obj}/root_joint") + + # Placement constraint + pc = Transformation( + placement_name, + robot, + joint, + Id, + ball_placement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[placement_name] = Implicit(pc, cts, [True, True, True]) + + # Placement complement + pc_comp = Transformation( + placement_name + "/complement", + robot, + joint, + Id, + ball_placement, + [True, True, False, False, False, True], + ) + cts_comp = ComparisonTypes() + cts_comp[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placement_name + "/complement"] = Implicit( + pc_comp, cts_comp, [True, True, True] + ) + + # Hold constraint (LockedJoint) + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, + ) + ll = LockedJoint( + robot, + f"{obj}/root_joint", + np.array([0, 0, 0.02, 0, 0, 0, 1]), + cts_hold, + ) + constraints[placement_name + "/hold"] = ll + + # Pre-placement constraint + preplace_name = f"preplace_{obj}" + ball_preplace = SE3(q, np.array([0, 0, 0.1])) + pc_pre = Transformation( + preplace_name, + robot, + joint, + Id, + ball_preplace, + [False, False, True, True, True, False], + ) + cts_pre = ComparisonTypes() + cts_pre[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[preplace_name] = Implicit(pc_pre, cts_pre, [True, True, True]) + + return constraints + + +def create_ur3_with_spheres(): + """ + Load UR3 with two spheres for manipulation testing. + Returns (robot, objects, constraints) tuple. + """ + robot = Device("ur3-spheres-test") + + # Load UR3 + ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, ur3_pose) + + # Load spheres + n_spheres = 2 + objects = [] + for i in range(n_spheres): + 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}") + + # Load ground + ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, 0, "kitchen_area", "anchor", GROUND_URDF, GROUND_SRDF, ground_pose + ) + + # Set robot joint bounds + robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) + robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) + robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) + + # Create constraints + constraints = _create_placement_constraints(robot, objects) + + return robot, objects, constraints + + +def create_constraint_graph_setup(): + """ + Create a complete constraint graph setup with factory. + Returns (problem, graph, factory, robot, objects) tuple. + """ + robot, objects, constraints = create_ur3_with_spheres() + + problem = Problem(robot) + graph = Graph("test-graph", robot, problem) + graph.maxIterations(40) + graph.errorThreshold(0.0001) + + # Register constraints with graph + for obj in objects: + graph.registerConstraints( + constraints[f"place_{obj}"], + constraints[f"place_{obj}/complement"], + constraints[f"place_{obj}/hold"], + ) + + # Setup handle masks (similar to graph_factory2.py pattern) + handles = robot.handles() + for obj in objects: + handle_name = f"{obj}/handle" + if handle_name in handles: + h = handles[handle_name] + h.mask = [True, True, True, False, True, True] + + # Create factory + factory = ConstraintGraphFactory(graph, constraints) + grippers = ["ur3/gripper"] + handles_per_object = [[f"{obj}/handle"] for obj in objects] + contacts_per_object = [[] for _ in objects] + + factory.setGrippers(grippers) + factory.setObjects(objects, handles_per_object, contacts_per_object) + factory.generate() + + graph.initialize() + + return problem, graph, factory, robot, objects + + +def create_security_margins_instance(): + """ + Create a SecurityMargins instance with basic configuration. + Returns (security_margins, problem, graph, factory, robot, objects) tuple. + """ + problem, graph, factory, robot, objects = create_constraint_graph_setup() + robots_and_objects = ["ur3"] + objects + + sm = SecurityMargins(problem, factory, robots_and_objects, robot) + + return sm, problem, graph, factory, robot, objects diff --git a/tests/unit/test_configuration_shooter.py b/tests/unit/test_configuration_shooter.py new file mode 100644 index 0000000..3513dee --- /dev/null +++ b/tests/unit/test_configuration_shooter.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestConfigurationShooter(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_returns_valid_configuration(self): + shooter = self.problem.configurationShooter() + + q = shooter.shoot() + + self.assertEqual(len(q), self.robot.configSize()) + + +class TestConfigurationShooterEdgeCases(unittest.TestCase): + """Edge case tests for ConfigurationShooter.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_shot_respects_joint_bounds(self): + """Shot configurations should respect joint bounds.""" + shooter = self.problem.configurationShooter() + + for _ in range(10): + q = shooter.shoot() + for i in range(len(q)): + self.assertFalse( + np.isnan(q[i]), + f"Configuration element {i} is NaN" + ) + self.assertFalse( + np.isinf(q[i]), + f"Configuration element {i} is infinite" + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_constraint_factory.py b/tests/unit/test_constraint_factory.py new file mode 100644 index 0000000..b0130f0 --- /dev/null +++ b/tests/unit/test_constraint_factory.py @@ -0,0 +1,151 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from unit.conftest import create_constraint_graph_setup +from pyhpp.manipulation.constraint_graph_factory import ( + Constraints, + ConstraintFactory, +) + + +class TestConstraintFactoryBuildGrasp(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_grasp_returns_dict(self): + result = self.factory.constraints.buildGrasp( + "ur3/gripper", "sphere0/handle" + ) + + self.assertIsInstance(result, dict) + self.assertIn("grasp", result) + self.assertIn("graspComplement", result) + self.assertIn("preGrasp", result) + + def test_build_grasp_constraint_values_are_constraints(self): + result = self.factory.constraints.buildGrasp( + "ur3/gripper", "sphere0/handle" + ) + + self.assertIsInstance(result["grasp"], Constraints) + self.assertIsInstance(result["graspComplement"], Constraints) + self.assertIsInstance(result["preGrasp"], Constraints) + + def test_build_grasp_caches_constraints(self): + cf = self.factory.constraints + + result1 = cf.buildGrasp("ur3/gripper", "sphere1/handle") + result2 = cf.buildGrasp("ur3/gripper", "sphere1/handle") + + self.assertEqual( + result1["grasp"].numConstraints, + result2["grasp"].numConstraints + ) + + +class TestConstraintFactoryBuildPlacement(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_placement_returns_dict(self): + result = self.factory.constraints.buildPlacement("sphere0") + + self.assertIsInstance(result, dict) + self.assertIn("placement", result) + self.assertIn("placementComplement", result) + self.assertIn("prePlacement", result) + + def test_build_placement_constraint_values_are_constraints(self): + result = self.factory.constraints.buildPlacement("sphere0") + + self.assertIsInstance(result["placement"], Constraints) + self.assertIsInstance(result["placementComplement"], Constraints) + self.assertIsInstance(result["prePlacement"], Constraints) + + +class TestConstraintFactoryRegistration(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_constraints_registered_after_build(self): + cf = self.factory.constraints + + cf.buildGrasp("ur3/gripper", "sphere0/handle") + + name = "ur3/gripper grasps sphere0/handle" + self.assertIn(name, cf.available_constraints) + + def test_pregrasp_registered_after_build(self): + cf = self.factory.constraints + + cf.buildGrasp("ur3/gripper", "sphere0/handle") + + name = "ur3/gripper pregrasps sphere0/handle" + self.assertIn(name, cf.available_constraints) + + +class TestConstraintFactoryNegativeCases(unittest.TestCase): + """Negative test cases for ConstraintFactory.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_grasp_invalid_gripper_raises(self): + """Building grasp with non-existent gripper should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildGrasp("nonexistent/gripper", "sphere0/handle") + + def test_build_grasp_invalid_handle_raises(self): + """Building grasp with non-existent handle should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildGrasp("ur3/gripper", "nonexistent/handle") + + def test_build_placement_invalid_object_raises(self): + """Building placement with non-existent object should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildPlacement("nonexistent_object") + + def test_grasp_constraints_not_empty(self): + """Built grasp constraints should not be empty.""" + cf = self.factory.constraints + + result = cf.buildGrasp("ur3/gripper", "sphere0/handle") + + self.assertFalse(result["grasp"].empty()) + + def test_placement_constraints_not_empty(self): + """Built placement constraints should not be empty.""" + cf = self.factory.constraints + + result = cf.buildPlacement("sphere0") + + self.assertFalse(result["placement"].empty()) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_constraint_graph_factory.py b/tests/unit/test_constraint_graph_factory.py new file mode 100644 index 0000000..92ec41c --- /dev/null +++ b/tests/unit/test_constraint_graph_factory.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from pyhpp.manipulation.constraint_graph_factory import ( + Constraints, + PossibleGrasps, + GraspIsAllowed, + Rules, + Rule, +) + +class TestPossibleGrasps(unittest.TestCase): + + def test_allowed_grasp_returns_true(self): + grippers = ["gripper1", "gripper2"] + handles = ["handle1", "handle2", "handle3"] + grasps = { + "gripper1": ["handle1", "handle2"], + "gripper2": ["handle3"] + } + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertTrue(validator((0, 2))) # gripper1->handle1, gripper2->handle3 + self.assertTrue(validator((1, 2))) # gripper1->handle2, gripper2->handle3 + + def test_forbidden_grasp_returns_false(self): + grippers = ["gripper1", "gripper2"] + handles = ["handle1", "handle2", "handle3"] + grasps = { + "gripper1": ["handle1"], + "gripper2": ["handle3"] + } + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertFalse(validator((1, 2))) # gripper1->handle2 not allowed + + def test_none_grasp_allowed(self): + grippers = ["gripper1"] + handles = ["handle1"] + grasps = {"gripper1": ["handle1"]} + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertTrue(validator((None,))) + + +class TestGraspIsAllowed(unittest.TestCase): + + def test_empty_validator_allows_all(self): + validator = GraspIsAllowed() + self.assertTrue(validator((0, 1, 2))) + self.assertTrue(validator((None, None))) + + def test_appended_validator_is_called(self): + validator = GraspIsAllowed() + validator.append(lambda g: g[0] == 0) + + self.assertTrue(validator((0,))) + self.assertFalse(validator((1,))) + + def test_all_validators_must_pass(self): + validator = GraspIsAllowed() + validator.append(lambda g: g[0] == 0) + validator.append(lambda g: len(g) > 1) + + self.assertFalse(validator((0,))) # fails second check + self.assertTrue(validator((0, 1))) + + +class TestRules(unittest.TestCase): + + def test_simple_rule_allows(self): + grippers = ["gripper1"] + handles = ["handle1", "handle2"] + rules = [Rule(grippers=["gripper1"], handles=["handle1"], link=True)] + + validator = Rules(grippers, handles, rules) + + self.assertTrue(validator((0,))) # gripper1 -> handle1 + + def test_simple_rule_forbids(self): + grippers = ["gripper1"] + handles = ["handle1", "handle2"] + rules = [Rule(grippers=["gripper1"], handles=["handle1"], link=False)] + + validator = Rules(grippers, handles, rules) + + self.assertFalse(validator((0,))) # gripper1 -> handle1 forbidden + + def test_regex_pattern_match(self): + """Regex patterns in rules should match gripper/handle names.""" + grippers = ["left_gripper", "right_gripper"] + handles = ["box_handle", "cup_handle"] + # Rule: left_.* can grasp box_.* (regex matching) + rules = [Rule(grippers=["left_.*"], handles=["box_.*"], link=True)] + + validator = Rules(grippers, handles, rules) + + # Tuple format: (handle_idx_for_gripper0, handle_idx_for_gripper1) + # (0, None) = left_gripper grasps box_handle, right_gripper grasps nothing + self.assertTrue(validator((0, None))) + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_device.py b/tests/unit/test_device.py new file mode 100644 index 0000000..efbb15b --- /dev/null +++ b/tests/unit/test_device.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 +from pyhpp.pinocchio import Device, urdf + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_device(): + robot = Device("ur5") + urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + return robot + + +class TestDeviceInstantiation(unittest.TestCase): + + def test_device_creates_with_name(self): + robot = Device("test_robot") + + self.assertEqual(robot.name(), "test_robot") + + def test_device_loads_urdf(self): + robot = create_ur5_device() + + self.assertEqual(robot.name(), "ur5") + self.assertGreater(robot.configSize(), 0) + + +class TestDeviceConfiguration(unittest.TestCase): + + def test_config_size_returns_dof_count(self): + robot = create_ur5_device() + + size = robot.configSize() + + self.assertEqual(size, 6) + + def test_number_dof_returns_velocity_size(self): + robot = create_ur5_device() + + ndof = robot.numberDof() + + self.assertEqual(ndof, 6) + + def test_current_configuration_settable(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = robot.currentConfiguration(q) + + self.assertTrue(result) + + def test_current_configuration_readable(self): + robot = create_ur5_device() + q = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + + current_q = robot.currentConfiguration() + + np.testing.assert_array_almost_equal(current_q, q) + + +class TestDeviceJointBounds(unittest.TestCase): + + def test_set_joint_bounds(self): + robot = create_ur5_device() + + robot.setJointBounds("ur5/shoulder_pan_joint", [-1.0, 1.0]) + + def test_set_joint_bounds_multiple_dof(self): + robot = create_ur5_device() + + robot.setJointBounds("ur5/shoulder_pan_joint", [-2.0, 2.0]) + + def test_set_joint_bounds_invalid_size_raises(self): + robot = create_ur5_device() + + with self.assertRaises(Exception): + robot.setJointBounds("ur5/shoulder_pan_joint", [-1.0]) + + +class TestDeviceForwardKinematics(unittest.TestCase): + + def test_compute_forward_kinematics_joint_position(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + + robot.computeForwardKinematics(1) + + def test_compute_frames_forward_kinematics(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + + robot.computeFramesForwardKinematics() + + def test_get_joint_position_returns_7_elements(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + position = robot.getJointPosition("ur5/ee_fixed_joint") + + self.assertEqual(len(position), 7) + + def test_joint_position_format(self): + """Joint position should be [x, y, z, qx, qy, qz, qw].""" + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + position = robot.getJointPosition("ur5/ee_fixed_joint") + + quat_norm = sum(x**2 for x in position[3:7]) + self.assertAlmostEqual(quat_norm, 1.0, places=5) + + def test_get_joints_position_batch(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + joints = ["ur5/shoulder_link", "ur5/ee_fixed_joint"] + + positions = robot.getJointsPosition(q, joints) + + self.assertEqual(len(positions), 2) + for pos in positions: + self.assertEqual(len(pos), 7) + + +class TestDeviceRankInConfiguration(unittest.TestCase): + + def test_rank_in_configuration_returns_dict(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + self.assertIsInstance(ranks, dict) + + def test_rank_in_configuration_has_joints(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + self.assertIn("ur5/shoulder_pan_joint", ranks) + self.assertIn("ur5/shoulder_lift_joint", ranks) + + def test_rank_values_are_valid_indices(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + for joint_name, rank in ranks.items(): + self.assertGreaterEqual(rank, 0) + self.assertLess(rank, robot.configSize()) + + +class TestDeviceNegativeCases(unittest.TestCase): + + def test_get_joint_position_invalid_frame_raises(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + with self.assertRaises(Exception): + robot.getJointPosition("nonexistent_joint") + + def test_set_joint_bounds_invalid_joint_raises(self): + robot = create_ur5_device() + + with self.assertRaises(Exception): + robot.setJointBounds("nonexistent_joint", [-1.0, 1.0]) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_differentiable_function.py b/tests/unit/test_differentiable_function.py new file mode 100644 index 0000000..c61ac16 --- /dev/null +++ b/tests/unit/test_differentiable_function.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pyhpp.pinocchio import LiegroupElement +from pyhpp.constraints import DifferentiableFunction + + +class DoubleFunction(DifferentiableFunction): + """Simple function that doubles the input.""" + + def __init__(self): + super().__init__(2, 2, 2, "Double") + + def impl_compute(self, res, arg): + res.v = 2 * arg + + def impl_jacobian(self, res, arg): + res = 2 * np.eye(2) + return res + + +class TestDifferentiableFunctionInheritance(unittest.TestCase): + + def test_create_subclass(self): + func = DoubleFunction() + + self.assertIsNotNone(func) + + def test_function_name(self): + func = DoubleFunction() + + self.assertIn("Double", str(func)) + + def test_input_output_sizes(self): + func = DoubleFunction() + + self.assertEqual(func.ni, 2) + self.assertEqual(func.ndo, 2) + self.assertEqual(func.ndi, 2) + + def test_value(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + v = LiegroupElement(func.outputSpace()) + func.value(v, q) + + np.testing.assert_array_almost_equal(v.vector(), [2.0, 2.0]) + + def test_jacobian(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + J = np.zeros((func.ndo, func.ndi)) + func.jacobian(J, q) + + expected = 2 * np.eye(2) + np.testing.assert_array_almost_equal(J, expected) + + def test_call_operator(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + v = func(q) + + self.assertIsNotNone(v) + + def test_jacobian_shorthand(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + J = func.J(q) + + self.assertEqual(J.shape, (func.ndo, func.ndi)) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_handles_grippers.py b/tests/unit/test_handles_grippers.py new file mode 100644 index 0000000..d4de577 --- /dev/null +++ b/tests/unit/test_handles_grippers.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 +from pyhpp.manipulation import Device, urdf + + +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" +SPHERE_URDF = "package://hpp_environments/urdf/construction_set/sphere.urdf" +SPHERE_SRDF = "package://hpp_environments/srdf/construction_set/sphere.srdf" + + +def create_manipulation_device(): + robot = Device("ur3-sphere") + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, SE3.Identity()) + urdf.loadModel(robot, 0, "sphere", "freeflyer", SPHERE_URDF, SPHERE_SRDF, SE3.Identity()) + return robot + + +class TestGrippers(unittest.TestCase): + + def test_grippers_returns_map(self): + robot = create_manipulation_device() + + grippers = robot.grippers() + + self.assertIsNotNone(grippers) + + def test_gripper_exists(self): + robot = create_manipulation_device() + + grippers = robot.grippers() + + self.assertIn("ur3/gripper", grippers) + + def test_gripper_has_local_position(self): + robot = create_manipulation_device() + grippers = robot.grippers() + + gripper = grippers["ur3/gripper"] + + self.assertTrue(hasattr(gripper, "localPosition")) + + +class TestHandles(unittest.TestCase): + + def test_handles_returns_map(self): + robot = create_manipulation_device() + + handles = robot.handles() + + self.assertIsNotNone(handles) + + def test_handle_exists(self): + robot = create_manipulation_device() + + handles = robot.handles() + + self.assertIn("sphere/handle", handles) + + def test_handle_has_local_position(self): + robot = create_manipulation_device() + handles = robot.handles() + + handle = handles["sphere/handle"] + + self.assertTrue(hasattr(handle, "localPosition")) + + def test_handle_has_mask(self): + robot = create_manipulation_device() + handles = robot.handles() + + handle = handles["sphere/handle"] + + self.assertTrue(hasattr(handle, "mask")) + + def test_handle_mask_modifiable(self): + robot = create_manipulation_device() + handles = robot.handles() + handle = handles["sphere/handle"] + + handle.mask = [True, True, True, False, True, True] + + self.assertEqual(handle.mask[3], False) + + +class TestGraspCreation(unittest.TestCase): + + def test_create_grasp_constraint(self): + robot = create_manipulation_device() + handles = robot.handles() + grippers = robot.grippers() + + handle = handles["sphere/handle"] + gripper = grippers["ur3/gripper"] + + constraint = handle.createGrasp(gripper, "test_grasp") + + self.assertIsNotNone(constraint) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_liegroup.py b/tests/unit/test_liegroup.py new file mode 100644 index 0000000..b184e58 --- /dev/null +++ b/tests/unit/test_liegroup.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pyhpp.pinocchio import LiegroupSpace, LiegroupElement + + +class TestLiegroupSpace(unittest.TestCase): + + def test_create_r1(self): + space = LiegroupSpace.R1(False) + + self.assertIsNotNone(space) + self.assertEqual(str(space), "R^1") + + def test_create_r2(self): + space = LiegroupSpace.R2() + + self.assertEqual(str(space), "R^2") + + def test_create_r3(self): + space = LiegroupSpace.R3() + + self.assertEqual(str(space), "R^3") + + def test_multiply_spaces(self): + space1 = LiegroupSpace.R2() + space2 = LiegroupSpace.R1(False) + + combined = space1 * space2 + + self.assertEqual(str(combined), "R^3") + + def test_inplace_multiply(self): + space = LiegroupSpace.R2() + space *= LiegroupSpace.R1(False) + + self.assertEqual(str(space), "R^3") + + def test_merge_vector_spaces(self): + space = LiegroupSpace.R2() * LiegroupSpace.R2() + space.mergeVectorSpaces() + + self.assertEqual(str(space), "R^4") + + +class TestLiegroupElement(unittest.TestCase): + + def test_create_element(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + self.assertIsNotNone(el) + + def test_element_vector(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + np.testing.assert_array_equal(el.vector(), v) + + def test_element_space(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + self.assertEqual(el.space(), space) + + def test_element_subtraction(self): + space = LiegroupSpace.R3() + el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) + el2 = LiegroupElement(np.array([1.0, 2.0, 3.0]), space) + + diff = el2 - el1 + + np.testing.assert_array_almost_equal(diff.flatten(), [1.0, 1.0, 1.0]) + + def test_element_addition(self): + space = LiegroupSpace.R3() + el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) + v = np.array([1.0, 1.0, 1.0]) + + el2 = el1 + v + + np.testing.assert_array_almost_equal(el2.vector(), [1.0, 2.0, 3.0]) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path.py b/tests/unit/test_path.py new file mode 100644 index 0000000..2cb4515 --- /dev/null +++ b/tests/unit/test_path.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +import pyhpp.core.path + + +class TestPathMethods(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_extract_returns_subpath(self): + """Path.extract should return a portion of the original path.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + original_length = path.length() + + # Extract middle half + t_start = original_length * 0.25 + t_end = original_length * 0.75 + extracted = path.extract(t_start, t_end) + + self.assertIsNotNone(extracted) + self.assertAlmostEqual(extracted.length(), original_length * 0.5, places=5) + + def test_copy_creates_independent_path(self): + """Path.copy should create an independent copy.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + copied = path.copy() + + self.assertIsNotNone(copied) + self.assertEqual(copied.length(), path.length()) + np.testing.assert_array_almost_equal(copied.initial(), path.initial()) + np.testing.assert_array_almost_equal(copied.end(), path.end()) + + def test_derivative_returns_velocity(self): + """Path.derivative should return velocity at given parameter.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + t_mid = path.length() / 2.0 + deriv = path.derivative(t_mid, 1) + + self.assertEqual(len(deriv), self.robot.numberDof()) + # For straight path, first joint derivative should be non-zero + self.assertNotEqual(deriv[0], 0.0) + + def test_length_is_non_negative(self): + """Path.length should be non-negative for valid paths.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertGreaterEqual(path.length(), 0.0) + + +class TestPathNegativeCases(unittest.TestCase): + """Negative test cases for Path operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_extract_valid_range(self): + """Extracting with valid range should succeed.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + length = path.length() + + # Valid extraction + extracted = path.extract(0.2 * length, 0.8 * length) + self.assertIsNotNone(extracted) + self.assertAlmostEqual(extracted.length(), 0.6 * length, places=5) + + def test_initial_and_end_accessors(self): + """Path.initial() and path.end() should return correct configs.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Check initial config + np.testing.assert_array_almost_equal(path.initial(), q1) + # Check end config + np.testing.assert_array_almost_equal(path.end(), q2) + + def test_call_operator_at_midpoint(self): + """Calling path(t) should return interpolated config and success status.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Path call operator returns (config, success) tuple + q_mid, success = path(path.length() / 2.0) + self.assertTrue(success) + # First joint should be midway + self.assertAlmostEqual(q_mid[0], 0.5, places=5) + + def test_derivative_at_order_one(self): + """Derivative at order 1 returns velocity.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Order 1 derivative is velocity + deriv1 = path.derivative(path.length() / 2.0, 1) + self.assertEqual(len(deriv1), self.robot.numberDof()) + + +class TestPathVector(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_create_empty_path_vector(self): + """PathVector.create should create an empty vector.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + self.assertIsNotNone(pv) + self.assertEqual(pv.numberPaths(), 0) + + def test_append_path_increases_count(self): + """Appending paths should increase numberPaths.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + pv.appendPath(path) + + self.assertEqual(pv.numberPaths(), 1) + + def test_path_vector_length_is_sum(self): + """PathVector length should be sum of contained paths.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path1 = steer(q1, q2) + path2 = steer(q2, q3) + + pv.appendPath(path1) + pv.appendPath(path2) + + expected_length = path1.length() + path2.length() + self.assertAlmostEqual(pv.length(), expected_length, places=10) + + +class TestPathVectorNegativeCases(unittest.TestCase): + """Negative test cases for PathVector operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_path_at_valid_index_works(self): + """Accessing path at valid index should work.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + pv.appendPath(path) + + retrieved = pv.pathAtRank(0) + self.assertIsNotNone(retrieved) + self.assertAlmostEqual(retrieved.length(), path.length(), places=10) + + def test_empty_path_vector_has_zero_length(self): + """Empty PathVector should have zero length.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + self.assertEqual(pv.length(), 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_optimizer.py b/tests/unit/test_path_optimizer.py new file mode 100644 index 0000000..10c34aa --- /dev/null +++ b/tests/unit/test_path_optimizer.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import RandomShortcut, SimpleShortcut +import pyhpp.core.path + + +class TestPathOptimizerInstantiation(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_random_shortcut_creates(self): + optimizer = RandomShortcut(self.problem) + + self.assertIsNotNone(optimizer) + + def test_simple_shortcut_creates(self): + optimizer = SimpleShortcut(self.problem) + + self.assertIsNotNone(optimizer) + + +class TestPathOptimizerOptimize(unittest.TestCase): + + def test_optimize_returns_path(self): + """PathOptimizer.optimize should return a valid path.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.3, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([0.6, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path1 = steer(q1, q2) + path2 = steer(q2, q3) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path1) + pv.appendPath(path2) + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(10) + + optimized = optimizer.optimize(pv) + + self.assertIsNotNone(optimized) + # Optimized path should preserve start and end + np.testing.assert_array_almost_equal(optimized.initial(), q1) + np.testing.assert_array_almost_equal(optimized.end(), q3) + + def test_max_iterations_settable(self): + """PathOptimizer.maxIterations should be settable.""" + problem, robot = create_ur5_problem() + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(50) + + # Verify it doesn't crash and actually limits iterations + # by running optimization on a simple path + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + steer = problem.steeringMethod() + path = steer(q1, q2) + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + + optimized = optimizer.optimize(pv) + self.assertIsNotNone(optimized) + + +class TestPathOptimizerNegativeCases(unittest.TestCase): + """Negative test cases for PathOptimizer.""" + + def test_optimize_zero_length_path(self): + """Optimizing a zero-length path should return zero-length path.""" + problem, robot = create_ur5_problem() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path = steer(q, q) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(10) + + optimized = optimizer.optimize(pv) + + self.assertEqual(optimized.length(), 0.0) + + def test_optimize_preserves_endpoints(self): + """Optimization must always preserve path endpoints.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.2, -1.4, -1.6, 0.1, 0.7, 0.1]) + q3 = np.array([0.4, -1.2, -1.4, 0.2, 0.6, 0.2]) + q4 = np.array([0.6, -1.0, -1.2, 0.3, 0.5, 0.3]) + + steer = problem.steeringMethod() + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(steer(q1, q2)) + pv.appendPath(steer(q2, q3)) + pv.appendPath(steer(q3, q4)) + + optimizer = SimpleShortcut(problem) + optimizer.maxIterations(20) + + optimized = optimizer.optimize(pv) + + np.testing.assert_array_almost_equal(optimized.initial(), q1) + np.testing.assert_array_almost_equal(optimized.end(), q4) + + def test_zero_iterations_returns_same_path(self): + """Zero iterations should return path with same length.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path = steer(q1, q2) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + original_length = pv.length() + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(0) + + optimized = optimizer.optimize(pv) + + self.assertAlmostEqual(optimized.length(), original_length, places=5) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_planner.py b/tests/unit/test_path_planner.py new file mode 100644 index 0000000..a5744c9 --- /dev/null +++ b/tests/unit/test_path_planner.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import ( + DiffusingPlanner, + BiRRTPlanner, + VisibilityPrmPlanner, +) + + +class TestPathPlannerInstantiation(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_diffusing_planner_creates(self): + planner = DiffusingPlanner(self.problem) + + self.assertIsNotNone(planner) + + def test_birrt_planner_creates(self): + planner = BiRRTPlanner(self.problem) + + self.assertIsNotNone(planner) + + def test_visibility_prm_planner_creates(self): + planner = VisibilityPrmPlanner(self.problem) + + self.assertIsNotNone(planner) + + +class TestPathPlannerConfiguration(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_max_iterations_settable(self): + planner = DiffusingPlanner(self.problem) + + planner.maxIterations(500) + + self.assertEqual(planner.maxIterations(), 500) + + def test_max_iterations_defaults_to_zero(self): + planner = BiRRTPlanner(self.problem) + + self.assertEqual(planner.maxIterations(), 0) + + +class TestPathPlannerSolve(unittest.TestCase): + + def test_birrt_solves_simple_path(self): + """BiRRT should find a path between nearby collision-free configurations.""" + problem, robot = create_ur5_problem() + + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(200) + + path = planner.solve() + + self.assertIsNotNone(path) + self.assertGreater(path.length(), 0) + np.testing.assert_array_almost_equal(path.initial(), q_init) + np.testing.assert_array_almost_equal(path.end(), q_goal) + + +class TestPathPlannerNegativeCases(unittest.TestCase): + """Negative test cases for path planners.""" + + def test_solve_without_goal_raises(self): + """Solving without a goal configuration should raise an error.""" + problem, robot = create_ur5_problem() + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + + planner = BiRRTPlanner(problem) + planner.maxIterations(10) + + with self.assertRaises(Exception): + planner.solve() + + def test_solve_without_init_raises(self): + """Solving without an initial configuration should raise an error.""" + problem, robot = create_ur5_problem() + q_goal = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(10) + + with self.assertRaises(Exception): + planner.solve() + + def test_max_iterations_zero_limits_exploration(self): + """Planner with zero max iterations should do minimal exploration.""" + problem, robot = create_ur5_problem() + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([3.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(0) + + try: + planner.solve() + except Exception: + pass + + roadmap = planner.roadmap() + self.assertLessEqual(len(roadmap.nodes()), 2) + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_projector.py b/tests/unit/test_path_projector.py new file mode 100644 index 0000000..d1fb11b --- /dev/null +++ b/tests/unit/test_path_projector.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import ( + Progressive, + GlobalProjector, +) + + +class TestPathProjectorInstantiation(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_progressive_projector_creates(self): + projector = Progressive(self.robot, 0.1) + + self.assertIsNotNone(projector) + + def test_global_projector_creates(self): + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + self.assertIsNotNone(projector) + + +class TestPathProjectorProject(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_global_projector_projects_unconstrained_path(self): + """Without constraints, projection should succeed and preserve endpoints.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q1, q2) + + success, projected_path = projector.apply(path) + + self.assertTrue(success) + self.assertIsNotNone(projected_path) + np.testing.assert_array_almost_equal(projected_path.initial(), q1) + np.testing.assert_array_almost_equal(projected_path.end(), q2) + + +class TestPathProjectorNegativeCases(unittest.TestCase): + """Negative test cases for PathProjector.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_zero_step_creates_projector(self): + """Projector with zero step should still be created.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + + projector = GlobalProjector(distance, steer, 0.0) + + self.assertIsNotNone(projector) + + def test_negative_step_creates_projector(self): + """Projector with negative step still creates (library accepts it).""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + + projector = GlobalProjector(distance, steer, -0.1) + + self.assertIsNotNone(projector) + + def test_zero_length_path_projection(self): + """Projection of zero-length path should succeed.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q, q) + + success, projected_path = projector.apply(path) + + self.assertTrue(success) + self.assertEqual(projected_path.length(), 0.0) + + def test_apply_returns_two_values(self): + """Apply should return (success, path) tuple.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q1, q2) + + result = projector.apply(path) + + self.assertEqual(len(result), 2) + self.assertIsInstance(result[0], bool) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_validation.py b/tests/unit/test_path_validation.py new file mode 100644 index 0000000..a65c4c2 --- /dev/null +++ b/tests/unit/test_path_validation.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestPathValidation(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_collision_free_path_is_valid(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + self.assertTrue(is_valid) + + def test_zero_length_path_returns_valid_part(self): + """A zero-length path (same start/end) should still return a valid_part.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q1) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + self.assertIsNotNone(valid_part) + self.assertEqual(valid_part.length(), 0.0) + + def test_validate_returns_three_values(self): + """Validate always returns (bool, path, report) tuple.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + result = self.problem.pathValidation().validate(path, False) + self.assertEqual(len(result), 3) + + +class TestPathValidationNegativeCases(unittest.TestCase): + """Negative test cases for PathValidation.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_valid_part_never_none(self): + """Valid part should never be None, even for invalid paths.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + + self.assertIsNotNone(valid_part) + + def test_valid_part_length_less_than_or_equal_original(self): + """Valid part length should never exceed original path length.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + + self.assertLessEqual(valid_part.length(), path.length()) + + def test_reverse_flag_produces_valid_result(self): + """Validation with reverse=True should still produce valid results.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.3, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, True + ) + + self.assertIsNotNone(valid_part) + self.assertIsInstance(is_valid, bool) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_position_constraint.py b/tests/unit/test_position_constraint.py new file mode 100644 index 0000000..c6b35ba --- /dev/null +++ b/tests/unit/test_position_constraint.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3, StdVec_Bool as Mask +from pyhpp.pinocchio import Device, LiegroupElement, urdf +from pyhpp.constraints import Position + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_device(): + robot = Device("ur5") + urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + return robot + + +class TestPositionConstraint(unittest.TestCase): + + def test_create_position_constraint(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "test_position", + robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + self.assertIsNotNone(pc) + + def test_position_constraint_str(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "my_position", + robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + self.assertIn("my_position", str(pc)) + + def test_position_constraint_dimensions(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", + robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + self.assertEqual(pc.ndo, 3) + + +class TestPositionConstraintEvaluation(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.robot = create_ur5_device() + + def test_value_cpp_api(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", + self.robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + q = np.zeros((pc.ni, 1)) + v = LiegroupElement(pc.outputSpace()) + pc.value(v, q) + + self.assertEqual(len(v.vector()), 3) + + def test_jacobian_cpp_api(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", + self.robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + q = self.robot.currentConfiguration() + J = np.zeros((pc.ndo, pc.ndi)) + pc.jacobian(J, q) + + self.assertEqual(J.shape, (3, self.robot.numberDof())) + + def test_call_operator(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", + self.robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + q = np.zeros((pc.ni, 1)) + result = pc(q) + + self.assertIsNotNone(result) + + def test_jacobian_shorthand(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", + self.robot, + joint_id, + SE3.Identity(), + SE3.Identity(), + mask + ) + + q = self.robot.currentConfiguration() + J = pc.J(q) + + self.assertEqual(J.shape[0], pc.ndo) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_problem.py b/tests/unit/test_problem.py new file mode 100644 index 0000000..231fbdf --- /dev/null +++ b/tests/unit/test_problem.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 +from unit.conftest import create_ur5_problem + +class TestProblemAccessors(unittest.TestCase): + + def test_steering_method_returns_object(self): + problem, robot = create_ur5_problem() + + sm = problem.steeringMethod() + + self.assertIsNotNone(sm) + + def test_distance_returns_object(self): + problem, robot = create_ur5_problem() + + distance = problem.distance() + + self.assertIsNotNone(distance) + + def test_configuration_shooter_returns_object(self): + problem, robot = create_ur5_problem() + + shooter = problem.configurationShooter() + + self.assertIsNotNone(shooter) + + def test_path_validation_returns_object(self): + problem, robot = create_ur5_problem() + + pv = problem.pathValidation() + + self.assertIsNotNone(pv) + + def test_config_validation_returns_object(self): + problem, robot = create_ur5_problem() + + cv = problem.configValidation() + + self.assertIsNotNone(cv) + + +class TestProblemDirectPath(unittest.TestCase): + + def test_direct_path_without_validation(self): + problem, robot = create_ur5_problem() + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = problem.directPath(q_start, q_end, False) + + self.assertIsInstance(result, tuple) + self.assertEqual(len(result), 3) + valid, path, report = result + self.assertTrue(valid) + self.assertIsNotNone(path) + + def test_direct_path_with_validation(self): + problem, robot = create_ur5_problem() + problem.addConfigValidation("JointBoundValidation") + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + valid, path, report = problem.directPath(q_start, q_end, True) + + self.assertIsInstance(valid, bool) + + def test_direct_path_endpoints_match(self): + problem, robot = create_ur5_problem() + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + valid, path, report = problem.directPath(q_start, q_end, False) + + np.testing.assert_array_almost_equal(path.initial(), q_start) + np.testing.assert_array_almost_equal(path.end(), q_end) + + +class TestProblemConstraintProjection(unittest.TestCase): + + def test_error_threshold_settable(self): + problem, robot = create_ur5_problem() + + problem.errorThreshold = 1e-6 + + self.assertAlmostEqual(problem.errorThreshold, 1e-6) + + def test_max_iter_projection_settable(self): + problem, robot = create_ur5_problem() + + problem.maxIterProjection = 50 + + self.assertEqual(problem.maxIterProjection, 50) + + def test_apply_constraints_returns_tuple(self): + problem, robot = create_ur5_problem() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = problem.applyConstraints(q) + + self.assertIsInstance(result, tuple) + self.assertEqual(len(result), 3) + success, output, residual = result + self.assertIsInstance(success, bool) + self.assertEqual(len(output), len(q)) + + +class TestProblemTransformationConstraints(unittest.TestCase): + + def test_create_transformation_constraint(self): + problem, robot = create_ur5_problem() + M = SE3.Identity() + mask = [True, True, True, True, True, True] + + constraint = problem.createTransformationConstraint( + "test_constraint", + "", + "ur5/ee_fixed_joint", + M, + mask + ) + + self.assertIsNotNone(constraint) + + def test_create_relative_transformation_constraint(self): + problem, robot = create_ur5_problem() + M = SE3.Identity() + mask = [True, True, True, True, True, True] + + constraint = problem.createTransformationConstraint( + "relative_constraint", + "ur5/shoulder_link", + "ur5/ee_fixed_joint", + M, + mask + ) + + self.assertIsNotNone(constraint) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_roadmap.py b/tests/unit/test_roadmap.py new file mode 100644 index 0000000..394c054 --- /dev/null +++ b/tests/unit/test_roadmap.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import Roadmap, WeighedDistance + + +class TestRoadmap(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + cls.distance = WeighedDistance(cls.robot) + + def test_init_creates_one_component(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + roadmap.initNode(q_init) + + self.assertEqual(roadmap.numberConnectedComponents(), 1) + + def test_goal_creates_separate_component(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([1.57, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q_init) + roadmap.addGoalNode(q_goal) + + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + def test_nearest_node_returns_closest(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + roadmap.initNode(q_init) + + q_query = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + cc = roadmap.getConnectedComponent(0) + q_near, dist = roadmap.nearestNode(q_query, cc) + + np.testing.assert_array_almost_equal(q_near, q_init) + self.assertGreater(dist, 0) + + def test_add_node_increases_component_count(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_other = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q_init) + self.assertEqual(roadmap.numberConnectedComponents(), 1) + + roadmap.addNode(q_other) + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + def test_add_edge_creates_connection(self): + """Adding an edge between nodes should create a connection.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + + nodes = roadmap.nodes() + self.assertEqual(len(nodes), 2) + node1, node2 = nodes[0], nodes[1] + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + roadmap.addEdge(node1, node2, path) + + # Edge was added successfully (no exception raised) + self.assertEqual(len(roadmap.nodes()), 2) + + +class TestRoadmapNegativeCases(unittest.TestCase): + """Negative test cases for Roadmap operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + cls.distance = WeighedDistance(cls.robot) + + def test_empty_roadmap_has_zero_components(self): + """Empty roadmap should have zero connected components.""" + roadmap = Roadmap(self.distance, self.robot) + + self.assertEqual(roadmap.numberConnectedComponents(), 0) + + def test_empty_roadmap_nodes_list_empty(self): + """Empty roadmap should have no nodes.""" + roadmap = Roadmap(self.distance, self.robot) + + self.assertEqual(len(roadmap.nodes()), 0) + + def test_multiple_nodes_tracked(self): + """Adding multiple nodes should increase node count.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + roadmap.addNode(q3) + + self.assertEqual(len(roadmap.nodes()), 3) + + def test_clear_resets_roadmap(self): + """Clear should reset the roadmap to empty state.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + roadmap.clear() + self.assertEqual(roadmap.numberConnectedComponents(), 0) + self.assertEqual(len(roadmap.nodes()), 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_security_margins.py b/tests/unit/test_security_margins.py new file mode 100644 index 0000000..5d350b3 --- /dev/null +++ b/tests/unit/test_security_margins.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from unit.conftest import ( + create_security_margins_instance, + create_constraint_graph_setup, +) +from pyhpp.manipulation.security_margins import SecurityMargins + + +class TestSecurityMarginsCorrectness(unittest.TestCase): + + def test_gripper_correctly_mapped_to_robot(self): + sm, *_ = create_security_margins_instance() + + self.assertIn("ur3/gripper", sm.gripperToRobot) + self.assertEqual(sm.gripperToRobot["ur3/gripper"], "ur3") + self.assertIn("ur3/gripper", sm.gripperToJoints) + self.assertGreater(len(sm.gripperToJoints["ur3/gripper"]), 0) + + def test_margins_stored_symmetrically(self): + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 0.05) + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "ur3"), 0.05) + + def test_default_margin_used_for_unset_pairs(self): + sm, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.03 + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "sphere1"), 0.03) + + def test_apply_sets_margins_on_edges(self): + sm, _, graph, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.02 + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + for edge in edges: + matrix = graph.getSecurityMarginMatrixForTransition(edge) + self.assertIsNotNone(matrix) + + def test_full_workflow_with_graph(self): + problem, graph, factory, robot, objects = create_constraint_graph_setup() + + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + sm.defaultMargin = 0.02 + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.setSecurityMarginBetween("ur3", "sphere1", 0.05) + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + state = graph.getState("free") + q = robot.currentConfiguration() + result, _, _ = graph.applyStateConstraints(state, q) + self.assertIsInstance(result, bool) + + def test_grasp_edge_has_zero_margin_between_gripper_and_object(self): + """ + Grasp edges must have zero margin between gripper and grasped object, + otherwise collision detection would always reject the grasp. + Free/transit edges keep the configured safety margin. + """ + problem, graph, factory, robot, objects = create_constraint_graph_setup() + + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + configured_margin = 0.05 + sm.defaultMargin = configured_margin + sm.setSecurityMarginBetween("ur3", "sphere0", configured_margin) + sm.apply() + + model = robot.model() + gripper_idx = model.getJointId("ur3/wrist_3_joint") + object_idx = model.getJointId("sphere0/root_joint") + + def get_margin(edge, idx1, idx2): + matrix = graph.getSecurityMarginMatrixForTransition(edge) + if matrix and len(matrix) > idx1 and len(matrix[idx1]) > idx2: + return matrix[idx1][idx2] + return None + + edges = graph.getTransitions() + edge_names = graph.getTransitionNames() + + found_free_loop = False + found_grasp_edge = False + free_margin = None + grasp_margin = None + + for edge, name in zip(edges, edge_names): + margin = get_margin(edge, gripper_idx, object_idx) + if margin is None: + continue + + if name == "Loop | f": + found_free_loop = True + free_margin = margin + self.assertAlmostEqual(margin, configured_margin, places=6) + + elif name == "Loop | 0-0": + found_grasp_edge = True + grasp_margin = margin + self.assertEqual(margin, 0.0) + + self.assertTrue(found_free_loop, "Did not find 'Loop | f' edge") + self.assertTrue(found_grasp_edge, f"Did not find 'Loop | 0-0' edge. Available: {edge_names}") + + if free_margin is not None and grasp_margin is not None: + self.assertNotEqual(free_margin, grasp_margin) + + def test_joint_to_robot_mapping_correct(self): + sm, *_ = create_security_margins_instance() + + self.assertIn("ur3", sm.robotToJoints) + self.assertIn("sphere0", sm.robotToJoints) + self.assertIn("sphere1", sm.robotToJoints) + + for joint in sm.robotToJoints["ur3"]: + self.assertEqual(sm.jointToRobot[joint], "ur3") + + for joint in sm.robotToJoints["sphere0"]: + self.assertEqual(sm.jointToRobot[joint], "sphere0") + + def test_get_active_constraints_returns_dict(self): + sm, _, graph, *_ = create_security_margins_instance() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + result = sm.getActiveConstraintsAlongEdge(edges[0]) + + self.assertIsInstance(result, dict) + self.assertIn("place", result) + self.assertIn("grasp", result) + self.assertIsInstance(result["place"], list) + self.assertIsInstance(result["grasp"], list) + + def test_grasp_edge_has_grasp_constraint(self): + problem, graph, factory, robot, objects = create_constraint_graph_setup() + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + + edge_names = graph.getTransitionNames() + edges = graph.getTransitions() + + for edge, name in zip(edges, edge_names): + if "0-0" in name: + result = sm.getActiveConstraintsAlongEdge(edge) + self.assertGreater(len(result["grasp"]), 0) + break + + +class TestSecurityMarginsNegativeCases(unittest.TestCase): + """Negative test cases for SecurityMargins.""" + + def test_get_margin_unknown_robot_returns_default(self): + """Getting margin for unknown robot pair should return default.""" + sm, *_ = create_security_margins_instance() + sm.defaultMargin = 0.01 + + margin = sm.getSecurityMarginBetween("unknown1", "unknown2") + self.assertEqual(margin, 0.01) + + def test_set_negative_margin_allowed(self): + """Negative margins should be allowed (for penetration tolerance).""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", -0.01) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), -0.01) + + def test_zero_default_margin(self): + """Zero default margin should work correctly.""" + sm, *_ = create_security_margins_instance() + sm.defaultMargin = 0.0 + + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "sphere1"), 0.0) + + def test_overwrite_margin(self): + """Setting margin twice should overwrite.""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.setSecurityMarginBetween("ur3", "sphere0", 0.10) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 0.10) + + def test_apply_multiple_times(self): + """Applying margins multiple times should not raise.""" + sm, _, graph, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.02 + sm.apply() + sm.defaultMargin = 0.03 + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + def test_large_margin_value(self): + """Very large margin values should be accepted.""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 100.0) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 100.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_steering_method.py b/tests/unit/test_steering_method.py new file mode 100644 index 0000000..9c3d631 --- /dev/null +++ b/tests/unit/test_steering_method.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestSteeringMethod(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_creates_path_with_correct_endpoints(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.57, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertIsNotNone(path) + self.assertGreater(path.length(), 0) + np.testing.assert_array_almost_equal(path.initial(), q1) + np.testing.assert_array_almost_equal(path.end(), q2) + + def test_path_length_is_positive(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.0, -1.5, 0.2, 0.5, 0.1]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertGreater(path.length(), 0) + + def test_identical_configs_creates_zero_length_path(self): + """Steering between identical configs should create a zero-length path.""" + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q, q) + + self.assertIsNotNone(path) + self.assertEqual(path.length(), 0.0) + + +if __name__ == "__main__": + unittest.main() From 01312f2d7ccf59ea213aa55baa0d688cbd8f621e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 14 Jan 2026 09:15:36 +0000 Subject: [PATCH 4/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/constraints/by-substitution.cc | 3 +- .../static_stability_constraint_factory.py | 4 +- tests/CMakeLists.txt | 16 +- tests/integration/benchmark_utils.py | 11 +- tests/integration/construction-set-m-rrt.py | 361 ++++++++++++------ tests/integration/pr2-in-iai-kitchen.py | 12 +- tests/integration/romeo-placard.py | 128 +++++-- tests/integration/test_benchmarks.py | 7 +- tests/unit/test_configuration_shooter.py | 9 +- tests/unit/test_constraint_factory.py | 15 +- tests/unit/test_constraint_graph_factory.py | 16 +- tests/unit/test_device.py | 6 - tests/unit/test_differentiable_function.py | 1 - tests/unit/test_handles_grippers.py | 8 +- tests/unit/test_liegroup.py | 2 - tests/unit/test_path.py | 2 - tests/unit/test_path_optimizer.py | 2 - tests/unit/test_path_planner.py | 4 +- tests/unit/test_path_projector.py | 2 - tests/unit/test_path_validation.py | 1 - tests/unit/test_position_constraint.py | 53 +-- tests/unit/test_problem.py | 17 +- tests/unit/test_roadmap.py | 1 - tests/unit/test_security_margins.py | 5 +- tests/unit/test_steering_method.py | 1 - 25 files changed, 407 insertions(+), 280 deletions(-) diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 11616a3..4a74eed 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -47,7 +47,8 @@ tuple BySubstitution_solve(const BySubstitution& hs, const vector_t& q) { void exposeBySubstitution() { enum_("SolverStatus") .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED) - .value("MAX_ITERATION_REACHED", HierarchicalIterative::MAX_ITERATION_REACHED) + .value("MAX_ITERATION_REACHED", + HierarchicalIterative::MAX_ITERATION_REACHED) .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) .value("SUCCESS", HierarchicalIterative::SUCCESS); diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 95c8ace..ab8c742 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -178,8 +178,8 @@ def createAlignedCOMStabilityConstraint( np.zeros(3), np.zeros(3), "", - x,git - [True] * 4, + x, + git[True] * 4, ) created_constraints[result[-1]] = constraint diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index bd0d927..678a5f2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -40,18 +40,16 @@ foreach(TEST_FILE ${UNITTEST_TEST_FILES}) get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) add_test( NAME unittest_${TEST_NAME} - COMMAND ${PYTHON_EXECUTABLE} -m unittest discover - -s ${CMAKE_CURRENT_SOURCE_DIR} - -p "${TEST_NAME}.py" - -v - WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} - ) + COMMAND ${PYTHON_EXECUTABLE} -m unittest discover -s + ${CMAKE_CURRENT_SOURCE_DIR} -p "${TEST_NAME}.py" -v + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}) set_tests_properties( unittest_${TEST_NAME} PROPERTIES - ENVIRONMENT "${UNITTEST_ENV_VARIABLES}" + ENVIRONMENT + "${UNITTEST_ENV_VARIABLES}" ENVIRONMENT_MODIFICATION "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;" - LABELS "python;unit" - ) + LABELS + "python;unit") endforeach() diff --git a/tests/integration/benchmark_utils.py b/tests/integration/benchmark_utils.py index b331d72..670b45c 100644 --- a/tests/integration/benchmark_utils.py +++ b/tests/integration/benchmark_utils.py @@ -36,6 +36,7 @@ def create_benchmark_parser(description: str = "HPP Benchmark") -> ArgumentParse @dataclass class BenchmarkResult: """Results from a single benchmark iteration.""" + success: bool time_elapsed: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) num_nodes: int = 0 @@ -46,6 +47,7 @@ class BenchmarkResult: @dataclass class BenchmarkStats: """Aggregated statistics from benchmark runs.""" + total_iterations: int = 0 num_successes: int = 0 total_time: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) @@ -164,9 +166,13 @@ def print_stats(self) -> None: if self.stats.num_successes > 0: print(f"Average time per success: {self.stats.avg_time_per_success:.4f}s") - print(f"Average number nodes per success: {self.stats.avg_nodes_per_success:.1f}") + print( + f"Average number nodes per success: {self.stats.avg_nodes_per_success:.1f}" + ) if self.stats.total_path_length > 0: - print(f"Average path length per success: {self.stats.avg_path_length_per_success:.4f}") + print( + f"Average path length per success: {self.stats.avg_path_length_per_success:.4f}" + ) def verify_results(self) -> bool: """ @@ -212,6 +218,7 @@ def run_benchmark_main( Returns: BenchmarkStats with results """ + def reset(): planner.roadmap().clear() problem.resetGoalConfigs() diff --git a/tests/integration/construction-set-m-rrt.py b/tests/integration/construction-set-m-rrt.py index 789b551..c2f55b6 100644 --- a/tests/integration/construction-set-m-rrt.py +++ b/tests/integration/construction-set-m-rrt.py @@ -22,16 +22,19 @@ from benchmark_utils import create_benchmark_parser, run_benchmark_main parser = create_benchmark_parser("Construction Set M-RRT Benchmark") -parser.add_argument('--bigGraph', 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() @@ -39,22 +42,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) @@ -71,20 +74,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) @@ -92,23 +95,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() @@ -127,42 +168,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]) @@ -172,52 +239,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] @@ -227,13 +320,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) @@ -242,23 +335,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 @@ -266,7 +362,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 @@ -277,103 +373,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) @@ -381,6 +515,7 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: if args.display: from pyhpp_plot import show_graph + show_graph(cg) manipulationPlanner = ManipulationPlanner(problem) @@ -394,4 +529,4 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: q_init=q0, q_goal=q_goal, num_iterations=args.N, - ) \ No newline at end of file + ) diff --git a/tests/integration/pr2-in-iai-kitchen.py b/tests/integration/pr2-in-iai-kitchen.py index b4508b8..9897b49 100644 --- a/tests/integration/pr2-in-iai-kitchen.py +++ b/tests/integration/pr2-in-iai-kitchen.py @@ -15,7 +15,7 @@ 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) @@ -31,17 +31,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) diff --git a/tests/integration/romeo-placard.py b/tests/integration/romeo-placard.py index 9e18d5c..cc481f7 100644 --- a/tests/integration/romeo-placard.py +++ b/tests/integration/romeo-placard.py @@ -5,7 +5,7 @@ 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, ) @@ -18,23 +18,33 @@ # 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() @@ -84,7 +94,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]) @@ -97,7 +107,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]) @@ -111,13 +121,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( @@ -125,9 +135,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] @@ -137,8 +147,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), @@ -148,7 +158,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() @@ -158,11 +168,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: @@ -192,5 +274,3 @@ q_goal=q_goal_proj, num_iterations=args.N, ) - - diff --git a/tests/integration/test_benchmarks.py b/tests/integration/test_benchmarks.py index 8122c5b..84ee36b 100644 --- a/tests/integration/test_benchmarks.py +++ b/tests/integration/test_benchmarks.py @@ -16,15 +16,14 @@ def main(): # List of benchmark files to run (excluding this file and utilities) excluded = {Path(__file__).name, "benchmark_utils.py", "__init__.py"} benchmarks = sorted( - script for script in folder.glob("*.py") - if script.name not in excluded + script for script in folder.glob("*.py") if script.name not in excluded ) print(f"Running {len(benchmarks)} benchmarks...") failed = [] for script in benchmarks: - print(f"\n{'='*60}") + print(f"\n{'=' * 60}") print(f"Running: {script.name}") print("=" * 60) @@ -37,7 +36,7 @@ def main(): failed.append(script.name) print(f"FAILED: {script.name} (exit code {result.returncode})") - print(f"\n{'='*60}") + print(f"\n{'=' * 60}") print("SUMMARY") print("=" * 60) print(f"Total benchmarks: {len(benchmarks)}") diff --git a/tests/unit/test_configuration_shooter.py b/tests/unit/test_configuration_shooter.py index 3513dee..fdfd4cf 100644 --- a/tests/unit/test_configuration_shooter.py +++ b/tests/unit/test_configuration_shooter.py @@ -10,7 +10,6 @@ class TestConfigurationShooter(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -37,13 +36,9 @@ def test_shot_respects_joint_bounds(self): for _ in range(10): q = shooter.shoot() for i in range(len(q)): + self.assertFalse(np.isnan(q[i]), f"Configuration element {i} is NaN") self.assertFalse( - np.isnan(q[i]), - f"Configuration element {i} is NaN" - ) - self.assertFalse( - np.isinf(q[i]), - f"Configuration element {i} is infinite" + np.isinf(q[i]), f"Configuration element {i} is infinite" ) diff --git a/tests/unit/test_constraint_factory.py b/tests/unit/test_constraint_factory.py index b0130f0..420805f 100644 --- a/tests/unit/test_constraint_factory.py +++ b/tests/unit/test_constraint_factory.py @@ -8,12 +8,10 @@ from unit.conftest import create_constraint_graph_setup from pyhpp.manipulation.constraint_graph_factory import ( Constraints, - ConstraintFactory, ) class TestConstraintFactoryBuildGrasp(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( @@ -21,9 +19,7 @@ def setUpClass(cls): ) def test_build_grasp_returns_dict(self): - result = self.factory.constraints.buildGrasp( - "ur3/gripper", "sphere0/handle" - ) + result = self.factory.constraints.buildGrasp("ur3/gripper", "sphere0/handle") self.assertIsInstance(result, dict) self.assertIn("grasp", result) @@ -31,9 +27,7 @@ def test_build_grasp_returns_dict(self): self.assertIn("preGrasp", result) def test_build_grasp_constraint_values_are_constraints(self): - result = self.factory.constraints.buildGrasp( - "ur3/gripper", "sphere0/handle" - ) + result = self.factory.constraints.buildGrasp("ur3/gripper", "sphere0/handle") self.assertIsInstance(result["grasp"], Constraints) self.assertIsInstance(result["graspComplement"], Constraints) @@ -46,13 +40,11 @@ def test_build_grasp_caches_constraints(self): result2 = cf.buildGrasp("ur3/gripper", "sphere1/handle") self.assertEqual( - result1["grasp"].numConstraints, - result2["grasp"].numConstraints + result1["grasp"].numConstraints, result2["grasp"].numConstraints ) class TestConstraintFactoryBuildPlacement(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( @@ -76,7 +68,6 @@ def test_build_placement_constraint_values_are_constraints(self): class TestConstraintFactoryRegistration(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( diff --git a/tests/unit/test_constraint_graph_factory.py b/tests/unit/test_constraint_graph_factory.py index 92ec41c..456f1af 100644 --- a/tests/unit/test_constraint_graph_factory.py +++ b/tests/unit/test_constraint_graph_factory.py @@ -6,22 +6,18 @@ import unittest from pyhpp.manipulation.constraint_graph_factory import ( - Constraints, PossibleGrasps, GraspIsAllowed, Rules, Rule, ) -class TestPossibleGrasps(unittest.TestCase): +class TestPossibleGrasps(unittest.TestCase): def test_allowed_grasp_returns_true(self): grippers = ["gripper1", "gripper2"] handles = ["handle1", "handle2", "handle3"] - grasps = { - "gripper1": ["handle1", "handle2"], - "gripper2": ["handle3"] - } + grasps = {"gripper1": ["handle1", "handle2"], "gripper2": ["handle3"]} validator = PossibleGrasps(grippers, handles, grasps) self.assertTrue(validator((0, 2))) # gripper1->handle1, gripper2->handle3 @@ -30,10 +26,7 @@ def test_allowed_grasp_returns_true(self): def test_forbidden_grasp_returns_false(self): grippers = ["gripper1", "gripper2"] handles = ["handle1", "handle2", "handle3"] - grasps = { - "gripper1": ["handle1"], - "gripper2": ["handle3"] - } + grasps = {"gripper1": ["handle1"], "gripper2": ["handle3"]} validator = PossibleGrasps(grippers, handles, grasps) self.assertFalse(validator((1, 2))) # gripper1->handle2 not allowed @@ -48,7 +41,6 @@ def test_none_grasp_allowed(self): class TestGraspIsAllowed(unittest.TestCase): - def test_empty_validator_allows_all(self): validator = GraspIsAllowed() self.assertTrue(validator((0, 1, 2))) @@ -71,7 +63,6 @@ def test_all_validators_must_pass(self): class TestRules(unittest.TestCase): - def test_simple_rule_allows(self): grippers = ["gripper1"] handles = ["handle1", "handle2"] @@ -103,5 +94,6 @@ def test_regex_pattern_match(self): # (0, None) = left_gripper grasps box_handle, right_gripper grasps nothing self.assertTrue(validator((0, None))) + if __name__ == "__main__": unittest.main() diff --git a/tests/unit/test_device.py b/tests/unit/test_device.py index efbb15b..d823798 100644 --- a/tests/unit/test_device.py +++ b/tests/unit/test_device.py @@ -21,7 +21,6 @@ def create_ur5_device(): class TestDeviceInstantiation(unittest.TestCase): - def test_device_creates_with_name(self): robot = Device("test_robot") @@ -35,7 +34,6 @@ def test_device_loads_urdf(self): class TestDeviceConfiguration(unittest.TestCase): - def test_config_size_returns_dof_count(self): robot = create_ur5_device() @@ -69,7 +67,6 @@ def test_current_configuration_readable(self): class TestDeviceJointBounds(unittest.TestCase): - def test_set_joint_bounds(self): robot = create_ur5_device() @@ -88,7 +85,6 @@ def test_set_joint_bounds_invalid_size_raises(self): class TestDeviceForwardKinematics(unittest.TestCase): - def test_compute_forward_kinematics_joint_position(self): robot = create_ur5_device() q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) @@ -141,7 +137,6 @@ def test_get_joints_position_batch(self): class TestDeviceRankInConfiguration(unittest.TestCase): - def test_rank_in_configuration_returns_dict(self): robot = create_ur5_device() @@ -168,7 +163,6 @@ def test_rank_values_are_valid_indices(self): class TestDeviceNegativeCases(unittest.TestCase): - def test_get_joint_position_invalid_frame_raises(self): robot = create_ur5_device() q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) diff --git a/tests/unit/test_differentiable_function.py b/tests/unit/test_differentiable_function.py index c61ac16..6259ca4 100644 --- a/tests/unit/test_differentiable_function.py +++ b/tests/unit/test_differentiable_function.py @@ -25,7 +25,6 @@ def impl_jacobian(self, res, arg): class TestDifferentiableFunctionInheritance(unittest.TestCase): - def test_create_subclass(self): func = DoubleFunction() diff --git a/tests/unit/test_handles_grippers.py b/tests/unit/test_handles_grippers.py index d4de577..c1e2073 100644 --- a/tests/unit/test_handles_grippers.py +++ b/tests/unit/test_handles_grippers.py @@ -5,7 +5,6 @@ # import unittest -import numpy as np from pinocchio import SE3 from pyhpp.manipulation import Device, urdf @@ -19,12 +18,13 @@ def create_manipulation_device(): robot = Device("ur3-sphere") urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, SE3.Identity()) - urdf.loadModel(robot, 0, "sphere", "freeflyer", SPHERE_URDF, SPHERE_SRDF, SE3.Identity()) + urdf.loadModel( + robot, 0, "sphere", "freeflyer", SPHERE_URDF, SPHERE_SRDF, SE3.Identity() + ) return robot class TestGrippers(unittest.TestCase): - def test_grippers_returns_map(self): robot = create_manipulation_device() @@ -49,7 +49,6 @@ def test_gripper_has_local_position(self): class TestHandles(unittest.TestCase): - def test_handles_returns_map(self): robot = create_manipulation_device() @@ -91,7 +90,6 @@ def test_handle_mask_modifiable(self): class TestGraspCreation(unittest.TestCase): - def test_create_grasp_constraint(self): robot = create_manipulation_device() handles = robot.handles() diff --git a/tests/unit/test_liegroup.py b/tests/unit/test_liegroup.py index b184e58..2e8821c 100644 --- a/tests/unit/test_liegroup.py +++ b/tests/unit/test_liegroup.py @@ -10,7 +10,6 @@ class TestLiegroupSpace(unittest.TestCase): - def test_create_r1(self): space = LiegroupSpace.R1(False) @@ -49,7 +48,6 @@ def test_merge_vector_spaces(self): class TestLiegroupElement(unittest.TestCase): - def test_create_element(self): space = LiegroupSpace.R3() v = np.array([1.0, 2.0, 3.0]) diff --git a/tests/unit/test_path.py b/tests/unit/test_path.py index 2cb4515..9fa863e 100644 --- a/tests/unit/test_path.py +++ b/tests/unit/test_path.py @@ -11,7 +11,6 @@ class TestPathMethods(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -136,7 +135,6 @@ def test_derivative_at_order_one(self): class TestPathVector(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() diff --git a/tests/unit/test_path_optimizer.py b/tests/unit/test_path_optimizer.py index 10c34aa..588f883 100644 --- a/tests/unit/test_path_optimizer.py +++ b/tests/unit/test_path_optimizer.py @@ -12,7 +12,6 @@ class TestPathOptimizerInstantiation(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -29,7 +28,6 @@ def test_simple_shortcut_creates(self): class TestPathOptimizerOptimize(unittest.TestCase): - def test_optimize_returns_path(self): """PathOptimizer.optimize should return a valid path.""" problem, robot = create_ur5_problem() diff --git a/tests/unit/test_path_planner.py b/tests/unit/test_path_planner.py index a5744c9..5acc1fe 100644 --- a/tests/unit/test_path_planner.py +++ b/tests/unit/test_path_planner.py @@ -15,7 +15,6 @@ class TestPathPlannerInstantiation(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -37,7 +36,6 @@ def test_visibility_prm_planner_creates(self): class TestPathPlannerConfiguration(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -56,7 +54,6 @@ def test_max_iterations_defaults_to_zero(self): class TestPathPlannerSolve(unittest.TestCase): - def test_birrt_solves_simple_path(self): """BiRRT should find a path between nearby collision-free configurations.""" problem, robot = create_ur5_problem() @@ -123,5 +120,6 @@ def test_max_iterations_zero_limits_exploration(self): roadmap = planner.roadmap() self.assertLessEqual(len(roadmap.nodes()), 2) + if __name__ == "__main__": unittest.main() diff --git a/tests/unit/test_path_projector.py b/tests/unit/test_path_projector.py index d1fb11b..d92d1ce 100644 --- a/tests/unit/test_path_projector.py +++ b/tests/unit/test_path_projector.py @@ -14,7 +14,6 @@ class TestPathProjectorInstantiation(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() @@ -33,7 +32,6 @@ def test_global_projector_creates(self): class TestPathProjectorProject(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() diff --git a/tests/unit/test_path_validation.py b/tests/unit/test_path_validation.py index a65c4c2..e0bfa71 100644 --- a/tests/unit/test_path_validation.py +++ b/tests/unit/test_path_validation.py @@ -10,7 +10,6 @@ class TestPathValidation(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() diff --git a/tests/unit/test_position_constraint.py b/tests/unit/test_position_constraint.py index c6b35ba..2d9d020 100644 --- a/tests/unit/test_position_constraint.py +++ b/tests/unit/test_position_constraint.py @@ -22,7 +22,6 @@ def create_ur5_device(): class TestPositionConstraint(unittest.TestCase): - def test_create_position_constraint(self): robot = create_ur5_device() joint_id = robot.model().getJointId("ur5/wrist_3_joint") @@ -30,12 +29,7 @@ def test_create_position_constraint(self): mask[:] = (True, True, True) pc = Position( - "test_position", - robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "test_position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) self.assertIsNotNone(pc) @@ -47,12 +41,7 @@ def test_position_constraint_str(self): mask[:] = (True, True, True) pc = Position( - "my_position", - robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "my_position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) self.assertIn("my_position", str(pc)) @@ -63,20 +52,12 @@ def test_position_constraint_dimensions(self): mask = Mask() mask[:] = (True, True, True) - pc = Position( - "position", - robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask - ) + pc = Position("position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask) self.assertEqual(pc.ndo, 3) class TestPositionConstraintEvaluation(unittest.TestCase): - @classmethod def setUpClass(cls): cls.robot = create_ur5_device() @@ -87,12 +68,7 @@ def test_value_cpp_api(self): mask[:] = (True, True, True) pc = Position( - "position", - self.robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) q = np.zeros((pc.ni, 1)) @@ -107,12 +83,7 @@ def test_jacobian_cpp_api(self): mask[:] = (True, True, True) pc = Position( - "position", - self.robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) q = self.robot.currentConfiguration() @@ -127,12 +98,7 @@ def test_call_operator(self): mask[:] = (True, True, True) pc = Position( - "position", - self.robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) q = np.zeros((pc.ni, 1)) @@ -146,12 +112,7 @@ def test_jacobian_shorthand(self): mask[:] = (True, True, True) pc = Position( - "position", - self.robot, - joint_id, - SE3.Identity(), - SE3.Identity(), - mask + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask ) q = self.robot.currentConfiguration() diff --git a/tests/unit/test_problem.py b/tests/unit/test_problem.py index 231fbdf..a56d316 100644 --- a/tests/unit/test_problem.py +++ b/tests/unit/test_problem.py @@ -9,8 +9,8 @@ from pinocchio import SE3 from unit.conftest import create_ur5_problem -class TestProblemAccessors(unittest.TestCase): +class TestProblemAccessors(unittest.TestCase): def test_steering_method_returns_object(self): problem, robot = create_ur5_problem() @@ -48,7 +48,6 @@ def test_config_validation_returns_object(self): class TestProblemDirectPath(unittest.TestCase): - def test_direct_path_without_validation(self): problem, robot = create_ur5_problem() q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) @@ -84,7 +83,6 @@ def test_direct_path_endpoints_match(self): class TestProblemConstraintProjection(unittest.TestCase): - def test_error_threshold_settable(self): problem, robot = create_ur5_problem() @@ -113,18 +111,13 @@ def test_apply_constraints_returns_tuple(self): class TestProblemTransformationConstraints(unittest.TestCase): - def test_create_transformation_constraint(self): problem, robot = create_ur5_problem() M = SE3.Identity() mask = [True, True, True, True, True, True] constraint = problem.createTransformationConstraint( - "test_constraint", - "", - "ur5/ee_fixed_joint", - M, - mask + "test_constraint", "", "ur5/ee_fixed_joint", M, mask ) self.assertIsNotNone(constraint) @@ -135,11 +128,7 @@ def test_create_relative_transformation_constraint(self): mask = [True, True, True, True, True, True] constraint = problem.createTransformationConstraint( - "relative_constraint", - "ur5/shoulder_link", - "ur5/ee_fixed_joint", - M, - mask + "relative_constraint", "ur5/shoulder_link", "ur5/ee_fixed_joint", M, mask ) self.assertIsNotNone(constraint) diff --git a/tests/unit/test_roadmap.py b/tests/unit/test_roadmap.py index 394c054..444075f 100644 --- a/tests/unit/test_roadmap.py +++ b/tests/unit/test_roadmap.py @@ -11,7 +11,6 @@ class TestRoadmap(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() diff --git a/tests/unit/test_security_margins.py b/tests/unit/test_security_margins.py index 5d350b3..77948c8 100644 --- a/tests/unit/test_security_margins.py +++ b/tests/unit/test_security_margins.py @@ -13,7 +13,6 @@ class TestSecurityMarginsCorrectness(unittest.TestCase): - def test_gripper_correctly_mapped_to_robot(self): sm, *_ = create_security_margins_instance() @@ -115,7 +114,9 @@ def get_margin(edge, idx1, idx2): self.assertEqual(margin, 0.0) self.assertTrue(found_free_loop, "Did not find 'Loop | f' edge") - self.assertTrue(found_grasp_edge, f"Did not find 'Loop | 0-0' edge. Available: {edge_names}") + self.assertTrue( + found_grasp_edge, f"Did not find 'Loop | 0-0' edge. Available: {edge_names}" + ) if free_margin is not None and grasp_margin is not None: self.assertNotEqual(free_margin, grasp_margin) diff --git a/tests/unit/test_steering_method.py b/tests/unit/test_steering_method.py index 9c3d631..0842c35 100644 --- a/tests/unit/test_steering_method.py +++ b/tests/unit/test_steering_method.py @@ -10,7 +10,6 @@ class TestSteeringMethod(unittest.TestCase): - @classmethod def setUpClass(cls): cls.problem, cls.robot = create_ur5_problem() From 3cf323f188b93d0c1ccb880044417ec3d0a563bc Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 10:24:23 +0100 Subject: [PATCH 5/6] fix typo and add missing test --- src/pyhpp/core/static_stability_constraint_factory.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index ab8c742..4ed4518 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -178,8 +178,8 @@ def createAlignedCOMStabilityConstraint( np.zeros(3), np.zeros(3), "", - x, - git[True] * 4, + x, + [True] * 4, ) created_constraints[result[-1]] = constraint From 7a8e07a0f157db98e26fd7661faac702db9ba4a9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 14 Jan 2026 09:26:27 +0000 Subject: [PATCH 6/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/static_stability_constraint_factory.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 4ed4518..cc9010b 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -178,7 +178,7 @@ def createAlignedCOMStabilityConstraint( np.zeros(3), np.zeros(3), "", - x, + x, [True] * 4, ) created_constraints[result[-1]] = constraint