From fbaecdec22d6ce2996fa983ba58365b6ad328c68 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 17:08:49 +0000 Subject: [PATCH 001/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.12.12 → v0.14.2](https://github.com/astral-sh/ruff-pre-commit/compare/v0.12.12...v0.14.2) - [github.com/pappasam/toml-sort: v0.24.2 → v0.24.3](https://github.com/pappasam/toml-sort/compare/v0.24.2...v0.24.3) - [github.com/pre-commit/mirrors-clang-format: v21.1.0 → v21.1.2](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.0...v21.1.2) --- .pre-commit-config.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 35972c41..5691e9a1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.12.12 + rev: v0.14.2 hooks: - id: ruff args: @@ -14,11 +14,11 @@ repos: hooks: - id: cmake-format - repo: https://github.com/pappasam/toml-sort - rev: v0.24.2 + rev: v0.24.3 hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.0 + rev: v21.1.2 hooks: - id: clang-format args: From 07588684a4f30a9b54aae47a79a08fc8dfbb82ad Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 08:55:48 +0200 Subject: [PATCH 002/109] Add relative-com constraint --- include/pyhpp/constraints/fwd.hh | 2 + src/CMakeLists.txt | 1 + src/pyhpp/constraints/bindings.cc | 1 + src/pyhpp/constraints/relative-com.cc | 73 +++++++++++++++++++++++++++ 4 files changed, 77 insertions(+) create mode 100644 src/pyhpp/constraints/relative-com.cc diff --git a/include/pyhpp/constraints/fwd.hh b/include/pyhpp/constraints/fwd.hh index 0cf1c510..b2162c40 100644 --- a/include/pyhpp/constraints/fwd.hh +++ b/include/pyhpp/constraints/fwd.hh @@ -42,6 +42,8 @@ void exposeExplicit(); void exposeLockedJoint(); void exposeHierarchicalIterativeSolver(); void exposeBySubstitution(); +void exposeRelativeCom(); + } // namespace constraints } // namespace pyhpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 31c06b4b..6aa06048 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -94,6 +94,7 @@ add_python_library( pyhpp/constraints/iterative-solver.cc pyhpp/constraints/by-substitution.cc pyhpp/constraints/locked-joint.cc + pyhpp/constraints/relative-com.cc pyhpp/constraints/bindings.cc LINK_LIBRARIES hpp-constraints::hpp-constraints) diff --git a/src/pyhpp/constraints/bindings.cc b/src/pyhpp/constraints/bindings.cc index cbe1c5fb..dcc26dba 100644 --- a/src/pyhpp/constraints/bindings.cc +++ b/src/pyhpp/constraints/bindings.cc @@ -49,4 +49,5 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::constraints::exposeExplicitConstraintSet(); pyhpp::constraints::exposeHierarchicalIterativeSolver(); pyhpp::constraints::exposeBySubstitution(); + pyhpp::constraints::exposeRelativeCom(); } diff --git a/src/pyhpp/constraints/relative-com.cc b/src/pyhpp/constraints/relative-com.cc new file mode 100644 index 00000000..a6a5c577 --- /dev/null +++ b/src/pyhpp/constraints/relative-com.cc @@ -0,0 +1,73 @@ +// +// Copyright (c) 2025 CNRS +// Authors: Paul Sardin +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include +using namespace boost::python; + +namespace pyhpp { +namespace constraints { +using namespace hpp::constraints; + +static RelativeComPtr_t create1( std::string& name, + const DevicePtr_t& robot, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + CenterOfMassComputationPtr_t comc = CenterOfMassComputation::create(robot); + comc->add(robot->rootJoint()); + return RelativeCom::create(name, robot, comc, joint, reference, mask); +} +static RelativeComPtr_t create2(const DevicePtr_t& robot, + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + return RelativeCom::create("RelativeCom", robot, comc, joint, reference, mask); +} +static RelativeComPtr_t create3(const std::string& name, + const DevicePtr_t& robot, + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + return RelativeCom::create(name, robot, comc, joint, reference, mask); +} + +void exposeRelativeCom() { +class_, boost::noncopyable>("RelativeCom", no_init) +.def("create", &create1) + .def("create", &create2) + .def("create", &create3).staticmethod("create"); +} +} // namespace constraints +} // namespace pyhpp From 57c3350bf79f369ca1abb32c3102ad2af96bb42b Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 08:59:37 +0200 Subject: [PATCH 003/109] Add ConstraintResult to problem --- include/pyhpp/core/problem.hh | 14 +++++++++++++- src/pyhpp/core/problem.cc | 5 +++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 5e2f1423..dfee8823 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -56,11 +56,23 @@ typedef hpp::core::ProblemTargetPtr_t ProblemTargetPtr_t; typedef hpp::core::DistancePtr_t DistancePtr_t; typedef hpp::core::value_type value_type; typedef hpp::core::size_type size_type; +typedef hpp::core::Configuration_t Configuration_t; + + +struct ConstraintResult { + bool success; + Configuration_t configuration; + value_type error; + + ConstraintResult() : success(false), configuration(), error(0.0) {} + ConstraintResult(bool s, const Configuration_t& config, value_type err) + : success(s), configuration(config), error(err) {} +}; + // Wrapper class for hpp::core::Problem struct Problem { hpp::core::ProblemPtr_t obj; - Problem(const DevicePtr_t& robot); Problem(hpp::core::ProblemPtr_t problemPtr) : obj(problemPtr) {} diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 2b1f4045..80b56c46 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -271,6 +271,11 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, addGoalConfig) .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs); register_problem_converters(); + + class_("ConstraintResult") + .def_readwrite("success", &ConstraintResult::success) + .def_readwrite("configuration", &ConstraintResult::configuration) + .def_readwrite("error", &ConstraintResult::error); } } // namespace core From caf17d1f3a041fc2cde8c1116947b7424acce51e Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:00:41 +0200 Subject: [PATCH 004/109] Improve extract_vector utility with better error handling --- include/pyhpp/util.hh | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/include/pyhpp/util.hh b/include/pyhpp/util.hh index 5756a0a2..0033c029 100644 --- a/include/pyhpp/util.hh +++ b/include/pyhpp/util.hh @@ -86,11 +86,20 @@ struct VectorOfPtr { return *v[i]; } }; - -template +template std::vector extract_vector(boost::python::list py_list) { - return std::vector(boost::python::stl_input_iterator(py_list), - boost::python::stl_input_iterator()); + std::vector result; + result.reserve(boost::python::len(py_list)); + + for (int i = 0; i < boost::python::len(py_list); ++i) { + boost::python::extract extractor(py_list[i]); + if (extractor.check()) { + result.push_back(extractor()); + } else { + throw std::runtime_error("Failed to extract element"); + } + } + return result; } template boost::python::list to_python_list(const std::vector& vec) { From 10ff231f2d8b25d7761e70892dac14f2ccc88c6a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:07:20 +0200 Subject: [PATCH 005/109] Add partial center of mass computation functionality --- include/pyhpp/core/problem.hh | 6 +++++ src/pyhpp/core/problem.cc | 46 ++++++++++++++++++++++++++++++++++- 2 files changed, 51 insertions(+), 1 deletion(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index dfee8823..faf46e91 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -114,6 +114,12 @@ struct Problem { bool isManipulationProblem() const { return bool(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Problem, obj)); } + + //Constraints utility functions + + void addPartialCom(const std::string& name, boost::python::list pyjointNames); + hpp::pinocchio::vector3_t getPartialCom(const std::string& name); + std::map centerOfMassComputations; }; } // namespace core diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 80b56c46..5fdee183 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -39,9 +39,17 @@ #include #include #include +#include #include #include #include +#include +#include +#include +#include +#include +#include + namespace pyhpp { namespace core { @@ -140,6 +148,38 @@ void Problem::addGoalConfig(ConfigurationIn_t config) { void Problem::resetGoalConfigs() { obj->resetGoalConfigs(); } +void Problem::addPartialCom(const std::string& name, boost::python::list pyjointNames) { + try { + hpp::pinocchio::CenterOfMassComputationPtr_t comc = + hpp::pinocchio::CenterOfMassComputation::create(robot()); + auto jointNames = extract_vector(pyjointNames); + + for (long unsigned int i = 0; i < jointNames.size(); ++i) { + std::string name(jointNames[i]); + hpp::pinocchio::JointPtr_t j = robot()->getJointByName(name); + if (!j) throw std::logic_error("One joint not found."); + comc->add(j); + } + centerOfMassComputations[name] = comc; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::pinocchio::vector3_t Problem::getPartialCom(const std::string& name) { + try { + if (!centerOfMassComputations[name]) { + throw std::logic_error("Partial COM " + name + " not found."); + } + hpp::pinocchio::CenterOfMassComputationPtr_t comc = + centerOfMassComputations[name]; + comc->compute(hpp::pinocchio::COM); + return comc->com(); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -269,7 +309,11 @@ void exposeProblem() { (arg("configurationShooter"))) .PYHPP_DEFINE_METHOD(Problem, initConfig) .PYHPP_DEFINE_METHOD(Problem, addGoalConfig) - .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs); + .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs) + .PYHPP_DEFINE_METHOD(Problem, addPartialCom) + .PYHPP_DEFINE_METHOD(Problem, getPartialCom) + ; + register_problem_converters(); class_("ConstraintResult") From 6cf05b484bbae7a6b2bd941d87150c4556bfe796 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:10:43 +0200 Subject: [PATCH 006/109] Add constraint creation methods for transformations and relative COM --- include/pyhpp/core/problem.hh | 16 +++++ src/pyhpp/core/problem.cc | 123 ++++++++++++++++++++++++++++++++++ 2 files changed, 139 insertions(+) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index faf46e91..1bc22e05 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -120,6 +120,22 @@ struct Problem { void addPartialCom(const std::string& name, boost::python::list pyjointNames); hpp::pinocchio::vector3_t getPartialCom(const std::string& name); std::map centerOfMassComputations; + hpp::constraints::ImplicitPtr_t createRelativeComConstraint(const char* constraintName, + const char* comName, + const char* jointName, + hpp::pinocchio::vector3_t point, + boost::python::list mask); + + hpp::constraints::ImplicitPtr_t createTransformationConstraint( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M, + boost::python::list mask); + + hpp::constraints::ImplicitPtr_t createTransformationConstraint2( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M1, + const hpp::pinocchio::Transform3s& M2, const boost::python::list mask); + }; } // namespace core diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 5fdee183..bb8ea85f 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -180,6 +180,126 @@ hpp::pinocchio::vector3_t Problem::getPartialCom(const std::string& name) { } } +static int numberOfTrue(const std::vector& mask) { + std::size_t res = 0; + for (std::size_t i = 0; i < mask.size(); ++i) + if (mask[i]) ++res; + return (int)res; +} + +hpp::constraints::ImplicitPtr_t Problem::createRelativeComConstraint( + const char* constraintName, const char* comName, const char* jointName, + hpp::pinocchio::vector3_t point, boost::python::list mask) { + hpp::pinocchio::JointPtr_t joint; + hpp::pinocchio::CenterOfMassComputationPtr_t comc; + + try { + joint = robot()->getJointByName(jointName); + auto m = extract_vector(mask); + // Test whether joint1 is world frame + std::string name(constraintName), comN(comName); + if (comN.compare("") == 0) { + return hpp::constraints::Implicit::create( + hpp::constraints::RelativeCom::create(name, robot(), joint, point, + m), + numberOfTrue(m) * hpp::constraints::EqualToZero); + } else { + if (!centerOfMassComputations[comN]) + throw std::logic_error("Partial COM not found."); + comc = centerOfMassComputations[comN]; + return hpp::constraints::Implicit::create( + hpp::constraints::RelativeCom::create(name, robot(), comc, joint, + point, m), + numberOfTrue(m) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M, + boost::python::list m) { + try { + if (!robot()) throw std::logic_error("No robot loaded"); + + using hpp::constraints::GenericTransformation; + using hpp::constraints::Implicit; + using hpp::constraints::OrientationBit; + using hpp::constraints::PositionBit; + + std::string name(constraintName); + + hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); + auto mask = extract_vector(m); + + const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M; + + if (joint1Name && std::strlen(joint1Name) > 0) { + // Relative transformation constraint between two frames + hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); + const hpp::pinocchio::Transform3s ref1 = + f1.positionInParentJoint() * hpp::pinocchio::Transform3s::Identity(); + auto func = GenericTransformation::create( + name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } else { + // Absolute transformation of frame/joint2 to M in world frame + const hpp::pinocchio::Transform3s Id = + hpp::pinocchio::Transform3s::Identity(); + auto func = GenericTransformation::create( + name, robot(), f2.joint(), ref2, Id, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M1, + const hpp::pinocchio::Transform3s& M2, const boost::python::list m) { + try { + if (!robot()) throw std::logic_error("No robot loaded"); + + using hpp::constraints::GenericTransformation; + using hpp::constraints::Implicit; + using hpp::constraints::OrientationBit; + using hpp::constraints::PositionBit; + auto mask = extract_vector(m); + + std::string name(constraintName); + + hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); + const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M2; + + if (joint1Name && std::strlen(joint1Name) > 0) { + hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); + const hpp::pinocchio::Transform3s ref1 = + f1.positionInParentJoint() * M1; + auto func = GenericTransformation::create( + name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } else { + const hpp::pinocchio::Transform3s Id = + hpp::pinocchio::Transform3s::Identity(); + auto func = GenericTransformation::create( + name, robot(), f2.joint(), ref2, Id, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -312,6 +432,9 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs) .PYHPP_DEFINE_METHOD(Problem, addPartialCom) .PYHPP_DEFINE_METHOD(Problem, getPartialCom) + .PYHPP_DEFINE_METHOD(Problem, createRelativeComConstraint) + .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint) + .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint2) ; register_problem_converters(); From 704edf4c6185d53a0b464e30e854e1195f73ce28 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:14:36 +0200 Subject: [PATCH 007/109] Add constraint application and configuration validation --- include/pyhpp/core/problem.hh | 5 ++++ src/pyhpp/core/problem.cc | 56 +++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 1bc22e05..ca34f536 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -136,6 +136,11 @@ struct Problem { const char* joint2Name, const hpp::pinocchio::Transform3s& M1, const hpp::pinocchio::Transform3s& M2, const boost::python::list mask); + void setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, + bool constant); + + ConstraintResult applyConstraints(ConfigurationIn_t config); + boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); }; } // namespace core diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index bb8ea85f..6311030c 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -300,6 +300,59 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( } } +void Problem::setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, + bool constant) { + try { + if (constant) { + hpp::constraints::ComparisonTypes_t eqtypes(constraint->function().outputDerivativeSize(), hpp::constraints::EqualToZero); + constraint->comparisonType(eqtypes); + } else { + hpp::constraints::ComparisonTypes_t eqtypes(constraint->function().outputDerivativeSize(), hpp::constraints::Equality); + constraint->comparisonType(eqtypes); + + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +ConstraintResult Problem::applyConstraints(ConfigurationIn_t config) { + bool success = false; + double residualError = 0.0; + try { + Configuration_t output = config; + success = constraints_->apply(output); + if (hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector()) { + residualError = configProjector->residualError(); + } + ConstraintResult result(success, output, residualError); + return result; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + + +boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { + try { + Configuration_t config = dofArray; + hpp::core::ValidationReportPtr_t validationReport; + bool validity = obj->configValidations()->validate(config, validationReport); + + std::string report; + if (validity) { + report = ""; + } else { + std::ostringstream oss; + oss << *validationReport; + report = oss.str(); + } + return boost::python::make_tuple(validity, report); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -435,6 +488,9 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, createRelativeComConstraint) .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint) .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint2) + .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) + .PYHPP_DEFINE_METHOD(Problem, applyConstraints) + .PYHPP_DEFINE_METHOD(Problem, isConfigValid) ; register_problem_converters(); From ab208f8d74b64368bb41c6424b25792ee91a0ca8 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:16:09 +0200 Subject: [PATCH 008/109] Add constraint and config-projector management to pyhpp problem --- include/pyhpp/core/problem.hh | 13 +++++ src/pyhpp/core/problem.cc | 95 ++++++++++++++++++++++++++++++++++- 2 files changed, 107 insertions(+), 1 deletion(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index ca34f536..90bdffd0 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -141,6 +141,19 @@ struct Problem { ConstraintResult applyConstraints(ConfigurationIn_t config); boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); + void addNumericalConstraintsToConfigProjector1(const char* configProjName, + boost::python::list constraints, + boost::python::list priorities); + void addNumericalConstraintsToConfigProjector2(const char* configProjName, + boost::python::list constraints); + hpp::constraints::ImplicitPtr_t createComBetweenFeet( + const char* constraintName, const char* comName, const char* jointLName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); + hpp::core::ConstraintSetPtr_t constraints_; + value_type errorThreshold_; + size_type maxIterProjection_; }; } // namespace core diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 6311030c..2bf08ca8 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -56,7 +56,9 @@ namespace core { using namespace boost::python; Problem::Problem(const DevicePtr_t& robot) - : obj(hpp::core::Problem::create(robot)) {} + : obj(hpp::core::Problem::create(robot)), errorThreshold_(1e-4), maxIterProjection_(20) { + constraints_ = hpp::core::ConstraintSet::create(robot, "Default constraint set"); + } const DevicePtr_t& Problem::robot() const { return obj->robot(); } @@ -353,6 +355,92 @@ boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { throw std::logic_error(exc.what()); } } + +void Problem::addNumericalConstraintsToConfigProjector1(const char* configProjName, + boost::python::list constraints, + boost::python::list priorities) { + try { + auto constraintsVec = extract_vector(constraints); + auto prioritiesVec = extract_vector(priorities); + hpp::core::ConfigProjectorPtr_t configProjector = constraints_->configProjector(); + for (unsigned int i = 0; i < constraintsVec.size(); ++i) { + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } + configProjector->add(constraintsVec[i], prioritiesVec[i]); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::addNumericalConstraintsToConfigProjector2(const char* configProjName, + boost::python::list constraints) { + try { + auto constraintsVec = extract_vector(constraints); + hpp::core::ConfigProjectorPtr_t configProjector = constraints_->configProjector(); + for (unsigned int i = 0; i < constraintsVec.size(); ++i) { + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } + configProjector->add(constraintsVec[i], 0); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createComBetweenFeet( + const char* constraintName, const char* comName, const char* jointLName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask) { + + if (!robot()) throw std::logic_error("No robot loaded"); + + try { + hpp::pinocchio::JointPtr_t jointL, jointR, jointRef; + hpp::pinocchio::CenterOfMassComputationPtr_t comc; + + auto m = extract_vector(mask); + + jointL = robot()->getJointByName(jointLName); + jointR = robot()->getJointByName(jointRName); + + if (std::string(jointRefName) == std::string("")) + jointRef = robot()->rootJoint(); + else + jointRef = robot()->getJointByName(jointRefName); + + std::string name(constraintName), comN(comName); + + hpp::constraints::ComparisonTypes_t comps(2, hpp::constraints::EqualToZero); + comps.push_back(hpp::constraints::Superior); + comps.push_back(hpp::constraints::Inferior); + + if (comN.compare("") == 0) { + return hpp::constraints::Implicit::create( + hpp::constraints::ComBetweenFeet::create( + name, robot(), jointL, jointR, pointL, pointR, jointRef, pointRef, m), + comps); + } else { + if (!centerOfMassComputations[comN]) + throw std::logic_error("Partial COM " + comN + " not found."); + comc = centerOfMassComputations[comN]; + return hpp::constraints::Implicit::create( + hpp::constraints::ComBetweenFeet::create( + name, robot(), comc, jointL, jointR, pointL, pointR, jointRef, pointRef, m), + comps); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -491,6 +579,11 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) .PYHPP_DEFINE_METHOD(Problem, applyConstraints) .PYHPP_DEFINE_METHOD(Problem, isConfigValid) + .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector1) + .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector2) + .def_readwrite("errorThreshold", &Problem::errorThreshold_) + .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) + .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet) ; register_problem_converters(); From 8f23333845649d63d55d8a8343690113f7d2d7e9 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:17:24 +0200 Subject: [PATCH 009/109] Add device utility methods: getCenterOfMass, rankInConfiguration, getJointsPosition --- src/pyhpp/pinocchio/device.cc | 78 +++++++++++++++++++++++++++++++++-- 1 file changed, 75 insertions(+), 3 deletions(-) diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 5c1fc1cb..717c161f 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -33,8 +33,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -91,11 +91,20 @@ Transform3s getObjectPositionInJoint(const GripperPtr_t& gripper) { return res; } +static hpp::pinocchio::vector3_t getCenterOfMass(Device& d) { + try { + d.computeForwardKinematics(); + return d.positionCenterOfMass(); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + typedef hpp::pinocchio::Frame Frame; typedef hpp::pinocchio::JointPtr_t JointPtr_t; static void setJointBounds(Device& device, const char* jointName, - boost::python::list py_jointBounds) { + boost::python::list py_jointBounds) { Frame frame = device.getFrameByName(jointName); JointPtr_t joint = frame.joint(); auto jointBounds = extract_vector(py_jointBounds); @@ -122,6 +131,65 @@ static void setJointBounds(Device& device, const char* jointName, } } +static boost::python::dict rankInConfiguration(Device& device) { + boost::python::dict rank_dict; + try { + auto joint_names = device.model().names; + for (const auto& joint_name : joint_names) { + Frame frame = device.getFrameByName(joint_name.c_str()); + if (!frame.isFixed()) { + JointPtr_t joint = frame.joint(); + if (joint) { + rank_dict[joint_name] = joint->rankInConfiguration(); + } + } + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } + return rank_dict; +} + +typedef hpp::pinocchio::FrameIndex FrameIndex; +typedef hpp::pinocchio::SE3 SE3; + +static boost::python::list getJointsPosition(Device& device, + const Configuration_t& dofArray, + const boost::python::list& jointNames) { + try { + device.currentConfiguration(dofArray); + device.computeForwardKinematics(); + device.computeFramesForwardKinematics(); + + const Model& model(device.model()); + const Data& data(device.data()); + boost::python::list transforms; + for (Py_ssize_t i = 0; i < static_cast(boost::python::len(jointNames)); ++i) { + std::string n = boost::python::extract(jointNames[i]); + if (!model.existFrame(n)) { + throw std::logic_error("Robot has no frame with name " + n); + } + FrameIndex joint = model.getFrameId(n); + if (model.frames.size() <= (std::size_t)joint) + throw std::logic_error("Frame index of joint " + n + " out of bounds: " + std::to_string(joint)); + const SE3& M = data.oMf[joint]; + Transform3s::Quaternion q(M.rotation()); + boost::python::list t; + t.append(M.translation()[0]); + t.append(M.translation()[1]); + t.append(M.translation()[2]); + t.append(q.x()); + t.append(q.y()); + t.append(q.z()); + t.append(q.w()); + transforms.append(t); + } + return transforms; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + void exposeGripper() { class_("Gripper", no_init) .def("create", &Gripper::create) @@ -177,7 +245,11 @@ void exposeDevice() { .def("computeFramesForwardKinematics", &Device::computeFramesForwardKinematics) .def("updateGeometryPlacements", &Device::updateGeometryPlacements) - .def("setJointBounds", &setJointBounds); + .add_property("rankInConfiguration", &rankInConfiguration) + .def("setJointBounds", &setJointBounds) + .def("getCenterOfMass", &getCenterOfMass) + .def("getJointsPosition", &getJointsPosition); + } } // namespace pinocchio } // namespace pyhpp From 3d924e8f3a4ca0ecf80c7fb2de06f517c14d9145 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:17:46 +0200 Subject: [PATCH 010/109] Add static stability constraint factory --- src/CMakeLists.txt | 1 + .../static_stability_constraint_factory.py | 219 ++++++++++++++++++ 2 files changed, 220 insertions(+) create mode 100644 src/pyhpp/core/static_stability_constraint_factory.py diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6aa06048..ebf76193 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -69,6 +69,7 @@ endmacro() python_install_on_site(pyhpp __init__.py) python_install_on_site(pyhpp/manipulation constraint_graph_factory.py) +python_install_on_site(pyhpp/core static_stability_constraint_factory.py) add_python_library( pyhpp/pinocchio diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py new file mode 100644 index 00000000..98e3a481 --- /dev/null +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are +# met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. + +from hpp import Transform +from pinocchio import SE3 +from pyhpp.constraints import RelativeCom, Implicit, Transformation, RelativeTransformation, ComparisonType, ComparisonTypes +# # This class provides tools to create static stability constraints +class StaticStabilityConstraintsFactory: + def __init__( + self, + problem + ): + self.problem = problem + self.robot = problem.robot() + def _getCOM(self, com): + from numpy import array + + if com == "": + return array(self.robot.getCenterOfMass()) + else: + return 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 + # # \param comName name of the PartialCOM. Put "" for + # # a full COM computations. + # # \param leftAnkle, rightAnkle: names of the ankle joints. + # # \param q0 input configuration for computing constraint reference, + # # \return a list of the names of the created constraints + + def createSlidingStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0 + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + # COM wrt left ankle frame + xloc = Ml.inverse().transform(x) + result.append(prefix + "relative-com") + constraint = self.problem.createRelativeComConstraint( + result[-1], comName, leftAnkle, xloc, [True] * 3 + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "relative-pose") + rel_transform = Mr.inverse() * Ml + constraint = self.problem.createTransformationConstraint2( + result[-1], + leftAnkle, + rightAnkle, + SE3.Identity(), + SE3(rel_transform.quaternion.toRotationMatrix(), rel_transform.translation), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [False, False, True, True, True, False], + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "pose-left-foot-complement") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [True, True, False, False, False, True], + ) + self.problem.setConstantRightHandSide(constraint, False) + created_constraints[result[-1]] = constraint + + return created_constraints + +# # # Create static stability constraints where the feet are fixed on the ground, + def createStaticStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0, maskCom=(True,) * 3 + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + + # COM wrt left ankle frame + xloc = Ml.inverse().transform(x) + result.append(prefix + "relative-com") + constraint = self.problem.createRelativeComConstraint( + result[-1], comName, leftAnkle, xloc, maskCom + ) + created_constraints[result[-1]] = constraint + + # Pose of the left foot + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + # Pose of the right foot + result.append(prefix + "pose-right-foot") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + rightAnkle, + SE3(Mr.quaternion.toRotationMatrix(), Mr.translation), + SE3.Identity(), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + return created_constraints + + + def createAlignedCOMStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0, sliding + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + + # 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], + "", + x, + [True] * 4, + ) + created_constraints[result[-1]] = constraint + + if sliding: + mask = [False, False, True, True, True, False] + else: + mask = [True] * 6 + + # Pose of the right foot + result.append(prefix + "pose-right-foot") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + rightAnkle, + SE3(Mr.quaternion.toRotationMatrix(), Mr.translation), + SE3.Identity(), + mask + ) + created_constraints[result[-1]] = constraint + + # Pose of the left foot + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint2( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + mask + ) + created_constraints[result[-1]] = constraint + + return created_constraints \ No newline at end of file From 0b1ce7be44874887d0547e48d316ea463c9a177c Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:18:04 +0200 Subject: [PATCH 011/109] Update manipulation graph to use core ConstraintResult --- src/pyhpp/manipulation/graph.cc | 18 +++++++---------- src/pyhpp/manipulation/graph.hh | 36 ++++++++++++++------------------- 2 files changed, 22 insertions(+), 32 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index eb5c3694..2b8f1213 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -30,6 +30,7 @@ #include <../src/pyhpp/manipulation/device.hh> #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/problem.hh> +#include "pyhpp/core/problem.hh" #include #include #include @@ -729,7 +730,7 @@ boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget( // Constraint application // ============================================================================= -ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, +pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(state->obj->configConstraint()); @@ -741,10 +742,10 @@ ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, constraint->configProjector()) { residualError = configProjector->residualError(); } - return ConstraintResult(success, output, residualError); + return pyhpp::core::ConstraintResult(success, output, residualError); } -ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, +pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; @@ -758,10 +759,10 @@ ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, success = constraint->apply(output); residualError = constraint->configProjector()->residualError(); } - return ConstraintResult(success, output, residualError); + return pyhpp::core::ConstraintResult(success, output, residualError); } -ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, +pyhpp::core::ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; @@ -778,7 +779,7 @@ ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, residualError = configProjector->residualError(); } - return ConstraintResult(success, output, residualError); + return pyhpp::core::ConstraintResult(success, output, residualError); } // ============================================================================= @@ -1330,11 +1331,6 @@ void exposeGraph() { // Initialization .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); - - class_("ConstraintResult") - .def_readwrite("success", &ConstraintResult::success) - .def_readwrite("configuration", &ConstraintResult::configuration) - .def_readwrite("error", &ConstraintResult::error); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index baa42be8..9f17e851 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -33,6 +33,13 @@ #include #include + +namespace pyhpp { +namespace core { +struct ConstraintResult; +} +} + namespace pyhpp { namespace manipulation { @@ -52,17 +59,6 @@ typedef hpp::manipulation::ConstraintsAndComplements_t ConstraintsAndComplements_t; typedef hpp::manipulation::ConstraintAndComplement_t ConstraintAndComplement_t; -/// Result structure for constraint operations -struct ConstraintResult { - bool success; - Configuration_t configuration; - value_type error; - - ConstraintResult() : success(false), configuration(), error(0.0) {} - ConstraintResult(bool s, const Configuration_t& config, value_type err) - : success(s), configuration(config), error(err) {} -}; - /// Python wrapper for State struct PyWState { StatePtr_t obj; @@ -201,16 +197,6 @@ struct PyWGraph { const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig, ConfigurationIn_t config) const; - // Constraint application - ConstraintResult applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input); - ConstraintResult applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); - ConstraintResult generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); - // Level set edges void addLevelSetFoliation(PyWEdgePtr_t edge, const boost::python::list& condNC, @@ -224,6 +210,14 @@ struct PyWGraph { void removeCollisionPairFromTransition(PyWEdgePtr_t edge, const char* joint1, const char* joint2); + pyhpp::core::ConstraintResult applyStateConstraints(PyWStatePtr_t state, + ConfigurationIn_t input); + pyhpp::core::ConstraintResult applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); + pyhpp::core::ConstraintResult generateTargetConfig(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); // Subgraph management void createSubGraph(const char* subgraphName, hpp::core::RoadmapPtr_t roadmap); From a4b5a4edfd1432441d5306b92f99a2206207e0d8 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 21 Oct 2025 09:19:45 +0200 Subject: [PATCH 012/109] Add pyrene-on-the-ground benchmark using hpp-python --- tests/benchmarks/pyrene-on-the-ground.py | 126 +++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100755 tests/benchmarks/pyrene-on-the-ground.py diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py new file mode 100755 index 00000000..06617e7a --- /dev/null +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import pi +import numpy as np +import datetime as dt + +from pyhpp.core.static_stability_constraint_factory import StaticStabilityConstraintsFactory +from pyhpp.core import Problem, DiffusingPlanner, createDichotomy, createProgressiveProjector, Straight +from pyhpp.constraints import LockedJoint, Implicit, Transformation, ComparisonType, ComparisonTypes +from pyhpp.pinocchio import Device, urdf +from pinocchio import SE3 +parser = ArgumentParser() +parser.add_argument('-N', default=20, 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.create('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) +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.create(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.create(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 = createDichotomy(robot, 0) +problem.steeringMethod = Straight(problem) +problem.pathProjector = createProgressiveProjector( + 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 = problem.applyConstraints(q1) +assert result.success +q1proj = result.configuration +valid = problem.isConfigValid(q1proj) +assert valid[0] +result = problem.applyConstraints(q2) +assert result.success +q2proj = result.configuration +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}") \ No newline at end of file From 67c0d2073bf23798bb22d2a91bf02066fd0efaa0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 21 Oct 2025 07:44:00 +0000 Subject: [PATCH 013/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/pyhpp/core/problem.hh | 45 ++-- include/pyhpp/util.hh | 24 +- src/pyhpp/constraints/relative-com.cc | 42 ++-- src/pyhpp/core/problem.cc | 188 ++++++++------- .../static_stability_constraint_factory.py | 34 ++- src/pyhpp/manipulation/graph.cc | 16 +- src/pyhpp/manipulation/graph.hh | 13 +- src/pyhpp/pinocchio/device.cc | 53 +++-- tests/benchmarks/pyrene-on-the-ground.py | 223 ++++++++++++++++-- 9 files changed, 410 insertions(+), 228 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 90bdffd0..f8cc1999 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -58,7 +58,6 @@ typedef hpp::core::value_type value_type; typedef hpp::core::size_type size_type; typedef hpp::core::Configuration_t Configuration_t; - struct ConstraintResult { bool success; Configuration_t configuration; @@ -69,7 +68,6 @@ struct ConstraintResult { : success(s), configuration(config), error(err) {} }; - // Wrapper class for hpp::core::Problem struct Problem { hpp::core::ProblemPtr_t obj; @@ -115,42 +113,41 @@ struct Problem { return bool(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Problem, obj)); } - //Constraints utility functions + // Constraints utility functions void addPartialCom(const std::string& name, boost::python::list pyjointNames); hpp::pinocchio::vector3_t getPartialCom(const std::string& name); - std::map centerOfMassComputations; - hpp::constraints::ImplicitPtr_t createRelativeComConstraint(const char* constraintName, - const char* comName, - const char* jointName, - hpp::pinocchio::vector3_t point, - boost::python::list mask); + std::map + centerOfMassComputations; + hpp::constraints::ImplicitPtr_t createRelativeComConstraint( + const char* constraintName, const char* comName, const char* jointName, + hpp::pinocchio::vector3_t point, boost::python::list mask); hpp::constraints::ImplicitPtr_t createTransformationConstraint( - const char* constraintName, const char* joint1Name, - const char* joint2Name, const hpp::pinocchio::Transform3s& M, - boost::python::list mask); + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M, + boost::python::list mask); hpp::constraints::ImplicitPtr_t createTransformationConstraint2( - const char* constraintName, const char* joint1Name, - const char* joint2Name, const hpp::pinocchio::Transform3s& M1, - const hpp::pinocchio::Transform3s& M2, const boost::python::list mask); + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M1, + const hpp::pinocchio::Transform3s& M2, const boost::python::list mask); void setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, bool constant); ConstraintResult applyConstraints(ConfigurationIn_t config); boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); - void addNumericalConstraintsToConfigProjector1(const char* configProjName, - boost::python::list constraints, - boost::python::list priorities); - void addNumericalConstraintsToConfigProjector2(const char* configProjName, - boost::python::list constraints); + void addNumericalConstraintsToConfigProjector1( + const char* configProjName, boost::python::list constraints, + boost::python::list priorities); + void addNumericalConstraintsToConfigProjector2( + const char* configProjName, boost::python::list constraints); hpp::constraints::ImplicitPtr_t createComBetweenFeet( - const char* constraintName, const char* comName, const char* jointLName, - const char* jointRName, const hpp::pinocchio::vector3_t& pointL, - const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, - const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); + const char* constraintName, const char* comName, const char* jointLName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); hpp::core::ConstraintSetPtr_t constraints_; value_type errorThreshold_; size_type maxIterProjection_; diff --git a/include/pyhpp/util.hh b/include/pyhpp/util.hh index 0033c029..ce461d4c 100644 --- a/include/pyhpp/util.hh +++ b/include/pyhpp/util.hh @@ -86,20 +86,20 @@ struct VectorOfPtr { return *v[i]; } }; -template +template std::vector extract_vector(boost::python::list py_list) { - std::vector result; - result.reserve(boost::python::len(py_list)); - - for (int i = 0; i < boost::python::len(py_list); ++i) { - boost::python::extract extractor(py_list[i]); - if (extractor.check()) { - result.push_back(extractor()); - } else { - throw std::runtime_error("Failed to extract element"); - } + std::vector result; + result.reserve(boost::python::len(py_list)); + + for (int i = 0; i < boost::python::len(py_list); ++i) { + boost::python::extract extractor(py_list[i]); + if (extractor.check()) { + result.push_back(extractor()); + } else { + throw std::runtime_error("Failed to extract element"); } - return result; + } + return result; } template boost::python::list to_python_list(const std::vector& vec) { diff --git a/src/pyhpp/constraints/relative-com.cc b/src/pyhpp/constraints/relative-com.cc index a6a5c577..44b003a8 100644 --- a/src/pyhpp/constraints/relative-com.cc +++ b/src/pyhpp/constraints/relative-com.cc @@ -28,46 +28,48 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include #include #include -#include -#include using namespace boost::python; namespace pyhpp { namespace constraints { using namespace hpp::constraints; -static RelativeComPtr_t create1( std::string& name, - const DevicePtr_t& robot, - const JointPtr_t& joint, - const vector3_t reference, - std::vector mask) { +static RelativeComPtr_t create1(std::string& name, const DevicePtr_t& robot, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { CenterOfMassComputationPtr_t comc = CenterOfMassComputation::create(robot); comc->add(robot->rootJoint()); return RelativeCom::create(name, robot, comc, joint, reference, mask); } static RelativeComPtr_t create2(const DevicePtr_t& robot, - const CenterOfMassComputationPtr_t& comc, - const JointPtr_t& joint, - const vector3_t reference, - std::vector mask) { - return RelativeCom::create("RelativeCom", robot, comc, joint, reference, mask); + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + return RelativeCom::create("RelativeCom", robot, comc, joint, reference, + mask); } static RelativeComPtr_t create3(const std::string& name, - const DevicePtr_t& robot, - const CenterOfMassComputationPtr_t& comc, - const JointPtr_t& joint, - const vector3_t reference, - std::vector mask) { + const DevicePtr_t& robot, + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { return RelativeCom::create(name, robot, comc, joint, reference, mask); } void exposeRelativeCom() { -class_, boost::noncopyable>("RelativeCom", no_init) -.def("create", &create1) + class_, + boost::noncopyable>("RelativeCom", no_init) + .def("create", &create1) .def("create", &create2) - .def("create", &create3).staticmethod("create"); + .def("create", &create3) + .staticmethod("create"); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 2bf08ca8..b7104fb5 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -31,7 +31,12 @@ #include "pyhpp/core/problem.hh" #include +#include +#include +#include +#include #include +#include #include #include #include @@ -39,16 +44,11 @@ #include #include #include -#include #include #include -#include #include -#include -#include -#include -#include #include +#include namespace pyhpp { namespace core { @@ -56,9 +56,12 @@ namespace core { using namespace boost::python; Problem::Problem(const DevicePtr_t& robot) - : obj(hpp::core::Problem::create(robot)), errorThreshold_(1e-4), maxIterProjection_(20) { - constraints_ = hpp::core::ConstraintSet::create(robot, "Default constraint set"); - } + : obj(hpp::core::Problem::create(robot)), + errorThreshold_(1e-4), + maxIterProjection_(20) { + constraints_ = + hpp::core::ConstraintSet::create(robot, "Default constraint set"); +} const DevicePtr_t& Problem::robot() const { return obj->robot(); } @@ -150,7 +153,8 @@ void Problem::addGoalConfig(ConfigurationIn_t config) { void Problem::resetGoalConfigs() { obj->resetGoalConfigs(); } -void Problem::addPartialCom(const std::string& name, boost::python::list pyjointNames) { +void Problem::addPartialCom(const std::string& name, + boost::python::list pyjointNames) { try { hpp::pinocchio::CenterOfMassComputationPtr_t comc = hpp::pinocchio::CenterOfMassComputation::create(robot()); @@ -202,8 +206,7 @@ hpp::constraints::ImplicitPtr_t Problem::createRelativeComConstraint( std::string name(constraintName), comN(comName); if (comN.compare("") == 0) { return hpp::constraints::Implicit::create( - hpp::constraints::RelativeCom::create(name, robot(), joint, point, - m), + hpp::constraints::RelativeCom::create(name, robot(), joint, point, m), numberOfTrue(m) * hpp::constraints::EqualToZero); } else { if (!centerOfMassComputations[comN]) @@ -220,9 +223,8 @@ hpp::constraints::ImplicitPtr_t Problem::createRelativeComConstraint( } hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( - const char* constraintName, const char* joint1Name, - const char* joint2Name, const hpp::pinocchio::Transform3s& M, - boost::python::list m) { + const char* constraintName, const char* joint1Name, const char* joint2Name, + const hpp::pinocchio::Transform3s& M, boost::python::list m) { try { if (!robot()) throw std::logic_error("No robot loaded"); @@ -243,9 +245,10 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); const hpp::pinocchio::Transform3s ref1 = f1.positionInParentJoint() * hpp::pinocchio::Transform3s::Identity(); - auto func = GenericTransformation::create( - name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); + auto func = GenericTransformation< + PositionBit | OrientationBit | + hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { @@ -263,8 +266,8 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( } hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( - const char* constraintName, const char* joint1Name, - const char* joint2Name, const hpp::pinocchio::Transform3s& M1, + const char* constraintName, const char* joint1Name, const char* joint2Name, + const hpp::pinocchio::Transform3s& M1, const hpp::pinocchio::Transform3s& M2, const boost::python::list m) { try { if (!robot()) throw std::logic_error("No robot loaded"); @@ -282,11 +285,11 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( if (joint1Name && std::strlen(joint1Name) > 0) { hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); - const hpp::pinocchio::Transform3s ref1 = - f1.positionInParentJoint() * M1; - auto func = GenericTransformation::create( - name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); + const hpp::pinocchio::Transform3s ref1 = f1.positionInParentJoint() * M1; + auto func = GenericTransformation< + PositionBit | OrientationBit | + hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { @@ -302,16 +305,19 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( } } -void Problem::setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, - bool constant) { +void Problem::setConstantRightHandSide( + hpp::constraints::ImplicitPtr_t constraint, bool constant) { try { if (constant) { - hpp::constraints::ComparisonTypes_t eqtypes(constraint->function().outputDerivativeSize(), hpp::constraints::EqualToZero); + hpp::constraints::ComparisonTypes_t eqtypes( + constraint->function().outputDerivativeSize(), + hpp::constraints::EqualToZero); constraint->comparisonType(eqtypes); } else { - hpp::constraints::ComparisonTypes_t eqtypes(constraint->function().outputDerivativeSize(), hpp::constraints::Equality); + hpp::constraints::ComparisonTypes_t eqtypes( + constraint->function().outputDerivativeSize(), + hpp::constraints::Equality); constraint->comparisonType(eqtypes); - } } catch (const std::exception& exc) { throw std::logic_error(exc.what()); @@ -335,40 +341,42 @@ ConstraintResult Problem::applyConstraints(ConfigurationIn_t config) { } } - boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { - try { - Configuration_t config = dofArray; - hpp::core::ValidationReportPtr_t validationReport; - bool validity = obj->configValidations()->validate(config, validationReport); - - std::string report; - if (validity) { - report = ""; - } else { - std::ostringstream oss; - oss << *validationReport; - report = oss.str(); - } - return boost::python::make_tuple(validity, report); - } catch (const std::exception& exc) { - throw std::logic_error(exc.what()); + try { + Configuration_t config = dofArray; + hpp::core::ValidationReportPtr_t validationReport; + bool validity = + obj->configValidations()->validate(config, validationReport); + + std::string report; + if (validity) { + report = ""; + } else { + std::ostringstream oss; + oss << *validationReport; + report = oss.str(); } + return boost::python::make_tuple(validity, report); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } } -void Problem::addNumericalConstraintsToConfigProjector1(const char* configProjName, - boost::python::list constraints, - boost::python::list priorities) { +void Problem::addNumericalConstraintsToConfigProjector1( + const char* configProjName, boost::python::list constraints, + boost::python::list priorities) { try { - auto constraintsVec = extract_vector(constraints); - auto prioritiesVec = extract_vector(priorities); - hpp::core::ConfigProjectorPtr_t configProjector = constraints_->configProjector(); + auto constraintsVec = + extract_vector(constraints); + auto prioritiesVec = extract_vector(priorities); + hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector(); for (unsigned int i = 0; i < constraintsVec.size(); ++i) { - if (!configProjector) { - configProjector = hpp::core::ConfigProjector::create( - robot(), configProjName, errorThreshold_, maxIterProjection_); - constraints_->addConstraint(configProjector); - } + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } configProjector->add(constraintsVec[i], prioritiesVec[i]); } } catch (const std::exception& exc) { @@ -376,17 +384,19 @@ void Problem::addNumericalConstraintsToConfigProjector1(const char* configProjNa } } -void Problem::addNumericalConstraintsToConfigProjector2(const char* configProjName, - boost::python::list constraints) { +void Problem::addNumericalConstraintsToConfigProjector2( + const char* configProjName, boost::python::list constraints) { try { - auto constraintsVec = extract_vector(constraints); - hpp::core::ConfigProjectorPtr_t configProjector = constraints_->configProjector(); + auto constraintsVec = + extract_vector(constraints); + hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector(); for (unsigned int i = 0; i < constraintsVec.size(); ++i) { - if (!configProjector) { - configProjector = hpp::core::ConfigProjector::create( - robot(), configProjName, errorThreshold_, maxIterProjection_); - constraints_->addConstraint(configProjector); - } + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } configProjector->add(constraintsVec[i], 0); } } catch (const std::exception& exc) { @@ -396,44 +406,45 @@ void Problem::addNumericalConstraintsToConfigProjector2(const char* configProjNa hpp::constraints::ImplicitPtr_t Problem::createComBetweenFeet( const char* constraintName, const char* comName, const char* jointLName, - const char* jointRName, const hpp::pinocchio::vector3_t& pointL, - const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask) { - if (!robot()) throw std::logic_error("No robot loaded"); - + try { hpp::pinocchio::JointPtr_t jointL, jointR, jointRef; hpp::pinocchio::CenterOfMassComputationPtr_t comc; - + auto m = extract_vector(mask); - + jointL = robot()->getJointByName(jointLName); jointR = robot()->getJointByName(jointRName); - + if (std::string(jointRefName) == std::string("")) jointRef = robot()->rootJoint(); else jointRef = robot()->getJointByName(jointRefName); - + std::string name(constraintName), comN(comName); - + hpp::constraints::ComparisonTypes_t comps(2, hpp::constraints::EqualToZero); comps.push_back(hpp::constraints::Superior); comps.push_back(hpp::constraints::Inferior); - + if (comN.compare("") == 0) { return hpp::constraints::Implicit::create( - hpp::constraints::ComBetweenFeet::create( - name, robot(), jointL, jointR, pointL, pointR, jointRef, pointRef, m), + hpp::constraints::ComBetweenFeet::create(name, robot(), jointL, + jointR, pointL, pointR, + jointRef, pointRef, m), comps); } else { if (!centerOfMassComputations[comN]) throw std::logic_error("Partial COM " + comN + " not found."); comc = centerOfMassComputations[comN]; return hpp::constraints::Implicit::create( - hpp::constraints::ComBetweenFeet::create( - name, robot(), comc, jointL, jointR, pointL, pointR, jointRef, pointRef, m), + hpp::constraints::ComBetweenFeet::create(name, robot(), comc, jointL, + jointR, pointL, pointR, + jointRef, pointRef, m), comps); } } catch (const std::exception& exc) { @@ -579,19 +590,20 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) .PYHPP_DEFINE_METHOD(Problem, applyConstraints) .PYHPP_DEFINE_METHOD(Problem, isConfigValid) - .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector1) - .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector2) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector1) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector2) .def_readwrite("errorThreshold", &Problem::errorThreshold_) .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) - .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet) - ; + .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet); register_problem_converters(); class_("ConstraintResult") - .def_readwrite("success", &ConstraintResult::success) - .def_readwrite("configuration", &ConstraintResult::configuration) - .def_readwrite("error", &ConstraintResult::error); + .def_readwrite("success", &ConstraintResult::success) + .def_readwrite("configuration", &ConstraintResult::configuration) + .def_readwrite("error", &ConstraintResult::error); } } // namespace core diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 98e3a481..952d62f5 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -30,15 +30,14 @@ from hpp import Transform from pinocchio import SE3 -from pyhpp.constraints import RelativeCom, Implicit, Transformation, RelativeTransformation, ComparisonType, ComparisonTypes + + # # This class provides tools to create static stability constraints class StaticStabilityConstraintsFactory: - def __init__( - self, - problem - ): + def __init__(self, problem): self.problem = problem self.robot = problem.robot() + def _getCOM(self, com): from numpy import array @@ -111,12 +110,12 @@ def createSlidingStabilityConstraint( return created_constraints -# # # Create static stability constraints where the feet are fixed on the ground, + # # # Create static stability constraints where the feet are fixed on the ground, def createStaticStabilityConstraint( self, prefix, comName, leftAnkle, rightAnkle, q0, maskCom=(True,) * 3 ): created_constraints = dict() - + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) Ml = Transform(_tfs[0]) Mr = Transform(_tfs[1]) @@ -158,12 +157,11 @@ def createStaticStabilityConstraint( return created_constraints - def createAlignedCOMStabilityConstraint( self, prefix, comName, leftAnkle, rightAnkle, q0, sliding ): created_constraints = dict() - + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) Ml = Transform(_tfs[0]) Mr = Transform(_tfs[1]) @@ -195,25 +193,25 @@ def createAlignedCOMStabilityConstraint( # Pose of the right foot result.append(prefix + "pose-right-foot") constraint = self.problem.createTransformationConstraint2( - result[-1], - "", - rightAnkle, + result[-1], + "", + rightAnkle, SE3(Mr.quaternion.toRotationMatrix(), Mr.translation), SE3.Identity(), - mask + mask, ) created_constraints[result[-1]] = constraint # Pose of the left foot result.append(prefix + "pose-left-foot") constraint = self.problem.createTransformationConstraint2( - result[-1], - "", - leftAnkle, + result[-1], + "", + leftAnkle, SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), SE3.Identity(), - mask + mask, ) created_constraints[result[-1]] = constraint - return created_constraints \ No newline at end of file + return created_constraints diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 2b8f1213..2103aa9e 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -30,7 +30,6 @@ #include <../src/pyhpp/manipulation/device.hh> #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/problem.hh> -#include "pyhpp/core/problem.hh" #include #include #include @@ -45,6 +44,7 @@ #include #include "hpp/manipulation/constraint-set.hh" +#include "pyhpp/core/problem.hh" namespace { @@ -730,8 +730,8 @@ boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget( // Constraint application // ============================================================================= -pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input) { +pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints( + PyWStatePtr_t state, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(state->obj->configConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -745,9 +745,8 @@ pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t stat return pyhpp::core::ConstraintResult(success, output, residualError); } -pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input) { +pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints( + PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->pathConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -762,9 +761,8 @@ pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transi return pyhpp::core::ConstraintResult(success, output, residualError); } -pyhpp::core::ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input) { +pyhpp::core::ConstraintResult PyWGraph::generateTargetConfig( + PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->targetConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 9f17e851..19e43da6 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -33,12 +33,11 @@ #include #include - namespace pyhpp { namespace core { struct ConstraintResult; } -} +} // namespace pyhpp namespace pyhpp { namespace manipulation { @@ -211,13 +210,13 @@ struct PyWGraph { const char* joint2); pyhpp::core::ConstraintResult applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input); + ConfigurationIn_t input); pyhpp::core::ConstraintResult applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); pyhpp::core::ConstraintResult generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); // Subgraph management void createSubGraph(const char* subgraphName, hpp::core::RoadmapPtr_t roadmap); diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 717c161f..c56a636a 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -33,8 +33,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -104,7 +104,7 @@ typedef hpp::pinocchio::Frame Frame; typedef hpp::pinocchio::JointPtr_t JointPtr_t; static void setJointBounds(Device& device, const char* jointName, - boost::python::list py_jointBounds) { + boost::python::list py_jointBounds) { Frame frame = device.getFrameByName(jointName); JointPtr_t joint = frame.joint(); auto jointBounds = extract_vector(py_jointBounds); @@ -136,13 +136,13 @@ static boost::python::dict rankInConfiguration(Device& device) { try { auto joint_names = device.model().names; for (const auto& joint_name : joint_names) { - Frame frame = device.getFrameByName(joint_name.c_str()); - if (!frame.isFixed()) { - JointPtr_t joint = frame.joint(); - if (joint) { - rank_dict[joint_name] = joint->rankInConfiguration(); - } - } + Frame frame = device.getFrameByName(joint_name.c_str()); + if (!frame.isFixed()) { + JointPtr_t joint = frame.joint(); + if (joint) { + rank_dict[joint_name] = joint->rankInConfiguration(); + } + } } } catch (const std::exception& exc) { throw std::logic_error(exc.what()); @@ -153,9 +153,9 @@ static boost::python::dict rankInConfiguration(Device& device) { typedef hpp::pinocchio::FrameIndex FrameIndex; typedef hpp::pinocchio::SE3 SE3; -static boost::python::list getJointsPosition(Device& device, - const Configuration_t& dofArray, - const boost::python::list& jointNames) { +static boost::python::list getJointsPosition( + Device& device, const Configuration_t& dofArray, + const boost::python::list& jointNames) { try { device.currentConfiguration(dofArray); device.computeForwardKinematics(); @@ -164,25 +164,27 @@ static boost::python::list getJointsPosition(Device& device, const Model& model(device.model()); const Data& data(device.data()); boost::python::list transforms; - for (Py_ssize_t i = 0; i < static_cast(boost::python::len(jointNames)); ++i) { + for (Py_ssize_t i = 0; + i < static_cast(boost::python::len(jointNames)); ++i) { std::string n = boost::python::extract(jointNames[i]); if (!model.existFrame(n)) { throw std::logic_error("Robot has no frame with name " + n); } FrameIndex joint = model.getFrameId(n); if (model.frames.size() <= (std::size_t)joint) - throw std::logic_error("Frame index of joint " + n + " out of bounds: " + std::to_string(joint)); - const SE3& M = data.oMf[joint]; - Transform3s::Quaternion q(M.rotation()); - boost::python::list t; - t.append(M.translation()[0]); - t.append(M.translation()[1]); - t.append(M.translation()[2]); - t.append(q.x()); - t.append(q.y()); - t.append(q.z()); - t.append(q.w()); - transforms.append(t); + throw std::logic_error("Frame index of joint " + n + + " out of bounds: " + std::to_string(joint)); + const SE3& M = data.oMf[joint]; + Transform3s::Quaternion q(M.rotation()); + boost::python::list t; + t.append(M.translation()[0]); + t.append(M.translation()[1]); + t.append(M.translation()[2]); + t.append(q.x()); + t.append(q.y()); + t.append(q.z()); + t.append(q.w()); + transforms.append(t); } return transforms; } catch (const std::exception& exc) { @@ -249,7 +251,6 @@ void exposeDevice() { .def("setJointBounds", &setJointBounds) .def("getCenterOfMass", &getCenterOfMass) .def("getJointsPosition", &getJointsPosition); - } } // namespace pinocchio } // namespace pyhpp diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 06617e7a..9f282f91 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -1,33 +1,97 @@ #!/usr/bin/env python from argparse import ArgumentParser -from math import pi import numpy as np import datetime as dt -from pyhpp.core.static_stability_constraint_factory import StaticStabilityConstraintsFactory -from pyhpp.core import Problem, DiffusingPlanner, createDichotomy, createProgressiveProjector, Straight -from pyhpp.constraints import LockedJoint, Implicit, Transformation, ComparisonType, ComparisonTypes +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pyhpp.core import ( + Problem, + DiffusingPlanner, + createDichotomy, + createProgressiveProjector, + Straight, +) +from pyhpp.constraints import LockedJoint from pyhpp.pinocchio import Device, urdf from pinocchio import SE3 + parser = ArgumentParser() -parser.add_argument('-N', default=20, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=20, 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' +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.create('pyrene') +robot = Device.create("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]) +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]) +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) @@ -35,22 +99,23 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -problem.addPartialCom ('pyrene', ['pyrene/root_joint']) +problem.addPartialCom("pyrene", ["pyrene/root_joint"]) constraints = dict() -leftAnkle = 'pyrene/leg_left_6_joint' -rightAnkle = 'pyrene/leg_right_6_joint' +leftAnkle = "pyrene/leg_left_6_joint" +rightAnkle = "pyrene/leg_right_6_joint" factory = StaticStabilityConstraintsFactory(problem) -constraints = factory.createSlidingStabilityConstraint("balance/", 'pyrene', leftAnkle, - rightAnkle, q0) +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_')] +gripperJoints = [j for j in jointNames if j.startswith("gripper_")] for j in gripperJoints: ljName = "locked_" + j value = q0[robot.rankInConfiguration[j]] @@ -58,13 +123,15 @@ constraints[ljName] = cs # Lock torso joints -for j in ['pyrene/torso_1_joint', 'pyrene/torso_2_joint']: +for j in ["pyrene/torso_1_joint", "pyrene/torso_2_joint"]: ljName = "locked_" + j value = q0[robot.rankInConfiguration[j]] cs = LockedJoint.create(robot, j, np.array([value])) constraints[ljName] = cs -problem.addNumericalConstraintsToConfigProjector("balance-proj", list(constraints.values()), [0]*len(constraints)) +problem.addNumericalConstraintsToConfigProjector( + "balance-proj", list(constraints.values()), [0] * len(constraints) +) # Set up path validation and projection problem.pathValidation = createDichotomy(robot, 0) problem.steeringMethod = Straight(problem) @@ -72,9 +139,117 @@ 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]) +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]) +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 = problem.applyConstraints(q1) assert result.success @@ -123,4 +298,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") From 27ea5b7253b824bdea35b3126981193bd3866a7a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 27 Oct 2025 10:46:36 +0100 Subject: [PATCH 014/109] Add utility functions and homogeinize syntax --- include/pyhpp/core/problem.hh | 13 +- include/pyhpp/manipulation/fwd.hh | 2 +- src/CMakeLists.txt | 1 + src/pyhpp/core/constraint.cc | 24 ++++ src/pyhpp/core/distance.cc | 3 +- src/pyhpp/core/problem.cc | 32 ++++- src/pyhpp/core/roadmap.cc | 36 +++++- src/pyhpp/manipulation/bindings.cc | 2 +- src/pyhpp/manipulation/path-planner.cc | 137 +++++++++++++++++----- src/pyhpp/manipulation/path-planner.hh | 29 ++++- src/pyhpp/manipulation/steering-method.cc | 38 +++++- src/pyhpp/manipulation/steering-method.hh | 14 ++- tests/path-planner.py | 2 +- tests/rrt.py | 5 +- 14 files changed, 277 insertions(+), 61 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index f8cc1999..f977ddfd 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -138,11 +138,14 @@ struct Problem { ConstraintResult applyConstraints(ConfigurationIn_t config); boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); - void addNumericalConstraintsToConfigProjector1( - const char* configProjName, boost::python::list constraints, - boost::python::list priorities); - void addNumericalConstraintsToConfigProjector2( - const char* configProjName, boost::python::list constraints); + void setConstraints(hpp::core::ConstraintSetPtr_t constraints); + void setRightHandSideFromConfig(ConfigurationIn_t configIn); + + void addNumericalConstraintsToConfigProjector1(const char* configProjName, + boost::python::list constraints, + boost::python::list priorities); + void addNumericalConstraintsToConfigProjector2(const char* configProjName, + boost::python::list constraints); hpp::constraints::ImplicitPtr_t createComBetweenFeet( const char* constraintName, const char* comName, const char* jointLName, const char* jointRName, const hpp::pinocchio::vector3_t& pointL, diff --git a/include/pyhpp/manipulation/fwd.hh b/include/pyhpp/manipulation/fwd.hh index bfdf2406..e94b5c70 100644 --- a/include/pyhpp/manipulation/fwd.hh +++ b/include/pyhpp/manipulation/fwd.hh @@ -53,7 +53,7 @@ void exposeGraph(); void exposePathPlanners(); void exposeProblem(); void exposePathProjector(); -void exposeGraphSteeringMethod(); +void exposeManipSteeringMethod(); } // namespace manipulation } // namespace pyhpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ebf76193..3d5be9fa 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -166,3 +166,4 @@ add_python_library( pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES hpp-manipulation-urdf::hpp-manipulation-urdf) + diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 74f21215..6f74bfd1 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -62,6 +62,26 @@ struct CWrapper { static ConstraintPtr_t copy(const Constraint* cs) { return cs->copy(); } }; +static ConstraintSetPtr_t createConstraintSet( + const hpp::pinocchio::DevicePtr_t& device, const std::string& name) { + return core::ConstraintSet::create(device, name); +} + +static ConfigProjectorPtr_t createConfigProjector( + const hpp::pinocchio::DevicePtr_t& device, const std::string& name, + value_type threshold, size_type iterations) { + return core::ConfigProjector::create(device, name, threshold, iterations); +} + +static void rightHandSideFromConfig(ConfigProjectorPtr_t& configProj, ConfigurationIn_t config) { + configProj->rightHandSideFromConfig(config); +} + +static void setRightHandSideOfConstraint(ConfigProjectorPtr_t& configProj, hpp::constraints::ImplicitPtr_t constraint, ConfigurationIn_t config) { + configProj->rightHandSide(constraint, config); +} + + void exposeConstraint() { class_("Constraint", no_init) .def("__str__", &to_str_from_operator) @@ -73,11 +93,13 @@ void exposeConstraint() { class_ >("ConstraintSet", no_init) + .def("__init__", make_constructor(&createConstraintSet)) .PYHPP_DEFINE_METHOD(ConstraintSet, addConstraint) .PYHPP_DEFINE_METHOD(ConstraintSet, configProjector); class_ >("ConfigProjector", no_init) + .def("__init__", make_constructor(&createConfigProjector)) .def("solver", static_cast( &ConfigProjector::solver), @@ -92,6 +114,8 @@ void exposeConstraint() { static_cast( &ConfigProjector::errorThreshold)) .PYHPP_DEFINE_METHOD(ConfigProjector, residualError) + .def("setRightHandSideFromConfig", &rightHandSideFromConfig) + .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint) .def("sigma", &ConfigProjector::sigma, return_value_policy()); } diff --git a/src/pyhpp/core/distance.cc b/src/pyhpp/core/distance.cc index 4bd9621a..aa6ac92e 100644 --- a/src/pyhpp/core/distance.cc +++ b/src/pyhpp/core/distance.cc @@ -59,8 +59,7 @@ void exposeDistance() { .def("compute", &DistanceWrapper::compute); class_, WeighedDistancePtr_t, boost::noncopyable>("WeighedDistance", no_init) - .def("create", &WeighedDistance::create) - .staticmethod("create") + .def("__init__", make_constructor(&WeighedDistance::create)) .def("asDistancePtr_t", &DistanceWrapper::AsDistancePtr_t) .def("getWeights", &DistanceWrapper::getWeights) .def("setWeights", &DistanceWrapper::setWeights); diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index b7104fb5..dd36f0c7 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -362,6 +362,30 @@ boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { } } +void Problem::setConstraints(hpp::core::ConstraintSetPtr_t constraints) { + try { + constraints_ = hpp::core::ConstraintSet::create(robot(), "Default constraint set"); + constraints_->addConstraint(constraints); + obj->constraints(constraints_); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::setRightHandSideFromConfig(ConfigurationIn_t configIn) { + try { + hpp::core::ConfigProjectorPtr_t configProjector( + constraints_->configProjector()); + if (!configProjector) { + throw std::runtime_error("No constraint has been set."); + } + Configuration_t q = configIn; + configProjector->rightHandSideFromConfig(q); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + void Problem::addNumericalConstraintsToConfigProjector1( const char* configProjName, boost::python::list constraints, boost::python::list priorities) { @@ -590,10 +614,10 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) .PYHPP_DEFINE_METHOD(Problem, applyConstraints) .PYHPP_DEFINE_METHOD(Problem, isConfigValid) - .def("addNumericalConstraintsToConfigProjector", - &Problem::addNumericalConstraintsToConfigProjector1) - .def("addNumericalConstraintsToConfigProjector", - &Problem::addNumericalConstraintsToConfigProjector2) + .PYHPP_DEFINE_METHOD(Problem, setConstraints) + .PYHPP_DEFINE_METHOD(Problem, setRightHandSideFromConfig) + .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector1) + .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector2) .def_readwrite("errorThreshold", &Problem::errorThreshold_) .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet); diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index f2151b0d..c87f93d5 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -140,12 +140,43 @@ struct RWrapper { } return to_python_list(res); } + + static boost::python::list nodesConnectedComponent( + Roadmap& roadmap, int connectedComponentId) { + try { + const ConnectedComponents_t& connectedComponents = + roadmap.connectedComponents(); + + if ((std::size_t)connectedComponentId >= connectedComponents.size()) { + std::ostringstream oss; + oss << "connectedComponentId=" << connectedComponentId + << " out of range [0," << connectedComponents.size() - 1 << "]."; + throw std::runtime_error(oss.str()); + } + + ConnectedComponents_t::const_iterator itcc = connectedComponents.begin(); + std::advance(itcc, connectedComponentId); + + const NodeVector_t& nodes = (*itcc)->nodes(); + + Configurations_t res; + res.reserve(nodes.size()); + + for (const auto& node : nodes) { + res.push_back(node->configuration()); + } + + return to_python_list(res); + + } catch (const std::exception& exc) { + throw std::runtime_error(exc.what()); + } + } }; void exposeRoadmap() { class_("Roadmap", no_init) - .def("create", &Roadmap::create) - .staticmethod("create") + .def("__init__", make_constructor(&Roadmap::create)) .def("__str__", &to_str) .PYHPP_DEFINE_METHOD(Roadmap, clear) .PYHPP_DEFINE_METHOD1(RWrapper, addNode, @@ -171,6 +202,7 @@ void exposeRoadmap() { .PYHPP_DEFINE_METHOD(Roadmap, resetGoalNodes) .PYHPP_DEFINE_METHOD(Roadmap, pathExists) .def("nodes", &RWrapper::nodes) + .def("nodesConnectedComponent", &RWrapper::nodesConnectedComponent) .def("initNode", &RWrapper::initNode1) .def("initNode", &RWrapper::initNode2, return_value_policy()) diff --git a/src/pyhpp/manipulation/bindings.cc b/src/pyhpp/manipulation/bindings.cc index f42bb4f4..c6aa9778 100644 --- a/src/pyhpp/manipulation/bindings.cc +++ b/src/pyhpp/manipulation/bindings.cc @@ -41,6 +41,6 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::manipulation::exposeDevice(); pyhpp::manipulation::exposeGraph(); pyhpp::manipulation::exposePathProjector(); - pyhpp::manipulation::exposeGraphSteeringMethod(); + pyhpp::manipulation::exposeManipSteeringMethod(); pyhpp::manipulation::exposePathPlanners(); } diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index c8762e98..6a08e1b1 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -31,6 +31,7 @@ #include <../src/pyhpp/manipulation/path-planner.hh> #include #include +#include #include #include #include @@ -50,35 +51,7 @@ struct ManipulationPlanner : public pyhpp::core::PathPlanner { } }; -void exposePathPlanners() { - boost::python::class_>( - "TransitionPlanner", boost::python::init()) - .def("innerPlanner", - static_cast( - &TransitionPlanner::innerPlanner)) - .def("innerPlanner", static_cast( - &TransitionPlanner::innerPlanner)) - .def("innerProblem", &TransitionPlanner::innerProblem) - .def("planPath", &TransitionPlanner::planPath) - .def("directPath", &TransitionPlanner::directPath) - .def("validateConfiguration", &TransitionPlanner::validateConfiguration) - .def("optimizePath", &TransitionPlanner::optimizePath) - .def("timeParameterization", &TransitionPlanner::timeParameterization) - .def("setEdge", &TransitionPlanner::setEdge) - .def("setReedsAndSheppSteeringMethod", - &TransitionPlanner::setReedsAndSheppSteeringMethod) - .def("pathProjector", &TransitionPlanner::pathProjector) - .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) - .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer); - - boost::python::class_>( - "ManipulationPlanner", - boost::python::init()); -} - +// TransitionPlanner implementation TransitionPlanner::TransitionPlanner(const pyhpp::core::Problem& problem) { obj = hpp::manipulation::pathPlanner::TransitionPlanner::createWithRoadmap( problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), @@ -102,6 +75,7 @@ pyhpp::core::PathPlanner TransitionPlanner::innerPlanner() const { void TransitionPlanner::innerPlanner(const pyhpp::core::PathPlanner& planner) { trObj()->innerPlanner(planner.obj); } + pyhpp::core::Problem TransitionPlanner::innerProblem() const { return pyhpp::core::Problem(trObj()->innerProblem()); } @@ -109,7 +83,6 @@ pyhpp::core::Problem TransitionPlanner::innerProblem() const { PathVectorPtr_t TransitionPlanner::planPath(ConfigurationIn_t qInit, matrixIn_t qGoals, bool resetRoadmap) { - // Check sizes of initial and goal configurations if (qInit.rows() != obj->problem()->robot()->configSize()) { std::ostringstream os; os << "qInit = " << hpp::pinocchio::displayConfig(qInit) @@ -132,7 +105,6 @@ PathVectorPtr_t TransitionPlanner::planPath(ConfigurationIn_t qInit, tuple TransitionPlanner::directPath(ConfigurationIn_t q1, ConfigurationIn_t q2, bool validate) { - // Check sizes of initial and goal configurations if (q1.rows() != obj->problem()->robot()->configSize()) { std::ostringstream os; os << "q1 = " << hpp::pinocchio::displayConfig(q1) << "should be of size " @@ -188,5 +160,108 @@ void TransitionPlanner::addPathOptimizer( trObj()->addPathOptimizer(pathOptimizer); } +// EndEffectorTrajectory implementation +EndEffectorTrajectory::EndEffectorTrajectory(const pyhpp::core::Problem& problem) { + obj = hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), + problem.obj->robot())); +} + +EndEffectorTrajectory::EndEffectorTrajectory(const pyhpp::core::Problem& problem, + const hpp::core::RoadmapPtr_t& roadmap) { + obj = hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, roadmap); +} + +hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t +EndEffectorTrajectory::eetObj() const { + assert(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::pathPlanner::EndEffectorTrajectory, + obj)); + return HPP_STATIC_PTR_CAST(hpp::manipulation::pathPlanner::EndEffectorTrajectory, + obj); +} + +int EndEffectorTrajectory::nRandomConfig() const { + return eetObj()->nRandomConfig(); +} + +void EndEffectorTrajectory::nRandomConfig(int n) { + eetObj()->nRandomConfig(n); +} + +int EndEffectorTrajectory::nDiscreteSteps() const { + return eetObj()->nDiscreteSteps(); +} + +void EndEffectorTrajectory::nDiscreteSteps(int n) { + eetObj()->nDiscreteSteps(n); +} + +void EndEffectorTrajectory::checkFeasibilityOnly(bool enable) { + eetObj()->checkFeasibilityOnly(enable); +} + +bool EndEffectorTrajectory::checkFeasibilityOnly() const { + return eetObj()->checkFeasibilityOnly(); +} + +// void EndEffectorTrajectory::ikSolverInitialization(IkSolverInitializationPtr_t solver) { +// eetObj()->ikSolverInitialization(solver); +// } + +void exposePathPlanners() { + boost::python::class_>( + "TransitionPlanner", boost::python::init()) + .def("innerPlanner", + static_cast( + &TransitionPlanner::innerPlanner)) + .def("innerPlanner", static_cast( + &TransitionPlanner::innerPlanner)) + .def("innerProblem", &TransitionPlanner::innerProblem) + .def("planPath", &TransitionPlanner::planPath) + .def("directPath", &TransitionPlanner::directPath) + .def("validateConfiguration", &TransitionPlanner::validateConfiguration) + .def("optimizePath", &TransitionPlanner::optimizePath) + .def("timeParameterization", &TransitionPlanner::timeParameterization) + .def("setEdge", &TransitionPlanner::setEdge) + .def("setReedsAndSheppSteeringMethod", + &TransitionPlanner::setReedsAndSheppSteeringMethod) + .def("pathProjector", &TransitionPlanner::pathProjector) + .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) + .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer); + + boost::python::class_>( + "ManipulationPlanner", + boost::python::init()); + + boost::python::class_>( + "EndEffectorTrajectory", boost::python::init()) + .def(boost::python::init()) + .def("nRandomConfig", + static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nRandomConfig", + static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nDiscreteSteps", + static_cast( + &EndEffectorTrajectory::nDiscreteSteps)) + .def("nDiscreteSteps", + static_cast( + &EndEffectorTrajectory::nDiscreteSteps)) + .def("checkFeasibilityOnly", + static_cast( + &EndEffectorTrajectory::checkFeasibilityOnly)) + .def("checkFeasibilityOnly", + static_cast( + &EndEffectorTrajectory::checkFeasibilityOnly)) + // .def("ikSolverInitialization", &EndEffectorTrajectory::ikSolverInitialization) + ; +} + } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/path-planner.hh b/src/pyhpp/manipulation/path-planner.hh index c35a7b30..5dbf6fd8 100644 --- a/src/pyhpp/manipulation/path-planner.hh +++ b/src/pyhpp/manipulation/path-planner.hh @@ -1,6 +1,6 @@ // // Copyright (c) 2025, CNRS -// Authors: Florent Lamiraux +// Authors: Florent Lamiraux, Paul Sardin // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ namespace pyhpp { namespace manipulation { - + typedef boost::python::tuple tuple; typedef hpp::core::ConfigurationIn_t ConfigurationIn_t; typedef hpp::core::PathOptimizerPtr_t PathOptimizerPtr_t; @@ -44,6 +44,7 @@ typedef hpp::core::PathPtr_t PathPtr_t; typedef hpp::core::PathVectorPtr_t PathVectorPtr_t; typedef hpp::core::RoadmapPtr_t RoadmapPtr_t; typedef hpp::manipulation::matrixIn_t matrixIn_t; +// typedef hpp::manipulation::pathPlanner::IkSolverInitializationPtr_t IkSolverInitializationPtr_t; struct TransitionPlanner : public pyhpp::core::PathPlanner { // Dynamic cast pointer into TransitionPlanner @@ -63,7 +64,23 @@ struct TransitionPlanner : public pyhpp::core::PathPlanner { void pathProjector(const PathProjectorPtr_t pathProjector); void clearPathOptimizers(); void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer); -}; // struct TransitionPlanner -} // namespace manipulation -} // namespace pyhpp -#endif // PYHPP_MANIPULATION_PATH_PLANNER_HH +}; + +struct EndEffectorTrajectory : public pyhpp::core::PathPlanner { + EndEffectorTrajectory(const pyhpp::core::Problem& problem); + EndEffectorTrajectory(const pyhpp::core::Problem& problem, const RoadmapPtr_t& roadmap); + hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t eetObj() const; + int nRandomConfig() const; + void nRandomConfig(int n); + int nDiscreteSteps() const; + void nDiscreteSteps(int n); + void checkFeasibilityOnly(bool enable); + bool checkFeasibilityOnly() const; + // void ikSolverInitialization(IkSolverInitializationPtr_t solver); +}; + +void exposePathPlanners(); + +} // namespace manipulation +} // namespace pyhpp +#endif // PYHPP_MANIPULATION_PATH_PLANNER_HH \ No newline at end of file diff --git a/src/pyhpp/manipulation/steering-method.cc b/src/pyhpp/manipulation/steering-method.cc index d64a2e17..e7ea1606 100644 --- a/src/pyhpp/manipulation/steering-method.cc +++ b/src/pyhpp/manipulation/steering-method.cc @@ -29,7 +29,9 @@ #include <../src/pyhpp/manipulation/steering-method.hh> #include +#include #include +#include namespace pyhpp { namespace manipulation { @@ -43,11 +45,43 @@ GraphSteeringMethod::GraphSteeringMethod( obj = graph; } -void exposeGraphSteeringMethod() { + +hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t +EndEffectorTrajectorySteeringMethod::eetObj() const { + assert(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::steeringMethod::EndEffectorTrajectory, + obj)); + return HPP_STATIC_PTR_CAST(hpp::manipulation::steeringMethod::EndEffectorTrajectory, + obj); +} + +EndEffectorTrajectorySteeringMethod::EndEffectorTrajectorySteeringMethod( + const hpp::core::ProblemConstPtr_t& problem) + : SteeringMethod(hpp::manipulation::steeringMethod::EndEffectorTrajectory::create(problem)) { +} + +void EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint( + const hpp::constraints::ImplicitPtr_t& ic) { + eetObj()->trajectoryConstraint(ic); +} + +void EndEffectorTrajectorySteeringMethod::setTrajectory( + const hpp::core::PathPtr_t& eeTraj, bool se3Output) { + eetObj()->trajectory(eeTraj, se3Output); +} + +void exposeManipSteeringMethod() { class_( "GraphSteeringMethod", boost::python::init()); + + class_>( + "EndEffectorTrajectorySteeringMethod", + boost::python::init()) + .def("setTrajectoryConstraint", + &EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint) + .def("setTrajectory", + &EndEffectorTrajectorySteeringMethod::setTrajectory); } } // namespace manipulation -} // namespace pyhpp +} // namespace pyhpp \ No newline at end of file diff --git a/src/pyhpp/manipulation/steering-method.hh b/src/pyhpp/manipulation/steering-method.hh index 70a3b515..56a0fb39 100644 --- a/src/pyhpp/manipulation/steering-method.hh +++ b/src/pyhpp/manipulation/steering-method.hh @@ -20,8 +20,12 @@ #define PYHPP_MANIPULATION_STEERING_METHOD_HH #include +#include +#include #include +#include #include +#include namespace pyhpp { namespace manipulation { @@ -31,11 +35,15 @@ typedef pyhpp::core::PyWSteeringMethodPtr_t PyWSteeringMethodPtr_t; struct GraphSteeringMethod { hpp::manipulation::steeringMethod::GraphPtr_t obj; - GraphSteeringMethod(const PyWSteeringMethodPtr_t& steeringMethodWrapper); }; +struct EndEffectorTrajectorySteeringMethod : pyhpp::core::SteeringMethod{ + hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t eetObj() const; + EndEffectorTrajectorySteeringMethod(const hpp::core::ProblemConstPtr_t& problem); + void setTrajectoryConstraint(const hpp::constraints::ImplicitPtr_t& ic); + void setTrajectory(const hpp::core::PathPtr_t& eeTraj, bool se3Output); +}; } // namespace manipulation } // namespace pyhpp - -#endif // PYHPP_MANIPULATION_STEERING_METHOD_HH +#endif // PYHPP_MANIPULATION_STEERING_METHOD_HH \ No newline at end of file diff --git a/tests/path-planner.py b/tests/path-planner.py index b0c7d9c6..d3aced99 100644 --- a/tests/path-planner.py +++ b/tests/path-planner.py @@ -92,7 +92,7 @@ # path = kPrmStar_inst.solve() infinite search # viewer.displayPath(path, 0.001, 50) -# roadmap = Roadmap.create(problem.distance(), robot) +# roadmap = Roadmap(problem.distance(), robot) # roadmap.initNode(q_init) # roadmap.addGoalNode(q_goal) # searchInRoadmap = SearchInRoadmap(problem, roadmap) diff --git a/tests/rrt.py b/tests/rrt.py index 6edf2447..c34da1e5 100644 --- a/tests/rrt.py +++ b/tests/rrt.py @@ -27,11 +27,10 @@ problem = Problem(robot) configurationShooter = problem.configurationShooter() steer = problem.steeringMethod() -weighedDistance = WeighedDistance.create(robot) -distance = weighedDistance.asDistancePtr_t() +weighedDistance = WeighedDistance(robot) # Initialize roadmap -roadmap = Roadmap.create(distance, robot) +roadmap = Roadmap(weighedDistance, robot) roadmap.initNode(qInit) roadmap.addGoalNode(qGoal) From ea8fc2bfd606df714f6601b8b06d718ec11dade3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 10:18:10 +0000 Subject: [PATCH 015/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/pyhpp/core/problem.hh | 10 ++-- src/CMakeLists.txt | 1 - src/pyhpp/core/constraint.cc | 8 ++- src/pyhpp/core/problem.cc | 9 ++- src/pyhpp/core/roadmap.cc | 16 +++--- src/pyhpp/manipulation/path-planner.cc | 68 ++++++++++++----------- src/pyhpp/manipulation/path-planner.hh | 14 +++-- src/pyhpp/manipulation/steering-method.cc | 35 ++++++------ src/pyhpp/manipulation/steering-method.hh | 13 +++-- 9 files changed, 93 insertions(+), 81 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index f977ddfd..cc2ee56a 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -141,11 +141,11 @@ struct Problem { void setConstraints(hpp::core::ConstraintSetPtr_t constraints); void setRightHandSideFromConfig(ConfigurationIn_t configIn); - void addNumericalConstraintsToConfigProjector1(const char* configProjName, - boost::python::list constraints, - boost::python::list priorities); - void addNumericalConstraintsToConfigProjector2(const char* configProjName, - boost::python::list constraints); + void addNumericalConstraintsToConfigProjector1( + const char* configProjName, boost::python::list constraints, + boost::python::list priorities); + void addNumericalConstraintsToConfigProjector2( + const char* configProjName, boost::python::list constraints); hpp::constraints::ImplicitPtr_t createComBetweenFeet( const char* constraintName, const char* comName, const char* jointLName, const char* jointRName, const hpp::pinocchio::vector3_t& pointL, diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3d5be9fa..ebf76193 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -166,4 +166,3 @@ add_python_library( pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES hpp-manipulation-urdf::hpp-manipulation-urdf) - diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 6f74bfd1..60ec1d9c 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -73,15 +73,17 @@ static ConfigProjectorPtr_t createConfigProjector( return core::ConfigProjector::create(device, name, threshold, iterations); } -static void rightHandSideFromConfig(ConfigProjectorPtr_t& configProj, ConfigurationIn_t config) { +static void rightHandSideFromConfig(ConfigProjectorPtr_t& configProj, + ConfigurationIn_t config) { configProj->rightHandSideFromConfig(config); } -static void setRightHandSideOfConstraint(ConfigProjectorPtr_t& configProj, hpp::constraints::ImplicitPtr_t constraint, ConfigurationIn_t config) { +static void setRightHandSideOfConstraint( + ConfigProjectorPtr_t& configProj, + hpp::constraints::ImplicitPtr_t constraint, ConfigurationIn_t config) { configProj->rightHandSide(constraint, config); } - void exposeConstraint() { class_("Constraint", no_init) .def("__str__", &to_str_from_operator) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index dd36f0c7..5d33510e 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -364,7 +364,8 @@ boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { void Problem::setConstraints(hpp::core::ConstraintSetPtr_t constraints) { try { - constraints_ = hpp::core::ConstraintSet::create(robot(), "Default constraint set"); + constraints_ = + hpp::core::ConstraintSet::create(robot(), "Default constraint set"); constraints_->addConstraint(constraints); obj->constraints(constraints_); } catch (const std::exception& exc) { @@ -616,8 +617,10 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, isConfigValid) .PYHPP_DEFINE_METHOD(Problem, setConstraints) .PYHPP_DEFINE_METHOD(Problem, setRightHandSideFromConfig) - .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector1) - .def("addNumericalConstraintsToConfigProjector", &Problem::addNumericalConstraintsToConfigProjector2) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector1) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector2) .def_readwrite("errorThreshold", &Problem::errorThreshold_) .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet); diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index c87f93d5..e2aaf422 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -141,12 +141,12 @@ struct RWrapper { return to_python_list(res); } - static boost::python::list nodesConnectedComponent( - Roadmap& roadmap, int connectedComponentId) { + static boost::python::list nodesConnectedComponent(Roadmap& roadmap, + int connectedComponentId) { try { const ConnectedComponents_t& connectedComponents = roadmap.connectedComponents(); - + if ((std::size_t)connectedComponentId >= connectedComponents.size()) { std::ostringstream oss; oss << "connectedComponentId=" << connectedComponentId @@ -156,18 +156,18 @@ struct RWrapper { ConnectedComponents_t::const_iterator itcc = connectedComponents.begin(); std::advance(itcc, connectedComponentId); - + const NodeVector_t& nodes = (*itcc)->nodes(); - + Configurations_t res; res.reserve(nodes.size()); - + for (const auto& node : nodes) { res.push_back(node->configuration()); } - + return to_python_list(res); - + } catch (const std::exception& exc) { throw std::runtime_error(exc.what()); } diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 6a08e1b1..60e7be92 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -30,8 +30,8 @@ #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/path-planner.hh> #include -#include #include +#include #include #include #include @@ -161,33 +161,35 @@ void TransitionPlanner::addPathOptimizer( } // EndEffectorTrajectory implementation -EndEffectorTrajectory::EndEffectorTrajectory(const pyhpp::core::Problem& problem) { - obj = hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( - problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), - problem.obj->robot())); +EndEffectorTrajectory::EndEffectorTrajectory( + const pyhpp::core::Problem& problem) { + obj = + hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), + problem.obj->robot())); } -EndEffectorTrajectory::EndEffectorTrajectory(const pyhpp::core::Problem& problem, - const hpp::core::RoadmapPtr_t& roadmap) { - obj = hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( - problem.obj, roadmap); +EndEffectorTrajectory::EndEffectorTrajectory( + const pyhpp::core::Problem& problem, + const hpp::core::RoadmapPtr_t& roadmap) { + obj = + hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, roadmap); } hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t EndEffectorTrajectory::eetObj() const { - assert(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::pathPlanner::EndEffectorTrajectory, - obj)); - return HPP_STATIC_PTR_CAST(hpp::manipulation::pathPlanner::EndEffectorTrajectory, - obj); + assert(HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::pathPlanner::EndEffectorTrajectory, obj)); + return HPP_STATIC_PTR_CAST( + hpp::manipulation::pathPlanner::EndEffectorTrajectory, obj); } int EndEffectorTrajectory::nRandomConfig() const { return eetObj()->nRandomConfig(); } -void EndEffectorTrajectory::nRandomConfig(int n) { - eetObj()->nRandomConfig(n); -} +void EndEffectorTrajectory::nRandomConfig(int n) { eetObj()->nRandomConfig(n); } int EndEffectorTrajectory::nDiscreteSteps() const { return eetObj()->nDiscreteSteps(); @@ -205,7 +207,9 @@ bool EndEffectorTrajectory::checkFeasibilityOnly() const { return eetObj()->checkFeasibilityOnly(); } -// void EndEffectorTrajectory::ikSolverInitialization(IkSolverInitializationPtr_t solver) { +// void +// EndEffectorTrajectory::ikSolverInitialization(IkSolverInitializationPtr_t +// solver) { // eetObj()->ikSolverInitialization(solver); // } @@ -239,27 +243,27 @@ void exposePathPlanners() { boost::python::class_>( - "EndEffectorTrajectory", boost::python::init()) - .def(boost::python::init()) - .def("nRandomConfig", + "EndEffectorTrajectory", + boost::python::init()) + .def(boost::python::init()) + .def("nRandomConfig", static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nRandomConfig", static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nDiscreteSteps", static_cast( - &EndEffectorTrajectory::nRandomConfig)) - .def("nRandomConfig", - static_cast( - &EndEffectorTrajectory::nRandomConfig)) - .def("nDiscreteSteps", - static_cast( - &EndEffectorTrajectory::nDiscreteSteps)) - .def("nDiscreteSteps", - static_cast( &EndEffectorTrajectory::nDiscreteSteps)) - .def("checkFeasibilityOnly", + .def("nDiscreteSteps", static_cast( + &EndEffectorTrajectory::nDiscreteSteps)) + .def("checkFeasibilityOnly", static_cast( &EndEffectorTrajectory::checkFeasibilityOnly)) - .def("checkFeasibilityOnly", + .def("checkFeasibilityOnly", static_cast( &EndEffectorTrajectory::checkFeasibilityOnly)) - // .def("ikSolverInitialization", &EndEffectorTrajectory::ikSolverInitialization) + // .def("ikSolverInitialization", + // &EndEffectorTrajectory::ikSolverInitialization) ; } diff --git a/src/pyhpp/manipulation/path-planner.hh b/src/pyhpp/manipulation/path-planner.hh index 5dbf6fd8..5ca00a8a 100644 --- a/src/pyhpp/manipulation/path-planner.hh +++ b/src/pyhpp/manipulation/path-planner.hh @@ -35,7 +35,7 @@ namespace pyhpp { namespace manipulation { - + typedef boost::python::tuple tuple; typedef hpp::core::ConfigurationIn_t ConfigurationIn_t; typedef hpp::core::PathOptimizerPtr_t PathOptimizerPtr_t; @@ -44,7 +44,8 @@ typedef hpp::core::PathPtr_t PathPtr_t; typedef hpp::core::PathVectorPtr_t PathVectorPtr_t; typedef hpp::core::RoadmapPtr_t RoadmapPtr_t; typedef hpp::manipulation::matrixIn_t matrixIn_t; -// typedef hpp::manipulation::pathPlanner::IkSolverInitializationPtr_t IkSolverInitializationPtr_t; +// typedef hpp::manipulation::pathPlanner::IkSolverInitializationPtr_t +// IkSolverInitializationPtr_t; struct TransitionPlanner : public pyhpp::core::PathPlanner { // Dynamic cast pointer into TransitionPlanner @@ -68,7 +69,8 @@ struct TransitionPlanner : public pyhpp::core::PathPlanner { struct EndEffectorTrajectory : public pyhpp::core::PathPlanner { EndEffectorTrajectory(const pyhpp::core::Problem& problem); - EndEffectorTrajectory(const pyhpp::core::Problem& problem, const RoadmapPtr_t& roadmap); + EndEffectorTrajectory(const pyhpp::core::Problem& problem, + const RoadmapPtr_t& roadmap); hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t eetObj() const; int nRandomConfig() const; void nRandomConfig(int n); @@ -81,6 +83,6 @@ struct EndEffectorTrajectory : public pyhpp::core::PathPlanner { void exposePathPlanners(); -} // namespace manipulation -} // namespace pyhpp -#endif // PYHPP_MANIPULATION_PATH_PLANNER_HH \ No newline at end of file +} // namespace manipulation +} // namespace pyhpp +#endif // PYHPP_MANIPULATION_PATH_PLANNER_HH diff --git a/src/pyhpp/manipulation/steering-method.cc b/src/pyhpp/manipulation/steering-method.cc index e7ea1606..bea7cab8 100644 --- a/src/pyhpp/manipulation/steering-method.cc +++ b/src/pyhpp/manipulation/steering-method.cc @@ -30,8 +30,8 @@ #include <../src/pyhpp/manipulation/steering-method.hh> #include #include -#include #include +#include namespace pyhpp { namespace manipulation { @@ -45,19 +45,19 @@ GraphSteeringMethod::GraphSteeringMethod( obj = graph; } - hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t EndEffectorTrajectorySteeringMethod::eetObj() const { - assert(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::steeringMethod::EndEffectorTrajectory, - obj)); - return HPP_STATIC_PTR_CAST(hpp::manipulation::steeringMethod::EndEffectorTrajectory, - obj); + assert(HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::steeringMethod::EndEffectorTrajectory, obj)); + return HPP_STATIC_PTR_CAST( + hpp::manipulation::steeringMethod::EndEffectorTrajectory, obj); } EndEffectorTrajectorySteeringMethod::EndEffectorTrajectorySteeringMethod( - const hpp::core::ProblemConstPtr_t& problem) - : SteeringMethod(hpp::manipulation::steeringMethod::EndEffectorTrajectory::create(problem)) { -} + const hpp::core::ProblemConstPtr_t& problem) + : SteeringMethod( + hpp::manipulation::steeringMethod::EndEffectorTrajectory::create( + problem)) {} void EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint( const hpp::constraints::ImplicitPtr_t& ic) { @@ -74,14 +74,15 @@ void exposeManipSteeringMethod() { "GraphSteeringMethod", boost::python::init()); - class_>( - "EndEffectorTrajectorySteeringMethod", - boost::python::init()) - .def("setTrajectoryConstraint", - &EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint) - .def("setTrajectory", - &EndEffectorTrajectorySteeringMethod::setTrajectory); + class_>( + "EndEffectorTrajectorySteeringMethod", + boost::python::init()) + .def("setTrajectoryConstraint", + &EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint) + .def("setTrajectory", + &EndEffectorTrajectorySteeringMethod::setTrajectory); } } // namespace manipulation -} // namespace pyhpp \ No newline at end of file +} // namespace pyhpp diff --git a/src/pyhpp/manipulation/steering-method.hh b/src/pyhpp/manipulation/steering-method.hh index 56a0fb39..ffb4a1c9 100644 --- a/src/pyhpp/manipulation/steering-method.hh +++ b/src/pyhpp/manipulation/steering-method.hh @@ -19,11 +19,11 @@ #ifndef PYHPP_MANIPULATION_STEERING_METHOD_HH #define PYHPP_MANIPULATION_STEERING_METHOD_HH -#include -#include #include -#include +#include +#include #include +#include #include #include @@ -38,12 +38,13 @@ struct GraphSteeringMethod { GraphSteeringMethod(const PyWSteeringMethodPtr_t& steeringMethodWrapper); }; -struct EndEffectorTrajectorySteeringMethod : pyhpp::core::SteeringMethod{ +struct EndEffectorTrajectorySteeringMethod : pyhpp::core::SteeringMethod { hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t eetObj() const; - EndEffectorTrajectorySteeringMethod(const hpp::core::ProblemConstPtr_t& problem); + EndEffectorTrajectorySteeringMethod( + const hpp::core::ProblemConstPtr_t& problem); void setTrajectoryConstraint(const hpp::constraints::ImplicitPtr_t& ic); void setTrajectory(const hpp::core::PathPtr_t& eeTraj, bool se3Output); }; } // namespace manipulation } // namespace pyhpp -#endif // PYHPP_MANIPULATION_STEERING_METHOD_HH \ No newline at end of file +#endif // PYHPP_MANIPULATION_STEERING_METHOD_HH From e937204ebd38b8bf19794a381530ff8cad0f91ea Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 10:18:10 +0000 Subject: [PATCH 016/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/pyhpp/core/problem.hh | 2 ++ src/pyhpp/core/problem.cc | 50 ++++++++++++++++++++++++++++++++++- src/pyhpp/core/roadmap.cc | 19 ++++++++++++- 3 files changed, 69 insertions(+), 2 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index cc2ee56a..718cade3 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -151,6 +151,8 @@ struct Problem { const char* jointRName, const hpp::pinocchio::vector3_t& pointL, const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); + + boost::python::tuple directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate); hpp::core::ConstraintSetPtr_t constraints_; value_type errorThreshold_; size_type maxIterProjection_; diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 5d33510e..d37a6f04 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -477,6 +478,52 @@ hpp::constraints::ImplicitPtr_t Problem::createComBetweenFeet( } } +boost::python::tuple Problem::directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate) { + std::string report = ""; + + if (!obj) throw std::runtime_error("The problem is not defined."); + + SteeringMethodPtr_t sm(obj->steeringMethod()); + PathPtr_t dp = (*sm)(start, end); + if (!dp) { + report = "Steering method failed to build a path."; + return boost::python::make_tuple(false, nullptr, report); + } + + PathPtr_t dp1, dp2; + hpp::core::PathValidationReportPtr_t r; + bool projValid = true, projected = true, pathValid = true; + + if (validate) { + PathProjectorPtr_t proj(obj->pathProjector()); + if (proj) { + projected = proj->apply(dp, dp1); + } else { + dp1 = dp; + } + projValid = obj->pathValidation()->validate(dp1, false, dp2, r); + pathValid = projValid && projected; + if (!projValid) { + if (r) { + std::ostringstream oss; + oss << *r; + report = oss.str(); + } else { + report = "No path validation report."; + } + } + } else { + dp2 = dp; + } + + hpp::core::PathVectorPtr_t path( + hpp::core::PathVector::create(dp2->outputSize(), dp2->outputDerivativeSize())); + path->appendPath(dp2); + return boost::python::make_tuple(pathValid, path, report); +} + + + typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -623,7 +670,8 @@ void exposeProblem() { &Problem::addNumericalConstraintsToConfigProjector2) .def_readwrite("errorThreshold", &Problem::errorThreshold_) .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) - .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet); + .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet) + .PYHPP_DEFINE_METHOD(Problem, directPath); register_problem_converters(); diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index e2aaf422..ac23a6ca 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -69,6 +70,21 @@ struct RWrapper { return; } + + static void addEdge2(Roadmap& roadmap, const ConfigurationIn_t from, + ConfigurationIn_t to, const PathPtr_t& path, bool bothEdges) { + NodePtr_t nodeFrom = roadmap.addNode(from); + NodePtr_t nodeTo = roadmap.addNode(to); + if (bothEdges) { + roadmap.addEdge(nodeFrom, nodeTo, path); + roadmap.addEdge(nodeTo, nodeFrom, path->reverse()); + return; + } + roadmap.addEdge(nodeFrom, nodeTo, path); + return; + } + + static boost::python::tuple nearestNode1(Roadmap& roadmap, ConfigurationIn_t configuration, bool reverse) { @@ -192,7 +208,8 @@ void exposeRoadmap() { return_value_policy()) .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdge, return_value_policy()) - .PYHPP_DEFINE_METHOD(RWrapper, addEdge) + .def("addEdge", &RWrapper::addEdge) + .def("addEdge", &RWrapper::addEdge2) .PYHPP_DEFINE_METHOD(Roadmap, addEdges) .def("merge", static_cast(&Roadmap::merge)) From f2e403ff8ee51a24d63ef5933cd0a89ea79787c0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 11:40:19 +0000 Subject: [PATCH 017/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/pyhpp/core/problem.hh | 5 +++-- src/pyhpp/core/problem.cc | 21 ++++++++++----------- src/pyhpp/core/roadmap.cc | 7 +++---- 3 files changed, 16 insertions(+), 17 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 718cade3..607e56e0 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -151,8 +151,9 @@ struct Problem { const char* jointRName, const hpp::pinocchio::vector3_t& pointL, const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); - - boost::python::tuple directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate); + + boost::python::tuple directPath(ConfigurationIn_t start, + ConfigurationIn_t end, bool validate); hpp::core::ConstraintSetPtr_t constraints_; value_type errorThreshold_; size_type maxIterProjection_; diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index d37a6f04..81338272 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -42,8 +42,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -478,22 +478,23 @@ hpp::constraints::ImplicitPtr_t Problem::createComBetweenFeet( } } -boost::python::tuple Problem::directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate) { +boost::python::tuple Problem::directPath(ConfigurationIn_t start, + ConfigurationIn_t end, bool validate) { std::string report = ""; - + if (!obj) throw std::runtime_error("The problem is not defined."); - + SteeringMethodPtr_t sm(obj->steeringMethod()); PathPtr_t dp = (*sm)(start, end); if (!dp) { report = "Steering method failed to build a path."; return boost::python::make_tuple(false, nullptr, report); } - + PathPtr_t dp1, dp2; hpp::core::PathValidationReportPtr_t r; bool projValid = true, projected = true, pathValid = true; - + if (validate) { PathProjectorPtr_t proj(obj->pathProjector()); if (proj) { @@ -515,15 +516,13 @@ boost::python::tuple Problem::directPath(ConfigurationIn_t start, ConfigurationI } else { dp2 = dp; } - - hpp::core::PathVectorPtr_t path( - hpp::core::PathVector::create(dp2->outputSize(), dp2->outputDerivativeSize())); + + hpp::core::PathVectorPtr_t path(hpp::core::PathVector::create( + dp2->outputSize(), dp2->outputDerivativeSize())); path->appendPath(dp2); return boost::python::make_tuple(pathValid, path, report); } - - typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index ac23a6ca..4dd4905e 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -28,8 +28,8 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include +#include #include #include @@ -70,9 +70,9 @@ struct RWrapper { return; } - static void addEdge2(Roadmap& roadmap, const ConfigurationIn_t from, - ConfigurationIn_t to, const PathPtr_t& path, bool bothEdges) { + ConfigurationIn_t to, const PathPtr_t& path, + bool bothEdges) { NodePtr_t nodeFrom = roadmap.addNode(from); NodePtr_t nodeTo = roadmap.addNode(to); if (bothEdges) { @@ -84,7 +84,6 @@ struct RWrapper { return; } - static boost::python::tuple nearestNode1(Roadmap& roadmap, ConfigurationIn_t configuration, bool reverse) { From 780c24c7b1a33b0deda0a6d21c7b82b4d985a7e3 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 27 Oct 2025 13:37:21 +0100 Subject: [PATCH 018/109] Further homogenization --- src/pyhpp/core/path-projector.cc | 10 +++++----- src/pyhpp/core/path-validation.cc | 12 ++++++------ src/pyhpp/manipulation/path-projector.cc | 18 +++++++++--------- tests/benchmarks/pyrene-on-the-ground.py | 8 ++++---- tests/benchmarks/ur3-spheres-spf.py | 8 ++++---- tests/benchmarks/ur3-spheres.py | 8 ++++---- 6 files changed, 32 insertions(+), 32 deletions(-) diff --git a/src/pyhpp/core/path-projector.cc b/src/pyhpp/core/path-projector.cc index a001d8cb..cac3bf01 100644 --- a/src/pyhpp/core/path-projector.cc +++ b/src/pyhpp/core/path-projector.cc @@ -78,7 +78,7 @@ void exposePathProjector() { "RecursiveHermiteProjector", no_init); def( - "createNoneProjector", + "NoneProjector", +[](const DistancePtr_t&, const PyWSteeringMethodPtr_t&, const value_type&) -> PathProjectorPtr_t { return PathProjectorPtr_t(); @@ -86,7 +86,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -96,7 +96,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -106,7 +106,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -116,7 +116,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { diff --git a/src/pyhpp/core/path-validation.cc b/src/pyhpp/core/path-validation.cc index 64fc95b0..d2c41e06 100644 --- a/src/pyhpp/core/path-validation.cc +++ b/src/pyhpp/core/path-validation.cc @@ -97,20 +97,20 @@ void exposePathValidation() { hpp::core::continuousValidation::DichotomyPtr_t, boost::noncopyable>( "Dichotomy", no_init); - def("createDiscretized", &pathValidation::createDiscretizedCollisionChecking, + def("Discretized", &pathValidation::createDiscretizedCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createDiscretizedCollision", + def("DiscretizedCollision", &pathValidation::createDiscretizedCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createDiscretizedJointBound", + def("DiscretizedJointBound", &pathValidation::createDiscretizedJointBound, (arg("robot"), arg("stepSize"))); - def("createDiscretizedCollisionAndJointBound", + def("DiscretizedCollisionAndJointBound", &PVWrapper::createDiscretizedJointBoundAndCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createProgressive", &continuousValidation::Progressive::create, + def("Progressive", &continuousValidation::Progressive::create, (arg("robot"), arg("tolerance"))); - def("createDichotomy", &continuousValidation::Dichotomy::create, + def("Dichotomy", &continuousValidation::Dichotomy::create, (arg("robot"), arg("tolerance"))); } } // namespace core diff --git a/src/pyhpp/manipulation/path-projector.cc b/src/pyhpp/manipulation/path-projector.cc index 418f2e7e..f420d24e 100644 --- a/src/pyhpp/manipulation/path-projector.cc +++ b/src/pyhpp/manipulation/path-projector.cc @@ -49,11 +49,11 @@ typedef pyhpp::core::PyWSteeringMethodPtr_t PyWSteeringMethodPtr_t; using namespace boost::python; void exposePathProjector() { def( - "createNoneProjector", + "NoneProjector", +[]() -> PathProjectorPtr_t { return PathProjectorPtr_t(); }); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -62,7 +62,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -72,7 +72,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -81,7 +81,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -91,7 +91,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -100,7 +100,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -110,7 +110,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -119,7 +119,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 9f282f91..881f42bc 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -10,8 +10,8 @@ from pyhpp.core import ( Problem, DiffusingPlanner, - createDichotomy, - createProgressiveProjector, + Dichotomy, + ProgressiveProjector, Straight, ) from pyhpp.constraints import LockedJoint @@ -133,9 +133,9 @@ "balance-proj", list(constraints.values()), [0] * len(constraints) ) # Set up path validation and projection -problem.pathValidation = createDichotomy(robot, 0) +problem.pathValidation = Dichotomy(robot, 0) problem.steeringMethod = Straight(problem) -problem.pathProjector = createProgressiveProjector( +problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.2 ) diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/benchmarks/ur3-spheres-spf.py index e0a516b9..3fde025e 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/benchmarks/ur3-spheres-spf.py @@ -15,11 +15,11 @@ Device, Graph, Problem, - createProgressiveProjector, + ProgressiveProjector, urdf, StatesPathFinder, ) -from pyhpp.core import createDichotomy, Straight +from pyhpp.core import Dichotomy, Straight from pyhpp.constraints import ( Transformation, @@ -281,10 +281,10 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = createDichotomy(robot.asPinDevice(), 0) +problem.pathValidation = Dichotomy(robot.asPinDevice(), 0) # need to set path projector due to implicit constraints added above -problem.pathProjector = createProgressiveProjector( +problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.01 ) diff --git a/tests/benchmarks/ur3-spheres.py b/tests/benchmarks/ur3-spheres.py index f52e9b48..1a48dd83 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/benchmarks/ur3-spheres.py @@ -7,11 +7,11 @@ Device, Graph, Problem, - createProgressiveProjector, + ProgressiveProjector, urdf, ManipulationPlanner, ) -from pyhpp.core import createDichotomy, Straight +from pyhpp.core import Dichotomy, Straight from pyhpp.constraints import ( Transformation, @@ -262,8 +262,8 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = createDichotomy(robot.asPinDevice(), 0) -problem.pathProjector = createProgressiveProjector( +problem.pathValidation = Dichotomy(robot.asPinDevice(), 0) +problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.01 ) From 6f51bc5b5e9952b4cd0d52f56d42a231d9fb40ca Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 12:40:05 +0000 Subject: [PATCH 019/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/path-validation.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/pyhpp/core/path-validation.cc b/src/pyhpp/core/path-validation.cc index d2c41e06..22988828 100644 --- a/src/pyhpp/core/path-validation.cc +++ b/src/pyhpp/core/path-validation.cc @@ -102,8 +102,7 @@ void exposePathValidation() { def("DiscretizedCollision", &pathValidation::createDiscretizedCollisionChecking, (arg("robot"), arg("stepSize"))); - def("DiscretizedJointBound", - &pathValidation::createDiscretizedJointBound, + def("DiscretizedJointBound", &pathValidation::createDiscretizedJointBound, (arg("robot"), arg("stepSize"))); def("DiscretizedCollisionAndJointBound", &PVWrapper::createDiscretizedJointBoundAndCollisionChecking, From 531eba1c448a2329048fb51cad4d3199181d7cd4 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 27 Oct 2025 13:58:51 +0100 Subject: [PATCH 020/109] Remove ConstraintResult structure and use boost::python::tuple to faciliate script conversion --- include/pyhpp/core/problem.hh | 12 +----------- src/pyhpp/core/problem.cc | 10 ++-------- src/pyhpp/manipulation/graph.cc | 18 +++++++++--------- src/pyhpp/manipulation/graph.hh | 18 ++++++------------ tests/benchmarks/pyrene-on-the-ground.py | 10 ++++------ tests/constraint_graph.py | 6 +++--- 6 files changed, 25 insertions(+), 49 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 607e56e0..595aa686 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -58,16 +58,6 @@ typedef hpp::core::value_type value_type; typedef hpp::core::size_type size_type; typedef hpp::core::Configuration_t Configuration_t; -struct ConstraintResult { - bool success; - Configuration_t configuration; - value_type error; - - ConstraintResult() : success(false), configuration(), error(0.0) {} - ConstraintResult(bool s, const Configuration_t& config, value_type err) - : success(s), configuration(config), error(err) {} -}; - // Wrapper class for hpp::core::Problem struct Problem { hpp::core::ProblemPtr_t obj; @@ -136,7 +126,7 @@ struct Problem { void setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, bool constant); - ConstraintResult applyConstraints(ConfigurationIn_t config); + boost::python::tuple applyConstraints(ConfigurationIn_t config); boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); void setConstraints(hpp::core::ConstraintSetPtr_t constraints); void setRightHandSideFromConfig(ConfigurationIn_t configIn); diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 81338272..b62c8188 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -325,7 +325,7 @@ void Problem::setConstantRightHandSide( } } -ConstraintResult Problem::applyConstraints(ConfigurationIn_t config) { +boost::python::tuple Problem::applyConstraints(ConfigurationIn_t config) { bool success = false; double residualError = 0.0; try { @@ -335,8 +335,7 @@ ConstraintResult Problem::applyConstraints(ConfigurationIn_t config) { constraints_->configProjector()) { residualError = configProjector->residualError(); } - ConstraintResult result(success, output, residualError); - return result; + return boost::python::make_tuple(success, output, residualError); } catch (const std::exception& exc) { throw std::logic_error(exc.what()); } @@ -673,11 +672,6 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, directPath); register_problem_converters(); - - class_("ConstraintResult") - .def_readwrite("success", &ConstraintResult::success) - .def_readwrite("configuration", &ConstraintResult::configuration) - .def_readwrite("error", &ConstraintResult::error); } } // namespace core diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 2103aa9e..33c25fb3 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -155,19 +155,19 @@ const char* DOC_GETCONFIGERRORFORTRANSITIONTARGET = const char* DOC_APPLYSTATECONSTRAINTS = "Apply constraints to a configuration. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm."; const char* DOC_APPLYLEAFCONSTRAINTS = "Apply transition constraints to a configuration. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm. " "If success, the output configuration is reachable from q_rhs along the " "transition."; const char* DOC_GENERATETARGETCONFIG = "Generate configuration in destination state on a given leaf. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm. " "Computes a configuration in the destination state of the transition, " "reachable from q_rhs."; @@ -730,7 +730,7 @@ boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget( // Constraint application // ============================================================================= -pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints( +boost::python::tuple PyWGraph::applyStateConstraints( PyWStatePtr_t state, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(state->obj->configConstraint()); @@ -742,10 +742,10 @@ pyhpp::core::ConstraintResult PyWGraph::applyStateConstraints( constraint->configProjector()) { residualError = configProjector->residualError(); } - return pyhpp::core::ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } -pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints( +boost::python::tuple PyWGraph::applyLeafConstraints( PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->pathConstraint()); @@ -758,10 +758,10 @@ pyhpp::core::ConstraintResult PyWGraph::applyLeafConstraints( success = constraint->apply(output); residualError = constraint->configProjector()->residualError(); } - return pyhpp::core::ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } -pyhpp::core::ConstraintResult PyWGraph::generateTargetConfig( +boost::python::tuple PyWGraph::generateTargetConfig( PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->targetConstraint()); @@ -777,7 +777,7 @@ pyhpp::core::ConstraintResult PyWGraph::generateTargetConfig( residualError = configProjector->residualError(); } - return pyhpp::core::ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } // ============================================================================= diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 19e43da6..4076f703 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -33,12 +33,6 @@ #include #include -namespace pyhpp { -namespace core { -struct ConstraintResult; -} -} // namespace pyhpp - namespace pyhpp { namespace manipulation { @@ -209,12 +203,12 @@ struct PyWGraph { void removeCollisionPairFromTransition(PyWEdgePtr_t edge, const char* joint1, const char* joint2); - pyhpp::core::ConstraintResult applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input); - pyhpp::core::ConstraintResult applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); - pyhpp::core::ConstraintResult generateTargetConfig(PyWEdgePtr_t transition, + boost::python::tuple applyStateConstraints(PyWStatePtr_t state, + ConfigurationIn_t input); + boost::python::tuple applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); + boost::python::tuple generateTargetConfig(PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input); // Subgraph management diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 881f42bc..a1409fa3 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -251,14 +251,12 @@ ] ) -result = problem.applyConstraints(q1) -assert result.success -q1proj = result.configuration +result, q1proj, err = problem.applyConstraints(q1) +assert result valid = problem.isConfigValid(q1proj) assert valid[0] -result = problem.applyConstraints(q2) -assert result.success -q2proj = result.configuration +result, q2proj, err = problem.applyConstraints(q2) +assert result assert problem.isConfigValid(q2proj) problem.initConfig(q1proj) diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py index edc5b47c..1c8da90c 100644 --- a/tests/constraint_graph.py +++ b/tests/constraint_graph.py @@ -162,7 +162,7 @@ for i in range(100): q = configurationShooter.shoot() - res = graph.applyStateConstraints(state_placement, q) - if res.success: - print("after applying constraints: ", res.configuration) + res, config, err = graph.applyStateConstraints(state_placement, q) + if res: + print("after applying constraints: ", config) break From 9878bc796175cf474dade42988832933a6e00fe5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 27 Oct 2025 12:59:19 +0000 Subject: [PATCH 021/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/manipulation/graph.cc | 14 ++++++++------ src/pyhpp/manipulation/graph.hh | 10 +++++----- 2 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 33c25fb3..723ba9ad 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -730,8 +730,8 @@ boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget( // Constraint application // ============================================================================= -boost::python::tuple PyWGraph::applyStateConstraints( - PyWStatePtr_t state, ConfigurationIn_t input) { +boost::python::tuple PyWGraph::applyStateConstraints(PyWStatePtr_t state, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(state->obj->configConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -745,8 +745,9 @@ boost::python::tuple PyWGraph::applyStateConstraints( return boost::python::make_tuple(success, output, residualError); } -boost::python::tuple PyWGraph::applyLeafConstraints( - PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { +boost::python::tuple PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->pathConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -761,8 +762,9 @@ boost::python::tuple PyWGraph::applyLeafConstraints( return boost::python::make_tuple(success, output, residualError); } -boost::python::tuple PyWGraph::generateTargetConfig( - PyWEdgePtr_t transition, ConfigurationIn_t q_rhs, ConfigurationIn_t input) { +boost::python::tuple PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->targetConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 4076f703..9befbc90 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -204,13 +204,13 @@ struct PyWGraph { const char* joint2); boost::python::tuple applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input); - boost::python::tuple applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, ConfigurationIn_t input); + boost::python::tuple applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); boost::python::tuple generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); // Subgraph management void createSubGraph(const char* subgraphName, hpp::core::RoadmapPtr_t roadmap); From 238c62fbf426958a15434c58ee3a5f2bd70517a1 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 28 Oct 2025 11:35:07 +0100 Subject: [PATCH 022/109] Improve api and fix test --- src/pyhpp/core/problem.cc | 31 +++++++++++++--------------- src/pyhpp/pinocchio/device.cc | 38 ++++++++++++++++++++++++++++++++--- tests/constraint_graph.py | 4 ++-- 3 files changed, 51 insertions(+), 22 deletions(-) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index b62c8188..ab422fee 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -228,34 +228,29 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( const hpp::pinocchio::Transform3s& M, boost::python::list m) { try { if (!robot()) throw std::logic_error("No robot loaded"); - using hpp::constraints::GenericTransformation; using hpp::constraints::Implicit; using hpp::constraints::OrientationBit; using hpp::constraints::PositionBit; - + std::string name(constraintName); - hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); auto mask = extract_vector(m); - - const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M; - + + const hpp::pinocchio::Transform3s ref2 = M; + if (joint1Name && std::strlen(joint1Name) > 0) { - // Relative transformation constraint between two frames hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); - const hpp::pinocchio::Transform3s ref1 = - f1.positionInParentJoint() * hpp::pinocchio::Transform3s::Identity(); + + const hpp::pinocchio::Transform3s ref1 = + hpp::pinocchio::Transform3s::Identity(); auto func = GenericTransformation< - PositionBit | OrientationBit | - hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), - f2.joint(), ref1, ref2, mask); + PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create( + name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { - // Absolute transformation of frame/joint2 to M in world frame - const hpp::pinocchio::Transform3s Id = - hpp::pinocchio::Transform3s::Identity(); + const hpp::pinocchio::Transform3s Id = hpp::pinocchio::Transform3s::Identity(); auto func = GenericTransformation::create( name, robot(), f2.joint(), ref2, Id, mask); return Implicit::create( @@ -655,8 +650,10 @@ void exposeProblem() { .PYHPP_DEFINE_METHOD(Problem, addPartialCom) .PYHPP_DEFINE_METHOD(Problem, getPartialCom) .PYHPP_DEFINE_METHOD(Problem, createRelativeComConstraint) - .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint) - .PYHPP_DEFINE_METHOD(Problem, createTransformationConstraint2) + .def("createTransformationConstraint", + &Problem::createTransformationConstraint) + .def("createTransformationConstraint", + &Problem::createTransformationConstraint2) .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) .PYHPP_DEFINE_METHOD(Problem, applyConstraints) .PYHPP_DEFINE_METHOD(Problem, isConfigValid) diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index c56a636a..ed461b39 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -131,6 +131,40 @@ static void setJointBounds(Device& device, const char* jointName, } } +typedef hpp::pinocchio::FrameIndex FrameIndex; +typedef hpp::pinocchio::SE3 SE3; +static boost::python::list getJointPosition(Device& device, const std::string& jointName) { + try { + const Model& model(device.model()); + const Data& data(device.data()); + + if (!model.existFrame(jointName)) { + throw std::logic_error("Robot has no frame with name " + jointName); + } + + FrameIndex joint = model.getFrameId(jointName); + if (model.frames.size() <= (std::size_t)joint) + throw std::logic_error("Frame index of joint " + jointName + + " out of bounds: " + std::to_string(joint)); + + const SE3& M = data.oMf[joint]; + Transform3s::Quaternion q(M.rotation()); + + boost::python::list t; + t.append(M.translation()[0]); + t.append(M.translation()[1]); + t.append(M.translation()[2]); + t.append(q.x()); + t.append(q.y()); + t.append(q.z()); + t.append(q.w()); + + return t; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + static boost::python::dict rankInConfiguration(Device& device) { boost::python::dict rank_dict; try { @@ -150,9 +184,6 @@ static boost::python::dict rankInConfiguration(Device& device) { return rank_dict; } -typedef hpp::pinocchio::FrameIndex FrameIndex; -typedef hpp::pinocchio::SE3 SE3; - static boost::python::list getJointsPosition( Device& device, const Configuration_t& dofArray, const boost::python::list& jointNames) { @@ -250,6 +281,7 @@ void exposeDevice() { .add_property("rankInConfiguration", &rankInConfiguration) .def("setJointBounds", &setJointBounds) .def("getCenterOfMass", &getCenterOfMass) + .def("getJointPosition", &getJointPosition) .def("getJointsPosition", &getJointsPosition); } } // namespace pinocchio diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py index 1c8da90c..09494c24 100644 --- a/tests/constraint_graph.py +++ b/tests/constraint_graph.py @@ -23,8 +23,8 @@ 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.25, 0, 0])) -r1_pose = SE3(rotation=np.identity(3), translation=np.array([1, 0, 0])) +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") From 56e91fed808b1965026b3c3366c6e9512fa2fe71 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 28 Oct 2025 10:35:37 +0000 Subject: [PATCH 023/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/problem.cc | 20 +++++++++++--------- src/pyhpp/pinocchio/device.cc | 13 +++++++------ 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index ab422fee..b4039063 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -232,25 +232,27 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( using hpp::constraints::Implicit; using hpp::constraints::OrientationBit; using hpp::constraints::PositionBit; - + std::string name(constraintName); hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); auto mask = extract_vector(m); - - const hpp::pinocchio::Transform3s ref2 = M; - + + const hpp::pinocchio::Transform3s ref2 = M; + if (joint1Name && std::strlen(joint1Name) > 0) { hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); - - const hpp::pinocchio::Transform3s ref1 = + + const hpp::pinocchio::Transform3s ref1 = hpp::pinocchio::Transform3s::Identity(); auto func = GenericTransformation< - PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create( - name, robot(), f1.joint(), f2.joint(), ref1, ref2, mask); + PositionBit | OrientationBit | + hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { - const hpp::pinocchio::Transform3s Id = hpp::pinocchio::Transform3s::Identity(); + const hpp::pinocchio::Transform3s Id = + hpp::pinocchio::Transform3s::Identity(); auto func = GenericTransformation::create( name, robot(), f2.joint(), ref2, Id, mask); return Implicit::create( diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index ed461b39..7ca4bce0 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -133,23 +133,24 @@ static void setJointBounds(Device& device, const char* jointName, typedef hpp::pinocchio::FrameIndex FrameIndex; typedef hpp::pinocchio::SE3 SE3; -static boost::python::list getJointPosition(Device& device, const std::string& jointName) { +static boost::python::list getJointPosition(Device& device, + const std::string& jointName) { try { const Model& model(device.model()); const Data& data(device.data()); - + if (!model.existFrame(jointName)) { throw std::logic_error("Robot has no frame with name " + jointName); } - + FrameIndex joint = model.getFrameId(jointName); if (model.frames.size() <= (std::size_t)joint) throw std::logic_error("Frame index of joint " + jointName + " out of bounds: " + std::to_string(joint)); - + const SE3& M = data.oMf[joint]; Transform3s::Quaternion q(M.rotation()); - + boost::python::list t; t.append(M.translation()[0]); t.append(M.translation()[1]); @@ -158,7 +159,7 @@ static boost::python::list getJointPosition(Device& device, const std::string& j t.append(q.y()); t.append(q.z()); t.append(q.w()); - + return t; } catch (const std::exception& exc) { throw std::logic_error(exc.what()); From a9a3cc7bb538fc38d489b094d2043b6ebe5d2863 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Thu, 30 Oct 2025 12:44:47 +0000 Subject: [PATCH 024/109] [Graph] Add access to PathValidation in transition. --- src/pyhpp/manipulation/graph.cc | 7 ++++++- src/pyhpp/manipulation/graph.hh | 3 +++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 723ba9ad..1565fddb 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -895,6 +895,10 @@ void PyWGraph::removeCollisionPairFromTransition(PyWEdgePtr_t edge, } } +PathValidationPtr_t PyWEdge::pathValidation() const { + return obj->pathValidation(); +} + // ============================================================================= // Subgraph management // ============================================================================= @@ -1210,7 +1214,8 @@ void exposeGraph() { .def("name", &PyWState::name); class_("Transition", no_init) - .def("name", &PyWEdge::name); + .def("name", &PyWEdge::name) + .def("pathValidation", &PyWEdge::pathValidation); class_( "Graph", diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index 9befbc90..d1befd58 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -30,12 +30,14 @@ #ifndef PYHPP_GRAPH_HH #define PYHPP_GRAPH_HH +#include #include #include namespace pyhpp { namespace manipulation { +typedef hpp::core::PathValidationPtr_t PathValidationPtr_t; typedef hpp::manipulation::Configuration_t Configuration_t; typedef hpp::manipulation::ConfigurationIn_t ConfigurationIn_t; typedef hpp::manipulation::size_type size_type; @@ -65,6 +67,7 @@ struct PyWEdge { EdgePtr_t obj; PyWEdge(const EdgePtr_t& object); std::string name() const; + PathValidationPtr_t pathValidation() const; }; typedef std::shared_ptr PyWEdgePtr_t; From 0b55cf1efd89c86a696e9a3a2db8f3fc72d2929a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 3 Nov 2025 17:14:31 +0000 Subject: [PATCH 025/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.2 → v0.14.3](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.2...v0.14.3) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5691e9a1..eaf228ef 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.2 + rev: v0.14.3 hooks: - id: ruff args: From ef5e6f8523d1809d022d2aa6e7f9b6f2a5b67bbb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 10 Nov 2025 17:17:18 +0000 Subject: [PATCH 026/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.3 → v0.14.4](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.3...v0.14.4) - [github.com/pre-commit/mirrors-clang-format: v21.1.2 → v21.1.5](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.2...v21.1.5) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index eaf228ef..5188d045 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.3 + rev: v0.14.4 hooks: - id: ruff args: @@ -18,7 +18,7 @@ repos: hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.2 + rev: v21.1.5 hooks: - id: clang-format args: From 6dfec2d879329949b89ec5b68269c7390cabee68 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 12 Nov 2025 17:27:54 +0100 Subject: [PATCH 027/109] [ConnectedComponent] Bind some more methods giving access to upstream and downstream connected components. --- src/pyhpp/core/connected-component.cc | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/core/connected-component.cc b/src/pyhpp/core/connected-component.cc index fdc31841..2fb85382 100644 --- a/src/pyhpp/core/connected-component.cc +++ b/src/pyhpp/core/connected-component.cc @@ -49,11 +49,29 @@ struct CCWrapper { } return to_python_list(res); } + static boost::python::list reachableFrom(ConnectedComponent& cc) { + std::vector res; + for(ConnectedComponent::RawPtrs_t::const_iterator itcc = cc.reachableFrom().begin(); + itcc != cc.reachableFrom().end(); ++itcc) { + res.push_back((*itcc)->self()); + } + return to_python_list(res); + } + static boost::python::list reachableTo(ConnectedComponent& cc) { + std::vector res; + for(ConnectedComponent::RawPtrs_t::const_iterator itcc = cc.reachableTo().begin(); + itcc != cc.reachableTo().end(); ++itcc) { + res.push_back((*itcc)->self()); + } + return to_python_list(res); + } }; void exposeConnectedComponent() { class_( "ConnectedComponent", no_init) - .def("nodes", &CCWrapper::nodes); + .def("nodes", &CCWrapper::nodes) + .def("reachableFrom", &CCWrapper::reachableFrom) + .def("reachableTo", &CCWrapper::reachableTo); } } // namespace core } // namespace pyhpp From cf90356023a5ecce7b6e9211956434bcbc741c9a Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 12 Nov 2025 17:28:54 +0100 Subject: [PATCH 028/109] [Roadmap] Improve addNodeAndEdge(s) check that first input node belongs to the roadmap and make implementation closer to C++. --- src/pyhpp/core/roadmap.cc | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index 4dd4905e..dca87732 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -48,14 +48,29 @@ using namespace hpp::core; struct RWrapper { static void addNodeAndEdges(Roadmap& roadmap, const ConfigurationIn_t from, ConfigurationIn_t to, const PathPtr_t path) { - NodePtr_t nodeFrom = roadmap.addNode(from); + value_type d; + NodePtr_t nodeFrom = roadmap.nearestNode(from, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::addNodeAndEdge: initial configuration (" << from + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } roadmap.addNodeAndEdges(nodeFrom, to, path); } static void addNodeAndEdge(Roadmap& roadmap, const ConfigurationIn_t from, ConfigurationIn_t to, const PathPtr_t path) { - NodePtr_t nodeFrom = roadmap.addNode(from); - roadmap.addNodeAndEdge(nodeFrom, to, path); + value_type d; + NodePtr_t nodeFrom = roadmap.nearestNode(from, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::addNodeAndEdge: initial configuration (" << from + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } + NodePtr_t nodeTo = roadmap.addNode(to); + roadmap.addEdge(nodeFrom, nodeTo, path); } static void addNode(Roadmap& roadmap, const ConfigurationIn_t config) { From e523278983e24e90857e1d7f89523432616f7756 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 12 Nov 2025 16:36:29 +0000 Subject: [PATCH 029/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/connected-component.cc | 10 ++++++---- src/pyhpp/core/roadmap.cc | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/core/connected-component.cc b/src/pyhpp/core/connected-component.cc index 2fb85382..35ee18ef 100644 --- a/src/pyhpp/core/connected-component.cc +++ b/src/pyhpp/core/connected-component.cc @@ -51,16 +51,18 @@ struct CCWrapper { } static boost::python::list reachableFrom(ConnectedComponent& cc) { std::vector res; - for(ConnectedComponent::RawPtrs_t::const_iterator itcc = cc.reachableFrom().begin(); - itcc != cc.reachableFrom().end(); ++itcc) { + for (ConnectedComponent::RawPtrs_t::const_iterator itcc = + cc.reachableFrom().begin(); + itcc != cc.reachableFrom().end(); ++itcc) { res.push_back((*itcc)->self()); } return to_python_list(res); } static boost::python::list reachableTo(ConnectedComponent& cc) { std::vector res; - for(ConnectedComponent::RawPtrs_t::const_iterator itcc = cc.reachableTo().begin(); - itcc != cc.reachableTo().end(); ++itcc) { + for (ConnectedComponent::RawPtrs_t::const_iterator itcc = + cc.reachableTo().begin(); + itcc != cc.reachableTo().end(); ++itcc) { res.push_back((*itcc)->self()); } return to_python_list(res); diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index dca87732..c3a40f34 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -53,7 +53,7 @@ struct RWrapper { if (d > 0) { std::ostringstream os; os << "Roadmap::addNodeAndEdge: initial configuration (" << from - << ") not in the roadmap"; + << ") not in the roadmap"; throw std::logic_error(os.str().c_str()); } roadmap.addNodeAndEdges(nodeFrom, to, path); @@ -66,7 +66,7 @@ struct RWrapper { if (d > 0) { std::ostringstream os; os << "Roadmap::addNodeAndEdge: initial configuration (" << from - << ") not in the roadmap"; + << ") not in the roadmap"; throw std::logic_error(os.str().c_str()); } NodePtr_t nodeTo = roadmap.addNode(to); From 84b0a1b3511691f43cd9399c64e7957de9ccff35 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 29 Oct 2025 14:10:04 +0100 Subject: [PATCH 030/109] Add pyhpp coverage --- CMakeLists.txt | 6 +++ include/pyhpp/coverage/fwd.hh | 42 ++++++++++++++++++ src/CMakeLists.txt | 9 ++++ src/pyhpp/coverage/bindings.cc | 38 +++++++++++++++++ src/pyhpp/coverage/path.cc | 78 ++++++++++++++++++++++++++++++++++ 5 files changed, 173 insertions(+) create mode 100644 include/pyhpp/coverage/fwd.hh create mode 100644 src/pyhpp/coverage/bindings.cc create mode 100644 src/pyhpp/coverage/path.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b2abe1..e9c891b2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,6 +85,7 @@ set(${PROJECT_NAME}_HEADERS include/pyhpp/manipulation/urdf/fwd.hh include/pyhpp/pinocchio/fwd.hh include/pyhpp/pinocchio/urdf/fwd.hh + include/pyhpp/coverage/fwd.hh include/pyhpp/ref.hh include/pyhpp/stl-pair.hh include/pyhpp/util.hh @@ -104,6 +105,11 @@ add_project_dependency(hpp-corbaserver REQUIRED) add_project_dependency(hpp-manipulation REQUIRED) add_project_dependency(hpp-manipulation-urdf REQUIRED) +option(BUILD_HPP_COVERAGE "Build with hpp-coverage support" ON) +if(BUILD_HPP_COVERAGE) + add_project_dependency(hpp-coverage REQUIRED) +endif() + if(BUILD_TESTING) find_package(example-robot-data REQUIRED) find_package(hpp-environments REQUIRED) diff --git a/include/pyhpp/coverage/fwd.hh b/include/pyhpp/coverage/fwd.hh new file mode 100644 index 00000000..0a454875 --- /dev/null +++ b/include/pyhpp/coverage/fwd.hh @@ -0,0 +1,42 @@ +// +// Copyright (c) 2025 CNRS +// Authors: Paul Sardin +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef PYHPP_COVERAGE_FWD_HH +#define PYHPP_COVERAGE_FWD_HH + +#include + +namespace pyhpp { +namespace coverage { +void exposeCoverage(); + +} // namespace coverage +} // namespace pyhpp + +#endif // PYHPP_COVERAGE_FWD_HH diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ebf76193..ac89f0cd 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -166,3 +166,12 @@ add_python_library( pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES hpp-manipulation-urdf::hpp-manipulation-urdf) + +if(BUILD_HPP_COVERAGE) + add_python_library( + pyhpp/coverage + FILES + pyhpp/coverage/path.cc + LINK_LIBRARIES + hpp-coverage::hpp-coverage) +endif() diff --git a/src/pyhpp/coverage/bindings.cc b/src/pyhpp/coverage/bindings.cc new file mode 100644 index 00000000..9566495b --- /dev/null +++ b/src/pyhpp/coverage/bindings.cc @@ -0,0 +1,38 @@ +// +// Copyright (c) 2025, CNRS +// Authors: Paul Sardin +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +BOOST_PYTHON_MODULE(bindings) { + INIT_PYHPP_MODULE; + + pyhpp::coverage::exposeCoverage(); + } diff --git a/src/pyhpp/coverage/path.cc b/src/pyhpp/coverage/path.cc new file mode 100644 index 00000000..6ca8b742 --- /dev/null +++ b/src/pyhpp/coverage/path.cc @@ -0,0 +1,78 @@ +// +// Copyright (c) 2025 CNRS +// Author: Paul Sardin +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::python; + +namespace pyhpp { +namespace coverage { +using hpp::coverage::Path; +using hpp::core::PathPtr_t; +using hpp::core::PathVector; +using hpp::core::PathVectorPtr_t; + +static PathPtr_t multiply(const PathPtr_t& p1, const PathPtr_t& p2) { + Path ops; + return ops.multiply(p1, p2); +} + +static PathPtr_t createSpline(hpp::core::ConfigurationIn_t pose0, + hpp::core::ConfigurationIn_t pose1, + hpp::core::value_type length, + hpp::core::size_type order) { + Path ops; + return ops.createSpline(pose0, pose1, length, order); +} + +static void setCost(hpp::core::Roadmap& roadmap, + const hpp::pinocchio::DevicePtr_t& robot, + const std::string& gripperName) { + roadmap.cost(hpp::coverage::ToolRotation::create(robot, gripperName)); +} + +BOOST_PYTHON_MODULE(bindings) { + boost::python::import("pyhpp.core"); + + class_, boost::noncopyable>("CoveragePath", no_init) + .def("create", +[]() { return ::hpp::shared_ptr(new Path()); }) + .staticmethod("create") + .def("multiply", &Path::multiply) + .def("createSpline", &Path::createSpline) + .def("setCost", &setCost) + .staticmethod("setCost"); +} + +} // namespace coverage +} // namespace pyhpp \ No newline at end of file From 91ee3ecab17c3435fee0dc99295b6661b5ed49f1 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 13 Nov 2025 08:17:41 +0100 Subject: [PATCH 031/109] Homogeinize object creation function, using constructors instead of static create methods --- src/pyhpp/constraints/generic-transformation.cc | 9 ++------- src/pyhpp/constraints/implicit.cc | 3 +-- src/pyhpp/constraints/locked-joint.cc | 6 ++---- src/pyhpp/manipulation/constraint_graph_factory.py | 2 +- src/pyhpp/pinocchio/device.cc | 3 +-- tests/benchmarks/pyrene-on-the-ground.py | 6 +++--- tests/benchmarks/ur3-spheres-spf.py | 12 ++++++------ tests/benchmarks/ur3-spheres.py | 12 ++++++------ tests/constraint_graph.py | 8 ++++---- tests/graph_factory.py | 4 ++-- tests/graph_factory2.py | 12 ++++++------ tests/load_ur3.py | 2 +- tests/path-planner.py | 2 +- tests/rrt.py | 2 +- tests/solver.py | 4 ++-- 15 files changed, 39 insertions(+), 48 deletions(-) diff --git a/src/pyhpp/constraints/generic-transformation.cc b/src/pyhpp/constraints/generic-transformation.cc index df647b3c..4c91b060 100644 --- a/src/pyhpp/constraints/generic-transformation.cc +++ b/src/pyhpp/constraints/generic-transformation.cc @@ -54,9 +54,7 @@ void exposeAbsoluteGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("create", &AbsoluteGenericTransformation_create, - args("name", "device", "joint2", "frame2", "frame1", "mask")) - .staticmethod("create"); + .def("__init__", make_constructor(&AbsoluteGenericTransformation_create)); } template @@ -75,10 +73,7 @@ template void exposeRelativeGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("create", &RelativeGenericTransformation_create, - args("name", "device", "joint1", "joint2", "frame1", "frame2", - "mask")) - .staticmethod("create"); + .def("__init__", make_constructor(&RelativeGenericTransformation_create)); } void exposeGenericTransformations() { diff --git a/src/pyhpp/constraints/implicit.cc b/src/pyhpp/constraints/implicit.cc index b83e4f83..fca3a0dc 100644 --- a/src/pyhpp/constraints/implicit.cc +++ b/src/pyhpp/constraints/implicit.cc @@ -54,8 +54,7 @@ void exposeImplicit() { .value("Superior", Superior) .value("Inferior", Inferior); class_("Implicit", no_init) - .def("create", &Implicit::create) - .staticmethod("create") + .def("__init__", make_constructor(&Implicit::create)) .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(Implicit, comparisonType, const ComparisonTypes_t&) .PYHPP_DEFINE_METHOD_INTERNAL_REF(Implicit, function) diff --git a/src/pyhpp/constraints/locked-joint.cc b/src/pyhpp/constraints/locked-joint.cc index 796dd6bb..450ae864 100644 --- a/src/pyhpp/constraints/locked-joint.cc +++ b/src/pyhpp/constraints/locked-joint.cc @@ -64,10 +64,8 @@ LockedJointPtr_t createLockedJointWithComp(const DevicePtr_t& robot, void exposeLockedJoint() { class_, LockedJointPtr_t, boost::noncopyable>( "LockedJoint", no_init) - .def("create", &createLockedJoint) - .staticmethod("create") - .def("createWithComp", &createLockedJointWithComp) - .staticmethod("create"); + .def("__init__", make_constructor(&createLockedJoint)) + .def("__init__", make_constructor(&createLockedJointWithComp)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index 6847d779..6bef84a5 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -769,7 +769,7 @@ def buildPlacement(self, o): ljs.append(n) q = self.graph.robot.getJointConfig(n) self.registerConstraint( - LockedJoint.create( + LockedJoint( self.graph.robot.asPinDevice(), n, np.array(q) ), n, diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 7ca4bce0..8d36db58 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -250,8 +250,7 @@ void exposeDevice() { void (Device::*cfk)(int) = &Device::computeForwardKinematics; class_("Device", no_init) .def("name", &Device::name, return_value_policy()) - .def("create", &Device::create) - .staticmethod("create") + .def("__init__", make_constructor(&Device::create)) .def("configSpace", &Device::configSpace, return_value_policy()) .def("model", static_cast(&Device::model), diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index a1409fa3..574ad2d8 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -28,7 +28,7 @@ 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.create("pyrene") +robot = Device("pyrene") # Load Talos robot talos_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) @@ -119,14 +119,14 @@ for j in gripperJoints: ljName = "locked_" + j value = q0[robot.rankInConfiguration[j]] - cs = LockedJoint.create(robot, j, np.array([value])) + 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.create(robot, j, np.array([value])) + cs = LockedJoint(robot, j, np.array([value])) constraints[ljName] = cs problem.addNumericalConstraintsToConfigProjector( diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/benchmarks/ur3-spheres-spf.py index 3fde025e..314d50e5 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/benchmarks/ur3-spheres-spf.py @@ -130,7 +130,7 @@ q = Quaternion(1, 0, 0, 0) ballPlacement = SE3(q, np.array([0, 0, 0.02])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( placementName, robot.asPinDevice(), joint, @@ -145,11 +145,11 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName] = implicitPlacementConstraint # placement complement constraint - pc = Transformation.create( + pc = Transformation( placementName + "/complement", robot.asPinDevice(), joint, @@ -163,7 +163,7 @@ ComparisonType.Equality, ) implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName + "/complement"] = implicitPlacementComplementConstraint # combination of placement and complement @@ -194,7 +194,7 @@ q = Quaternion(1, 0, 0, 0) ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( preplacementName, robot.asPinDevice(), joint, @@ -208,7 +208,7 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[preplacementName] = implicitPrePlacementConstraint q_init = [ diff --git a/tests/benchmarks/ur3-spheres.py b/tests/benchmarks/ur3-spheres.py index 1a48dd83..874ff28c 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/benchmarks/ur3-spheres.py @@ -107,7 +107,7 @@ q = Quaternion(1, 0, 0, 0) ballPlacement = SE3(q, np.array([0, 0, 0.02])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( placementName, robot.asPinDevice(), joint, @@ -122,10 +122,10 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName] = implicitPlacementConstraint # placement complement constraint - pc = Transformation.create( + pc = Transformation( placementName + "/complement", robot.asPinDevice(), joint, @@ -139,7 +139,7 @@ ComparisonType.Equality, ) implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName + "/complement"] = implicitPlacementComplementConstraint # combination of placement and complement @@ -169,7 +169,7 @@ q = Quaternion(1, 0, 0, 0) ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( preplacementName, robot.asPinDevice(), joint, @@ -183,7 +183,7 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[preplacementName] = implicitPrePlacementConstraint q_init = [ diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py index 09494c24..330919c9 100644 --- a/tests/constraint_graph.py +++ b/tests/constraint_graph.py @@ -116,7 +116,7 @@ ] q = Quaternion(0, 0, 0, 1) ballGround = SE3(q, np.array([0, 0, 0.15])) -pc = Transformation.create( +pc = Transformation( "placementConstraint", robot.asPinDevice(), joint2, Id, ballGround, m ) cts = ComparisonTypes() @@ -126,14 +126,14 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] -implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) +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.create( +pc = RelativeTransformation( "grasp", robot.asPinDevice(), joint1, joint2, ballInGripper, Id, m ) cts = ComparisonTypes() @@ -145,7 +145,7 @@ ComparisonType.EqualToZero, ComparisonType.EqualToZero, ) -implicitGraspConstraint = Implicit.create(pc, cts, m) +implicitGraspConstraint = Implicit(pc, cts, m) # Set constraints of nodes and edges diff --git a/tests/graph_factory.py b/tests/graph_factory.py index 0a85675d..c2f7623f 100644 --- a/tests/graph_factory.py +++ b/tests/graph_factory.py @@ -68,10 +68,10 @@ q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746] # Create constraints -ll = LockedJoint.create( +ll = LockedJoint( robot.asPinDevice(), "pr2/l_gripper_l_finger_joint", np.array([0.5]) ) -lr = LockedJoint.create( +lr = LockedJoint( robot.asPinDevice(), "pr2/l_gripper_r_finger_joint", np.array([0.5]) ) diff --git a/tests/graph_factory2.py b/tests/graph_factory2.py index e90f9fd9..ca021b39 100644 --- a/tests/graph_factory2.py +++ b/tests/graph_factory2.py @@ -89,7 +89,7 @@ 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.create( + pc = Transformation( placementName, robot.asPinDevice(), joint, @@ -104,10 +104,10 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName] = implicitPlacementConstraint # placement complement constraint - pc = Transformation.create( + pc = Transformation( placementName + "/complement", robot.asPinDevice(), joint, @@ -121,7 +121,7 @@ ComparisonType.Equality, ) implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName + "/complement"] = implicitPlacementComplementConstraint # combination of placement and complement @@ -151,7 +151,7 @@ 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.create( + pc = Transformation( preplacementName, robot.asPinDevice(), joint, @@ -165,7 +165,7 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[preplacementName] = implicitPrePlacementConstraint q_init = [ diff --git a/tests/load_ur3.py b/tests/load_ur3.py index b27059b2..420a6faa 100644 --- a/tests/load_ur3.py +++ b/tests/load_ur3.py @@ -11,7 +11,7 @@ "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" ) -robot = Device.create("ur3") +robot = Device("ur3") urdf.loadModel(robot, 0, "", "anchor", urdfFilename, srdfFilename, SE3.Identity()) q = robot.currentConfiguration() diff --git a/tests/path-planner.py b/tests/path-planner.py index d3aced99..287f18a0 100644 --- a/tests/path-planner.py +++ b/tests/path-planner.py @@ -15,7 +15,7 @@ rootJointType = "planar" # Initialize robot and viewer -robot = Device.create("ur2") +robot = Device("ur2") urdf.loadModel( robot, 0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity() ) diff --git a/tests/rrt.py b/tests/rrt.py index c34da1e5..081b7665 100644 --- a/tests/rrt.py +++ b/tests/rrt.py @@ -8,7 +8,7 @@ srdfFilename = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" # Initialize robot and viewer -robot = Device.create("ur5") +robot = Device("ur5") # Add robot and obstacles to scene urdf.loadModel(robot, 0, "r0", "anchor", urdfFilename, srdfFilename, SE3.Identity()) diff --git a/tests/solver.py b/tests/solver.py index a7d7baed..a280fb04 100644 --- a/tests/solver.py +++ b/tests/solver.py @@ -11,7 +11,7 @@ ) from pinocchio import SE3, StdVec_Bool as Mask -robot = Device.create("ur3") +robot = Device("ur3") urdf.loadRobotModel( robot, "anchor", @@ -35,7 +35,7 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) -solver.add(Implicit.create(pc, cts, [True, True, True]), 0) +solver.add(Implicit(pc, cts, [True, True, True]), 0) print(solver) From e50f1fb66a8891679c0a9b21596d111eb7bd84fb Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 13 Nov 2025 08:25:15 +0100 Subject: [PATCH 032/109] Add pyhpp::pinocchio::Device class so manipulation device can inherit from it. Add converters so manipulation device can be interpreted as a pinocchio device in python scripts --- .../manipulation/constraint_graph_factory.py | 2 +- src/pyhpp/manipulation/device.cc | 107 +++------------- src/pyhpp/manipulation/device.hh | 24 +--- src/pyhpp/manipulation/graph.cc | 36 +++--- src/pyhpp/manipulation/problem.cc | 2 +- src/pyhpp/manipulation/urdf/util.cc | 4 +- src/pyhpp/pinocchio/device.cc | 117 ++++++++++++------ tests/benchmarks/ur3-spheres-spf.py | 12 +- tests/benchmarks/ur3-spheres.py | 12 +- tests/constraint_graph.py | 4 +- tests/graph_factory.py | 5 +- tests/graph_factory2.py | 10 +- tests/load_ur3.py | 2 +- 13 files changed, 144 insertions(+), 193 deletions(-) diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index 6bef84a5..697218d9 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -770,7 +770,7 @@ def buildPlacement(self, o): q = self.graph.robot.getJointConfig(n) self.registerConstraint( LockedJoint( - self.graph.robot.asPinDevice(), n, np.array(q) + self.graph.robot, n, np.array(q) ), n, ) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9c67a3c2..790f91e0 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -36,49 +36,22 @@ namespace pyhpp { namespace manipulation { using namespace boost::python; -Device::Device(const hpp::manipulation::DevicePtr_t& object) { obj = object; } Device::Device(const std::string& name) - : obj(hpp::manipulation::Device::create(name)) {} -// Methods from hpp::pinocchio::Device -const std::string& Device::name() const { return obj->name(); } -const LiegroupSpacePtr_t& Device::configSpace() const { - return obj->configSpace(); -} -Model& Device::model() { return obj->model(); } -Data& Device::data() { return obj->data(); } -GeomData& Device::geomData() { return obj->geomData(); } -GeomModel& Device::geomModel() { return obj->geomModel(); } -GeomModel& Device::visualModel() { return obj->visualModel(); } + : pyhpp::pinocchio::Device(hpp::manipulation::Device::create(name)) {} -size_type Device::configSize() const { return obj->configSize(); } -size_type Device::numberDof() const { return obj->numberDof(); } -const Configuration_t& Device::currentConfiguration() const { - return obj->currentConfiguration(); -} -bool Device::currentConfiguration(ConfigurationIn_t configuration) { - return obj->currentConfiguration(configuration); -} -void Device::computeForwardKinematics(int flag) { - return obj->computeForwardKinematics(flag); -} -void Device::computeFramesForwardKinematics() { - return obj->computeFramesForwardKinematics(); -} -void Device::updateGeometryPlacements() { - return obj->updateGeometryPlacements(); -} -// Methods for hpp::manipulation::Device void Device::setRobotRootPosition(const std::string& robotName, const Transform3s& positionWRTParentJoint) { - return obj->setRobotRootPosition(robotName, positionWRTParentJoint); + std::dynamic_pointer_cast(obj) + ->setRobotRootPosition(robotName, positionWRTParentJoint); } -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload, Model::getFrameId, - 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload, Model::existFrame, - 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload, - Model::addJointFrame, 1, 2) +std::map Device::handles() { + return std::dynamic_pointer_cast(obj)->handles.map; +} + +std::map Device::grippers() { + return std::dynamic_pointer_cast(obj)->grippers.map; +} PinDevicePtr_t Device::asPinDevice() { hpp::pinocchio::DevicePtr_t pinDevice = @@ -140,30 +113,6 @@ boost::python::list Device::getJointNames() { } } -bool Device_currentConfiguration(Device& d, const Configuration_t& c) { - return d.currentConfiguration(c); -} - -Transform3s getObjectPositionInJoint(const GripperPtr_t& gripper) { - Transform3s res(gripper->objectPositionInJoint()); - return res; -} - -bool check(const Device& device) { - for (auto const& g : device.obj->grippers.map) { - std::cout << g.first << std::endl; - } - return true; -} - -std::map getDeviceHandles(const Device& device) { - return device.obj->handles.map; -} - -std::map getDeviceGrippers(const Device& device) { - return device.obj->grippers.map; -} - std::string getHandleName(const HandlePtr_t& handle) { return handle->name(); } void setHandleName(const HandlePtr_t& handle, const std::string& name) { @@ -217,38 +166,12 @@ void exposeHandle() { true>()); } void exposeDevice() { - void (Device::*cfk)(int) = &Device::computeForwardKinematics; - // Enable automatic conversion of std::map - class_("Device", init()) - .def("name", &Device::name, return_value_policy()) - .def("configSpace", &Device::configSpace, - return_value_policy()) - .def("model", static_cast(&Device::model), - return_internal_reference<>()) - .def("data", static_cast(&Device::data), - return_internal_reference<>()) - .def("geomData", static_cast(&Device::geomData), - return_internal_reference<>()) - .def("geomModel", - static_cast(&Device::geomModel), - return_internal_reference<>()) - .def("visualModel", - static_cast(&Device::visualModel), - return_internal_reference<>()) - .PYHPP_DEFINE_METHOD(Device, configSize) - .PYHPP_DEFINE_METHOD(Device, numberDof) - .def("currentConfiguration", - static_cast( - &Device::currentConfiguration), - return_value_policy()) - .def("currentConfiguration", Device_currentConfiguration) - .def("computeForwardKinematics", cfk) - .def("computeFramesForwardKinematics", - &Device::computeFramesForwardKinematics) - .def("updateGeometryPlacements", &Device::updateGeometryPlacements) + class_, + boost::shared_ptr, boost::noncopyable> + ("Device", init()) .def("setRobotRootPosition", &Device::setRobotRootPosition) - .def("handles", &getDeviceHandles) - .def("grippers", &getDeviceGrippers) + .def("handles", &Device::handles) + .def("grippers", &Device::grippers) .def("asPinDevice", &Device::asPinDevice) .def("getJointNames", &Device::getJointNames) .def("getJointConfig", &Device::getJointConfig) diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index ae89fbd3..66bc7c26 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -40,6 +40,7 @@ #include #include #include +#include <../src/pyhpp/pinocchio/device.hh> #include namespace pyhpp { @@ -72,28 +73,13 @@ typedef hpp::pinocchio::JointPtr_t JointPtr_t; // Boost python does not correctly handle weak pointers. To overcome this // limitation, we create a wrapper class and bind this class with boost python // instead of the original class. -struct Device { - DevicePtr_t obj; - Device(const DevicePtr_t& object); +struct Device : public pyhpp::pinocchio::Device { Device(const std::string& name); - // Methods from hpp::pinocchio::Device - const std::string& name() const; - const LiegroupSpacePtr_t& configSpace() const; - Model& model(); - Data& data(); - GeomData& geomData(); - GeomModel& geomModel(); - GeomModel& visualModel(); - size_type configSize() const; - size_type numberDof() const; - const Configuration_t& currentConfiguration() const; - bool currentConfiguration(ConfigurationIn_t configuration); - void computeForwardKinematics(int flag); - void computeFramesForwardKinematics(); - void updateGeometryPlacements(); - // Methods for hpp::manipulation::Device + void setRobotRootPosition(const std::string& robotName, const Transform3s& positionWRTParentJoint); + std::map handles(); + std::map grippers(); PinDevicePtr_t asPinDevice(); boost::python::list getJointConfig(const char* jointName); boost::python::list getJointNames(); diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 1565fddb..dcedaf31 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -266,7 +266,7 @@ PyWGraph::PyWGraph(const hpp::manipulation::graph::GraphPtr_t& object) PyWGraph::PyWGraph(const std::string& name, const PyWDevicePtr_t& d, const PyWProblemPtr_t& problem) : obj(hpp::manipulation::graph::Graph::create( - name, d->obj, problem->asManipulationProblem())), + name, d->asManipulationDevice(), problem->asManipulationProblem())), robot(d), problem(problem) {} @@ -1017,14 +1017,14 @@ boost::python::tuple PyWGraph::createPlacementConstraint( auto surface2 = extract_vector(py_surface2); bool explicit_(true); - if (!robot->obj) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); it1 != surface1.end(); ++it1) { - if (!robot->obj->jointAndShapes.has(*it1)) + if (!robot->asManipulationDevice()->jointAndShapes.has(*it1)) throw std::runtime_error("First list of triangles not found."); - l = robot->obj->jointAndShapes.get(*it1); + l = robot->asManipulationDevice()->jointAndShapes.get(*it1); objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end()); for (auto js : l) { @@ -1041,8 +1041,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( for (std::vector::const_iterator it2 = surface2.begin(); it2 != surface2.end(); ++it2) { - if (robot->obj->jointAndShapes.has(*it2)) { - l = robot->obj->jointAndShapes.get(*it2); + if (robot->asManipulationDevice()->jointAndShapes.has(*it2)) { + l = robot->asManipulationDevice()->jointAndShapes.get(*it2); } else if (problem->jointAndShapes.has(*it2)) { l = problem->jointAndShapes.get(*it2); } else { @@ -1055,7 +1055,7 @@ boost::python::tuple PyWGraph::createPlacementConstraint( typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t; Constraint_t::Constraints_t constraints( Constraint_t::createConstraintAndComplement( - name, robot->obj, floorSurfaces, objectSurfaces, margin)); + name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces, margin)); assert(hpp::dynamic_pointer_cast( std::get<0>(constraints)->functionPtr())); @@ -1081,7 +1081,7 @@ boost::python::tuple PyWGraph::createPlacementConstraint( std::pair functions(hpp::constraints::ConvexShapeContactComplement::createPair( - name, robot->obj, floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces)); functions.first->setNormalMargin(margin); @@ -1118,21 +1118,21 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( auto surface1 = extract_vector(py_surface1); auto surface2 = extract_vector(py_surface2); - if (!robot->obj) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); it1 != surface1.end(); ++it1) { - if (!robot->obj->jointAndShapes.has(*it1)) + if (!robot->asManipulationDevice()->jointAndShapes.has(*it1)) throw std::runtime_error("First list of triangles not found."); - l = robot->obj->jointAndShapes.get(*it1); + l = robot->asManipulationDevice()->jointAndShapes.get(*it1); objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end()); } for (std::vector::const_iterator it2 = surface2.begin(); it2 != surface2.end(); ++it2) { - if (robot->obj->jointAndShapes.has(*it2)) { - l = robot->obj->jointAndShapes.get(*it2); + if (robot->asManipulationDevice()->jointAndShapes.has(*it2)) { + l = robot->asManipulationDevice()->jointAndShapes.get(*it2); } else if (problem->jointAndShapes.has(*it2)) { l = problem->jointAndShapes.get(*it2); } else { @@ -1143,7 +1143,7 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( hpp::constraints::ConvexShapeContactPtr_t cvxShape( hpp::constraints::ConvexShapeContact::create( - name, robot->obj, floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces)); cvxShape->setNormalMargin(margin + width); return hpp::constraints::Implicit::create( @@ -1171,9 +1171,9 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, const std::string& handle) { boost::python::list result; - GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); const std::string cname = name + "/complement"; const std::string bname = name + "/hold"; @@ -1194,9 +1194,9 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, const std::string& gripper, const std::string& handle) { - GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); value_type c = h->clearance() + g->clearance(); diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index c2dec1cd..ca156be8 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -45,7 +45,7 @@ namespace pyhpp { namespace manipulation { Problem::Problem(const PyWDevicePtr_t& robot) - : pyhpp::core::Problem(hpp::manipulation::Problem::create(robot->obj)) {} + : pyhpp::core::Problem(hpp::manipulation::Problem::create(robot->asManipulationDevice())) {} Problem::Problem(const hpp::manipulation::ProblemPtr_t& object) : pyhpp::core::Problem(object) {} diff --git a/src/pyhpp/manipulation/urdf/util.cc b/src/pyhpp/manipulation/urdf/util.cc index e6d42219..67dbb95c 100644 --- a/src/pyhpp/manipulation/urdf/util.cc +++ b/src/pyhpp/manipulation/urdf/util.cc @@ -51,7 +51,7 @@ void loadModel(const Device& robot, const FrameIndex& baseFrame, hpp::pinocchio::urdf::loadModel(robot.obj, baseFrame, prefix, rootType, urdfPath, srdfPath, bMr); if (!std::string(srdfPath).empty()) { - hpp::manipulation::srdf::loadModelFromFile(robot.obj, prefix, srdfPath); + hpp::manipulation::srdf::loadModelFromFile(robot.asManipulationDevice(), prefix, srdfPath); } } @@ -61,7 +61,7 @@ void loadModelFromString(const Device& robot, const FrameIndex& baseFrame, const std::string& srdfString, const SE3& bMr) { hpp::pinocchio::urdf::loadModelFromString( robot.obj, baseFrame, prefix, rootType, urdfString, srdfString, bMr); - hpp::manipulation::srdf::loadModelFromXML(robot.obj, prefix, srdfString); + hpp::manipulation::srdf::loadModelFromXML(robot.asManipulationDevice(), prefix, srdfString); } void exposeUtil() { diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 8d36db58..41adda33 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -39,6 +39,7 @@ #include #include #include +#include <../src/pyhpp/pinocchio/device.hh> #include // #include @@ -56,22 +57,12 @@ namespace pyhpp { namespace pinocchio { namespace bp = boost::python; -typedef hpp::pinocchio::Model Model; -typedef hpp::pinocchio::Data Data; -typedef hpp::pinocchio::ModelPtr_t ModelPtr_t; -typedef hpp::pinocchio::GeomData GeomData; -typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; -typedef hpp::pinocchio::GeomModel GeomModel; -typedef hpp::pinocchio::Configuration_t Configuration_t; -typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; -typedef hpp::pinocchio::size_type size_type; -typedef hpp::pinocchio::Transform3s Transform3s; -typedef hpp::pinocchio::Device Device; -typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; typedef hpp::pinocchio::GripperPtr_t GripperPtr_t; typedef hpp::pinocchio::Gripper Gripper; -typedef hpp::pinocchio::Computation_t Computation_t; -typedef hpp::pinocchio::value_type value_type; +typedef hpp::pinocchio::FrameIndex FrameIndex; +typedef hpp::pinocchio::SE3 SE3; +typedef hpp::pinocchio::Frame Frame; +typedef hpp::pinocchio::JointPtr_t JointPtr_t; using namespace boost::python; @@ -93,19 +84,16 @@ Transform3s getObjectPositionInJoint(const GripperPtr_t& gripper) { static hpp::pinocchio::vector3_t getCenterOfMass(Device& d) { try { - d.computeForwardKinematics(); - return d.positionCenterOfMass(); + d.computeForwardKinematics(hpp::pinocchio::COM); + return d.obj->positionCenterOfMass(); } catch (const std::exception& exc) { throw std::logic_error(exc.what()); } } -typedef hpp::pinocchio::Frame Frame; -typedef hpp::pinocchio::JointPtr_t JointPtr_t; - static void setJointBounds(Device& device, const char* jointName, boost::python::list py_jointBounds) { - Frame frame = device.getFrameByName(jointName); + Frame frame = device.obj->getFrameByName(jointName); JointPtr_t joint = frame.joint(); auto jointBounds = extract_vector(py_jointBounds); @@ -131,8 +119,6 @@ static void setJointBounds(Device& device, const char* jointName, } } -typedef hpp::pinocchio::FrameIndex FrameIndex; -typedef hpp::pinocchio::SE3 SE3; static boost::python::list getJointPosition(Device& device, const std::string& jointName) { try { @@ -171,7 +157,7 @@ static boost::python::dict rankInConfiguration(Device& device) { try { auto joint_names = device.model().names; for (const auto& joint_name : joint_names) { - Frame frame = device.getFrameByName(joint_name.c_str()); + Frame frame = device.obj->getFrameByName(joint_name.c_str()); if (!frame.isFixed()) { JointPtr_t joint = frame.joint(); if (joint) { @@ -190,7 +176,7 @@ static boost::python::list getJointsPosition( const boost::python::list& jointNames) { try { device.currentConfiguration(dofArray); - device.computeForwardKinematics(); + device.computeForwardKinematics(hpp::pinocchio::JOINT_POSITION); device.computeFramesForwardKinematics(); const Model& model(device.model()); @@ -239,6 +225,65 @@ void exposeGripper() { boost::python::map_indexing_suite, true>()); } + +static boost::shared_ptr createDevice(const std::string& name) { + return boost::make_shared( + hpp::pinocchio::Device::create(name)); +} + + +struct DeviceWrapperConverter { + static void* convertible(PyObject* obj_ptr) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + return extractor.check() ? obj_ptr : nullptr; + } + + static void construct_shared_ptr( + PyObject* obj_ptr, + boost::python::converter::rvalue_from_python_stage1_data* data) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + + const Device& wrapper = extractor(); + typedef std::shared_ptr PinDevicePtr_t; + typedef boost::python::converter::rvalue_from_python_storage StorageType; + + void* storage = ((StorageType*)data)->storage.bytes; + new (storage) PinDevicePtr_t(wrapper.obj); + data->convertible = storage; + } + + static void construct_const_shared_ptr( + PyObject* obj_ptr, + boost::python::converter::rvalue_from_python_stage1_data* data) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + + const Device& wrapper = extractor(); + typedef std::shared_ptr ConstPinDevicePtr_t; + typedef boost::python::converter::rvalue_from_python_storage StorageType; + + void* storage = ((StorageType*)data)->storage.bytes; + new (storage) ConstPinDevicePtr_t(wrapper.obj); + data->convertible = storage; + } +}; + +void register_device_converters() { + typedef std::shared_ptr PinDevicePtr_t; + typedef std::shared_ptr ConstPinDevicePtr_t; + + boost::python::converter::registry::push_back( + &DeviceWrapperConverter::convertible, + &DeviceWrapperConverter::construct_shared_ptr, + boost::python::type_id()); + + boost::python::converter::registry::push_back( + &DeviceWrapperConverter::convertible, + &DeviceWrapperConverter::construct_const_shared_ptr, + boost::python::type_id()); +} void exposeDevice() { enum_("ComputationFlag") .value("JOINT_POSITION", hpp::pinocchio::JOINT_POSITION) @@ -248,23 +293,18 @@ void exposeDevice() { .value("COM", hpp::pinocchio::COM) .value("COMPUTE_ALL", hpp::pinocchio::COMPUTE_ALL); void (Device::*cfk)(int) = &Device::computeForwardKinematics; - class_("Device", no_init) + +class_, boost::noncopyable> + ("Device", no_init) + .def("__init__", make_constructor(&createDevice)) .def("name", &Device::name, return_value_policy()) - .def("__init__", make_constructor(&Device::create)) .def("configSpace", &Device::configSpace, return_value_policy()) - .def("model", static_cast(&Device::model), - return_internal_reference<>()) - .def("data", static_cast(&Device::data), - return_internal_reference<>()) - .def("geomData", static_cast(&Device::geomData), - return_internal_reference<>()) - .def("geomModel", - static_cast(&Device::geomModel), - return_internal_reference<>()) - .def("visualModel", - static_cast(&Device::visualModel), - return_internal_reference<>()) + .def("model", &Device::model, return_internal_reference<>()) + .def("data", &Device::data, return_internal_reference<>()) + .def("geomData", &Device::geomData, return_internal_reference<>()) + .def("geomModel", &Device::geomModel, return_internal_reference<>()) + .def("visualModel", &Device::visualModel, return_internal_reference<>()) .PYHPP_DEFINE_METHOD(Device, configSize) .PYHPP_DEFINE_METHOD(Device, numberDof) @@ -283,6 +323,7 @@ void exposeDevice() { .def("getCenterOfMass", &getCenterOfMass) .def("getJointPosition", &getJointPosition) .def("getJointsPosition", &getJointsPosition); + register_device_converters(); } } // namespace pinocchio } // namespace pyhpp diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/benchmarks/ur3-spheres-spf.py index 314d50e5..0586f645 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/benchmarks/ur3-spheres-spf.py @@ -132,7 +132,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( placementName, - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -151,7 +151,7 @@ # placement complement constraint pc = Transformation( placementName + "/complement", - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -175,8 +175,8 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), + ll = LockedJoint( + robot, "sphere{0}/root_joint".format(i), np.array([0, 0, 0.02, 0, 0, 0, 1]), cts, @@ -196,7 +196,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( preplacementName, - robot.asPinDevice(), + robot, joint, Id, ballPrePlacement, @@ -281,7 +281,7 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = Dichotomy(robot.asPinDevice(), 0) +problem.pathValidation = Dichotomy(robot, 0) # need to set path projector due to implicit constraints added above problem.pathProjector = ProgressiveProjector( diff --git a/tests/benchmarks/ur3-spheres.py b/tests/benchmarks/ur3-spheres.py index 874ff28c..3b2591fa 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/benchmarks/ur3-spheres.py @@ -109,7 +109,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( placementName, - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -127,7 +127,7 @@ # placement complement constraint pc = Transformation( placementName + "/complement", - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -151,8 +151,8 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), + ll = LockedJoint( + robot, "sphere{0}/root_joint".format(i), np.array([0, 0, 0.02, 0, 0, 0, 1]), cts, @@ -171,7 +171,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( preplacementName, - robot.asPinDevice(), + robot, joint, Id, ballPrePlacement, @@ -262,7 +262,7 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = Dichotomy(robot.asPinDevice(), 0) +problem.pathValidation = Dichotomy(robot, 0) problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.01 ) diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py index 330919c9..13d90716 100644 --- a/tests/constraint_graph.py +++ b/tests/constraint_graph.py @@ -117,7 +117,7 @@ q = Quaternion(0, 0, 0, 1) ballGround = SE3(q, np.array([0, 0, 0.15])) pc = Transformation( - "placementConstraint", robot.asPinDevice(), joint2, Id, ballGround, m + "placementConstraint", robot, joint2, Id, ballGround, m ) cts = ComparisonTypes() cts[:] = ( @@ -134,7 +134,7 @@ m = Mask() m[:] = (True,) * 6 pc = RelativeTransformation( - "grasp", robot.asPinDevice(), joint1, joint2, ballInGripper, Id, m + "grasp", robot, joint1, joint2, ballInGripper, Id, m ) cts = ComparisonTypes() cts[:] = ( diff --git a/tests/graph_factory.py b/tests/graph_factory.py index c2f7623f..2e17d902 100644 --- a/tests/graph_factory.py +++ b/tests/graph_factory.py @@ -39,6 +39,7 @@ # Create problem problem = Problem(robot) +print(model.names) # Generate initial and goal configuration. rankInConfiguration = dict() @@ -69,10 +70,10 @@ # Create constraints ll = LockedJoint( - robot.asPinDevice(), "pr2/l_gripper_l_finger_joint", np.array([0.5]) + robot, "pr2/l_gripper_l_finger_joint", np.array([0.5]) ) lr = LockedJoint( - robot.asPinDevice(), "pr2/l_gripper_r_finger_joint", np.array([0.5]) + robot, "pr2/l_gripper_r_finger_joint", np.array([0.5]) ) # Create the constraint graph diff --git a/tests/graph_factory2.py b/tests/graph_factory2.py index ca021b39..26f1cbe4 100644 --- a/tests/graph_factory2.py +++ b/tests/graph_factory2.py @@ -91,7 +91,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( placementName, - robot.asPinDevice(), + robot, joint, ballPlacement, Id, @@ -109,7 +109,7 @@ # placement complement constraint pc = Transformation( placementName + "/complement", - robot.asPinDevice(), + robot, joint, ballPlacement, Id, @@ -133,8 +133,8 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), + ll = LockedJoint( + robot, "sphere{0}/root_joint".format(i), np.array([0, 0, 0.02, 0, 0, 0, 1]), cts, @@ -153,7 +153,7 @@ joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) pc = Transformation( preplacementName, - robot.asPinDevice(), + robot, joint, ballPrePlacement, Id, diff --git a/tests/load_ur3.py b/tests/load_ur3.py index 420a6faa..e7e37e60 100644 --- a/tests/load_ur3.py +++ b/tests/load_ur3.py @@ -21,7 +21,7 @@ m = Mask() m[:] = (True,) * 3 Id = SE3.Identity() -pc = Position.create( +pc = Position( "position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m ) print(pc) From b047616febda59e6b46aed2c29cebac9394cee11 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 13 Nov 2025 08:02:54 +0100 Subject: [PATCH 033/109] Revert "Add pyhpp coverage" This reverts commit cb58fb47be1b0ebe3af4d9b9aa072834f5068384. --- CMakeLists.txt | 6 --- include/pyhpp/coverage/fwd.hh | 42 ------------------ src/CMakeLists.txt | 9 ---- src/pyhpp/coverage/bindings.cc | 38 ----------------- src/pyhpp/coverage/path.cc | 78 ---------------------------------- 5 files changed, 173 deletions(-) delete mode 100644 include/pyhpp/coverage/fwd.hh delete mode 100644 src/pyhpp/coverage/bindings.cc delete mode 100644 src/pyhpp/coverage/path.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index e9c891b2..37b2abe1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,7 +85,6 @@ set(${PROJECT_NAME}_HEADERS include/pyhpp/manipulation/urdf/fwd.hh include/pyhpp/pinocchio/fwd.hh include/pyhpp/pinocchio/urdf/fwd.hh - include/pyhpp/coverage/fwd.hh include/pyhpp/ref.hh include/pyhpp/stl-pair.hh include/pyhpp/util.hh @@ -105,11 +104,6 @@ add_project_dependency(hpp-corbaserver REQUIRED) add_project_dependency(hpp-manipulation REQUIRED) add_project_dependency(hpp-manipulation-urdf REQUIRED) -option(BUILD_HPP_COVERAGE "Build with hpp-coverage support" ON) -if(BUILD_HPP_COVERAGE) - add_project_dependency(hpp-coverage REQUIRED) -endif() - if(BUILD_TESTING) find_package(example-robot-data REQUIRED) find_package(hpp-environments REQUIRED) diff --git a/include/pyhpp/coverage/fwd.hh b/include/pyhpp/coverage/fwd.hh deleted file mode 100644 index 0a454875..00000000 --- a/include/pyhpp/coverage/fwd.hh +++ /dev/null @@ -1,42 +0,0 @@ -// -// Copyright (c) 2025 CNRS -// Authors: Paul Sardin -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#ifndef PYHPP_COVERAGE_FWD_HH -#define PYHPP_COVERAGE_FWD_HH - -#include - -namespace pyhpp { -namespace coverage { -void exposeCoverage(); - -} // namespace coverage -} // namespace pyhpp - -#endif // PYHPP_COVERAGE_FWD_HH diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ac89f0cd..ebf76193 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -166,12 +166,3 @@ add_python_library( pyhpp/manipulation/urdf FILES pyhpp/manipulation/urdf/util.cc pyhpp/manipulation/urdf/bindings.cc LINK_LIBRARIES hpp-manipulation-urdf::hpp-manipulation-urdf) - -if(BUILD_HPP_COVERAGE) - add_python_library( - pyhpp/coverage - FILES - pyhpp/coverage/path.cc - LINK_LIBRARIES - hpp-coverage::hpp-coverage) -endif() diff --git a/src/pyhpp/coverage/bindings.cc b/src/pyhpp/coverage/bindings.cc deleted file mode 100644 index 9566495b..00000000 --- a/src/pyhpp/coverage/bindings.cc +++ /dev/null @@ -1,38 +0,0 @@ -// -// Copyright (c) 2025, CNRS -// Authors: Paul Sardin -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -BOOST_PYTHON_MODULE(bindings) { - INIT_PYHPP_MODULE; - - pyhpp::coverage::exposeCoverage(); - } diff --git a/src/pyhpp/coverage/path.cc b/src/pyhpp/coverage/path.cc deleted file mode 100644 index 6ca8b742..00000000 --- a/src/pyhpp/coverage/path.cc +++ /dev/null @@ -1,78 +0,0 @@ -// -// Copyright (c) 2025 CNRS -// Author: Paul Sardin -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// 1. Redistributions of source code must retain the above copyright notice, -// this list of conditions and the following disclaimer. -// -// 2. Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace boost::python; - -namespace pyhpp { -namespace coverage { -using hpp::coverage::Path; -using hpp::core::PathPtr_t; -using hpp::core::PathVector; -using hpp::core::PathVectorPtr_t; - -static PathPtr_t multiply(const PathPtr_t& p1, const PathPtr_t& p2) { - Path ops; - return ops.multiply(p1, p2); -} - -static PathPtr_t createSpline(hpp::core::ConfigurationIn_t pose0, - hpp::core::ConfigurationIn_t pose1, - hpp::core::value_type length, - hpp::core::size_type order) { - Path ops; - return ops.createSpline(pose0, pose1, length, order); -} - -static void setCost(hpp::core::Roadmap& roadmap, - const hpp::pinocchio::DevicePtr_t& robot, - const std::string& gripperName) { - roadmap.cost(hpp::coverage::ToolRotation::create(robot, gripperName)); -} - -BOOST_PYTHON_MODULE(bindings) { - boost::python::import("pyhpp.core"); - - class_, boost::noncopyable>("CoveragePath", no_init) - .def("create", +[]() { return ::hpp::shared_ptr(new Path()); }) - .staticmethod("create") - .def("multiply", &Path::multiply) - .def("createSpline", &Path::createSpline) - .def("setCost", &setCost) - .staticmethod("setCost"); -} - -} // namespace coverage -} // namespace pyhpp \ No newline at end of file From 8953aa8f71f5ce8223564ab084a0dd49430f9d54 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Nov 2025 07:26:23 +0000 Subject: [PATCH 034/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../constraints/generic-transformation.cc | 6 +++-- .../manipulation/constraint_graph_factory.py | 4 +-- src/pyhpp/manipulation/device.cc | 8 +++--- src/pyhpp/manipulation/device.hh | 4 +-- src/pyhpp/manipulation/graph.cc | 27 ++++++++++++------- src/pyhpp/manipulation/problem.cc | 3 ++- src/pyhpp/manipulation/urdf/util.cc | 6 +++-- src/pyhpp/pinocchio/device.cc | 27 ++++++++++--------- tests/constraint_graph.py | 8 ++---- tests/graph_factory.py | 8 ++---- tests/load_ur3.py | 4 +-- 11 files changed, 54 insertions(+), 51 deletions(-) diff --git a/src/pyhpp/constraints/generic-transformation.cc b/src/pyhpp/constraints/generic-transformation.cc index 4c91b060..a4be2f8a 100644 --- a/src/pyhpp/constraints/generic-transformation.cc +++ b/src/pyhpp/constraints/generic-transformation.cc @@ -54,7 +54,8 @@ void exposeAbsoluteGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("__init__", make_constructor(&AbsoluteGenericTransformation_create)); + .def("__init__", + make_constructor(&AbsoluteGenericTransformation_create)); } template @@ -73,7 +74,8 @@ template void exposeRelativeGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("__init__", make_constructor(&RelativeGenericTransformation_create)); + .def("__init__", + make_constructor(&RelativeGenericTransformation_create)); } void exposeGenericTransformations() { diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index 697218d9..c14d643f 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -769,9 +769,7 @@ def buildPlacement(self, o): ljs.append(n) q = self.graph.robot.getJointConfig(n) self.registerConstraint( - LockedJoint( - self.graph.robot, n, np.array(q) - ), + LockedJoint(self.graph.robot, n, np.array(q)), n, ) return dict( diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 790f91e0..f414bf93 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -50,7 +50,8 @@ std::map Device::handles() { } std::map Device::grippers() { - return std::dynamic_pointer_cast(obj)->grippers.map; + return std::dynamic_pointer_cast(obj) + ->grippers.map; } PinDevicePtr_t Device::asPinDevice() { @@ -166,9 +167,8 @@ void exposeHandle() { true>()); } void exposeDevice() { - class_, - boost::shared_ptr, boost::noncopyable> - ("Device", init()) + class_, boost::shared_ptr, + boost::noncopyable>("Device", init()) .def("setRobotRootPosition", &Device::setRobotRootPosition) .def("handles", &Device::handles) .def("grippers", &Device::grippers) diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 66bc7c26..3b60042d 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -30,6 +30,7 @@ #ifndef PYHPP_MANIPULATION_DEVICE_HH #define PYHPP_MANIPULATION_DEVICE_HH +#include <../src/pyhpp/pinocchio/device.hh> #include #include #include @@ -40,7 +41,6 @@ #include #include #include -#include <../src/pyhpp/pinocchio/device.hh> #include namespace pyhpp { @@ -75,7 +75,7 @@ typedef hpp::pinocchio::JointPtr_t JointPtr_t; // instead of the original class. struct Device : public pyhpp::pinocchio::Device { Device(const std::string& name); - + void setRobotRootPosition(const std::string& robotName, const Transform3s& positionWRTParentJoint); std::map handles(); diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index dcedaf31..8d00c431 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -1017,7 +1017,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( auto surface2 = extract_vector(py_surface2); bool explicit_(true); - if (!robot->asManipulationDevice()) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) + throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); @@ -1055,7 +1056,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t; Constraint_t::Constraints_t constraints( Constraint_t::createConstraintAndComplement( - name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces, margin)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces, margin)); assert(hpp::dynamic_pointer_cast( std::get<0>(constraints)->functionPtr())); @@ -1081,7 +1083,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( std::pair functions(hpp::constraints::ConvexShapeContactComplement::createPair( - name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces)); functions.first->setNormalMargin(margin); @@ -1118,7 +1121,8 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( auto surface1 = extract_vector(py_surface1); auto surface2 = extract_vector(py_surface2); - if (!robot->asManipulationDevice()) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) + throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); @@ -1143,7 +1147,8 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( hpp::constraints::ConvexShapeContactPtr_t cvxShape( hpp::constraints::ConvexShapeContact::create( - name, robot->asManipulationDevice(), floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces)); cvxShape->setNormalMargin(margin + width); return hpp::constraints::Implicit::create( @@ -1171,9 +1176,11 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, const std::string& handle) { boost::python::list result; - GripperPtr_t g = robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = + robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = + robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); const std::string cname = name + "/complement"; const std::string bname = name + "/hold"; @@ -1194,9 +1201,11 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, const std::string& gripper, const std::string& handle) { - GripperPtr_t g = robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = + robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = + robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); value_type c = h->clearance() + g->clearance(); diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index ca156be8..44111b6e 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -45,7 +45,8 @@ namespace pyhpp { namespace manipulation { Problem::Problem(const PyWDevicePtr_t& robot) - : pyhpp::core::Problem(hpp::manipulation::Problem::create(robot->asManipulationDevice())) {} + : pyhpp::core::Problem( + hpp::manipulation::Problem::create(robot->asManipulationDevice())) {} Problem::Problem(const hpp::manipulation::ProblemPtr_t& object) : pyhpp::core::Problem(object) {} diff --git a/src/pyhpp/manipulation/urdf/util.cc b/src/pyhpp/manipulation/urdf/util.cc index 67dbb95c..60c8561e 100644 --- a/src/pyhpp/manipulation/urdf/util.cc +++ b/src/pyhpp/manipulation/urdf/util.cc @@ -51,7 +51,8 @@ void loadModel(const Device& robot, const FrameIndex& baseFrame, hpp::pinocchio::urdf::loadModel(robot.obj, baseFrame, prefix, rootType, urdfPath, srdfPath, bMr); if (!std::string(srdfPath).empty()) { - hpp::manipulation::srdf::loadModelFromFile(robot.asManipulationDevice(), prefix, srdfPath); + hpp::manipulation::srdf::loadModelFromFile(robot.asManipulationDevice(), + prefix, srdfPath); } } @@ -61,7 +62,8 @@ void loadModelFromString(const Device& robot, const FrameIndex& baseFrame, const std::string& srdfString, const SE3& bMr) { hpp::pinocchio::urdf::loadModelFromString( robot.obj, baseFrame, prefix, rootType, urdfString, srdfString, bMr); - hpp::manipulation::srdf::loadModelFromXML(robot.asManipulationDevice(), prefix, srdfString); + hpp::manipulation::srdf::loadModelFromXML(robot.asManipulationDevice(), + prefix, srdfString); } void exposeUtil() { diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 41adda33..3e37f69e 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -27,6 +27,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED // OF THE POSSIBILITY OF SUCH DAMAGE. +#include <../src/pyhpp/pinocchio/device.hh> #include #include #include @@ -39,7 +40,6 @@ #include #include #include -#include <../src/pyhpp/pinocchio/device.hh> #include // #include @@ -227,11 +227,9 @@ void exposeGripper() { } static boost::shared_ptr createDevice(const std::string& name) { - return boost::make_shared( - hpp::pinocchio::Device::create(name)); + return boost::make_shared(hpp::pinocchio::Device::create(name)); } - struct DeviceWrapperConverter { static void* convertible(PyObject* obj_ptr) { boost::python::object obj(boost::python::borrowed(obj_ptr)); @@ -247,8 +245,9 @@ struct DeviceWrapperConverter { const Device& wrapper = extractor(); typedef std::shared_ptr PinDevicePtr_t; - typedef boost::python::converter::rvalue_from_python_storage StorageType; - + typedef boost::python::converter::rvalue_from_python_storage + StorageType; + void* storage = ((StorageType*)data)->storage.bytes; new (storage) PinDevicePtr_t(wrapper.obj); data->convertible = storage; @@ -262,8 +261,10 @@ struct DeviceWrapperConverter { const Device& wrapper = extractor(); typedef std::shared_ptr ConstPinDevicePtr_t; - typedef boost::python::converter::rvalue_from_python_storage StorageType; - + typedef boost::python::converter::rvalue_from_python_storage< + ConstPinDevicePtr_t> + StorageType; + void* storage = ((StorageType*)data)->storage.bytes; new (storage) ConstPinDevicePtr_t(wrapper.obj); data->convertible = storage; @@ -273,7 +274,7 @@ struct DeviceWrapperConverter { void register_device_converters() { typedef std::shared_ptr PinDevicePtr_t; typedef std::shared_ptr ConstPinDevicePtr_t; - + boost::python::converter::registry::push_back( &DeviceWrapperConverter::convertible, &DeviceWrapperConverter::construct_shared_ptr, @@ -293,9 +294,9 @@ void exposeDevice() { .value("COM", hpp::pinocchio::COM) .value("COMPUTE_ALL", hpp::pinocchio::COMPUTE_ALL); void (Device::*cfk)(int) = &Device::computeForwardKinematics; - -class_, boost::noncopyable> - ("Device", no_init) + + class_, boost::noncopyable>("Device", + no_init) .def("__init__", make_constructor(&createDevice)) .def("name", &Device::name, return_value_policy()) .def("configSpace", &Device::configSpace, @@ -323,7 +324,7 @@ class_, boost::noncopyable> .def("getCenterOfMass", &getCenterOfMass) .def("getJointPosition", &getJointPosition) .def("getJointsPosition", &getJointsPosition); - register_device_converters(); + register_device_converters(); } } // namespace pinocchio } // namespace pyhpp diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py index 13d90716..d9d1b091 100644 --- a/tests/constraint_graph.py +++ b/tests/constraint_graph.py @@ -116,9 +116,7 @@ ] q = Quaternion(0, 0, 0, 1) ballGround = SE3(q, np.array([0, 0, 0.15])) -pc = Transformation( - "placementConstraint", robot, joint2, Id, ballGround, m -) +pc = Transformation("placementConstraint", robot, joint2, Id, ballGround, m) cts = ComparisonTypes() cts[:] = ( ComparisonType.EqualToZero, @@ -133,9 +131,7 @@ ballInGripper = SE3(q, np.array([0, 0.137, 0])) m = Mask() m[:] = (True,) * 6 -pc = RelativeTransformation( - "grasp", robot, joint1, joint2, ballInGripper, Id, m -) +pc = RelativeTransformation("grasp", robot, joint1, joint2, ballInGripper, Id, m) cts = ComparisonTypes() cts[:] = ( ComparisonType.EqualToZero, diff --git a/tests/graph_factory.py b/tests/graph_factory.py index 2e17d902..215addba 100644 --- a/tests/graph_factory.py +++ b/tests/graph_factory.py @@ -69,12 +69,8 @@ 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]) -) +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"] diff --git a/tests/load_ur3.py b/tests/load_ur3.py index e7e37e60..9cda9a76 100644 --- a/tests/load_ur3.py +++ b/tests/load_ur3.py @@ -21,9 +21,7 @@ m = Mask() m[:] = (True,) * 3 Id = SE3.Identity() -pc = Position( - "position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m -) +pc = Position("position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m) print(pc) qa = np.zeros((pc.ni, 1)) From ef6d4b0ed37ea3d9fa059f7f1b2a0da0da5941e3 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 13 Nov 2025 09:10:20 +0100 Subject: [PATCH 035/109] Add missing device header file --- src/pyhpp/pinocchio/device.hh | 73 +++++++++++++++++++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 src/pyhpp/pinocchio/device.hh diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh new file mode 100644 index 00000000..4f6a8199 --- /dev/null +++ b/src/pyhpp/pinocchio/device.hh @@ -0,0 +1,73 @@ +// +// Copyright (c) 2025, CNRS +// Authors: Paul Sardin +// + +#ifndef PYHPP_PINOCCHIO_DEVICE_HH +#define PYHPP_PINOCCHIO_DEVICE_HH + +#include +#include +#include + +namespace pyhpp { +namespace pinocchio { + +typedef hpp::pinocchio::Model Model; +typedef hpp::pinocchio::Data Data; +typedef hpp::pinocchio::ModelPtr_t ModelPtr_t; +typedef hpp::pinocchio::GeomData GeomData; +typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; +typedef hpp::pinocchio::GeomModel GeomModel; +typedef hpp::pinocchio::Configuration_t Configuration_t; +typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; +typedef hpp::pinocchio::size_type size_type; +typedef hpp::pinocchio::Transform3s Transform3s; +typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; +typedef hpp::pinocchio::Computation_t Computation_t; +typedef hpp::pinocchio::value_type value_type; + +struct Device { + DevicePtr_t obj; + + Device(const DevicePtr_t& object) : obj(object) {} + + const std::string& name() const { return obj->name(); } + const LiegroupSpacePtr_t& configSpace() const { return obj->configSpace(); } + Model& model() { return obj->model(); } + Data& data() { return obj->data(); } + GeomData& geomData() { return obj->geomData(); } + GeomModel& geomModel() { return obj->geomModel(); } + GeomModel& visualModel() { return obj->visualModel(); } + size_type configSize() const { return obj->configSize(); } + size_type numberDof() const { return obj->numberDof(); } + const Configuration_t& currentConfiguration() const { + return obj->currentConfiguration(); + } + bool currentConfiguration(ConfigurationIn_t configuration) { + return obj->currentConfiguration(configuration); + } + void computeForwardKinematics(int flag) { + obj->computeForwardKinematics(flag); + } + void computeFramesForwardKinematics() { + obj->computeFramesForwardKinematics(); + } + void updateGeometryPlacements() { + obj->updateGeometryPlacements(); + } + + + hpp::manipulation::DevicePtr_t asManipulationDevice() const { + auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj); + if (!manipDevice) { + throw std::runtime_error("Not a manipulation device²"); + } + return manipDevice; + } +}; + +} // namespace pinocchio +} // namespace pyhpp + +#endif // PYHPP_PINOCCHIO_DEVICE_HH \ No newline at end of file From 0d4d3c25f6f7ecb1d3f18f33df37ac1102ee688b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Nov 2025 08:10:34 +0000 Subject: [PATCH 036/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/pinocchio/device.hh | 29 +++++++++++++---------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 4f6a8199..1b59d284 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/src/pyhpp/pinocchio/device.hh @@ -6,8 +6,8 @@ #ifndef PYHPP_PINOCCHIO_DEVICE_HH #define PYHPP_PINOCCHIO_DEVICE_HH -#include #include +#include #include namespace pyhpp { @@ -29,9 +29,9 @@ typedef hpp::pinocchio::value_type value_type; struct Device { DevicePtr_t obj; - + Device(const DevicePtr_t& object) : obj(object) {} - + const std::string& name() const { return obj->name(); } const LiegroupSpacePtr_t& configSpace() const { return obj->configSpace(); } Model& model() { return obj->model(); } @@ -41,22 +41,19 @@ struct Device { GeomModel& visualModel() { return obj->visualModel(); } size_type configSize() const { return obj->configSize(); } size_type numberDof() const { return obj->numberDof(); } - const Configuration_t& currentConfiguration() const { - return obj->currentConfiguration(); - } - bool currentConfiguration(ConfigurationIn_t configuration) { - return obj->currentConfiguration(configuration); + const Configuration_t& currentConfiguration() const { + return obj->currentConfiguration(); } - void computeForwardKinematics(int flag) { - obj->computeForwardKinematics(flag); + bool currentConfiguration(ConfigurationIn_t configuration) { + return obj->currentConfiguration(configuration); } - void computeFramesForwardKinematics() { - obj->computeFramesForwardKinematics(); + void computeForwardKinematics(int flag) { + obj->computeForwardKinematics(flag); } - void updateGeometryPlacements() { - obj->updateGeometryPlacements(); + void computeFramesForwardKinematics() { + obj->computeFramesForwardKinematics(); } - + void updateGeometryPlacements() { obj->updateGeometryPlacements(); } hpp::manipulation::DevicePtr_t asManipulationDevice() const { auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj); @@ -70,4 +67,4 @@ struct Device { } // namespace pinocchio } // namespace pyhpp -#endif // PYHPP_PINOCCHIO_DEVICE_HH \ No newline at end of file +#endif // PYHPP_PINOCCHIO_DEVICE_HH From 4df5cdcd92b10d7d55a52c45bbd7a343536b48bb Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" Date: Thu, 13 Nov 2025 16:21:02 +0000 Subject: [PATCH 037/109] flake.lock: Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Flake lock file updates: • Updated input 'gepetto': 'github:gepetto/nix/e82291718d5bc01d145781811db48b4f22eb6ce1?narHash=sha256-eEuunrRx0tfi0oq05CuQsV0/U/q6yQYCKUEUbjT0Am8%3D' (2025-10-24) → 'github:gepetto/nix/6dccd759d955686bdf6e63006993219478de90df?narHash=sha256-rhs3zT1eavw6kjJ3wZSY%2BFR4jikS1NU6MFWBYNfpQUU%3D' (2025-11-10) • Updated input 'gepetto/flake-parts': 'github:hercules-ci/flake-parts/864599284fc7c0ba6357ed89ed5e2cd5040f0c04?narHash=sha256-TmWcdiUUaWk8J4lpjzu4gCGxWY6/Ok7mOK4fIFfBuU4%3D' (2025-10-20) → 'github:hercules-ci/flake-parts/26d05891e14c88eb4a5d5bee659c0db5afb609d8?narHash=sha256-xxdepIcb39UJ94%2BYydGP221rjnpkDZUlykKuF54PsqI%3D' (2025-11-06) • Updated input 'gepetto/flake-parts/nixpkgs-lib': 'github:nix-community/nixpkgs.lib/a73b9c743612e4244d865a2fdee11865283c04e6?narHash=sha256-x2rJ%2BOvzq0sCMpgfgGaaqgBSwY%2BLST%2BWbZ6TytnT9Rk%3D' (2025-08-10) → 'github:nix-community/nixpkgs.lib/719359f4562934ae99f5443f20aa06c2ffff91fc?narHash=sha256-b0yj6kfvO8ApcSE%2BQmA6mUfu8IYG6/uU28OFn4PaC8M%3D' (2025-10-29) • Updated input 'gepetto/nix-ros-overlay': 'github:lopsided98/nix-ros-overlay/8a927d104d10114d64528d3731eb7663bea5d843?narHash=sha256-eojj3Mggmd8fAHlEsl%2Bc9pbPWwsmQqmn8u/FoYX/az4%3D' (2025-10-20) → 'github:lopsided98/nix-ros-overlay/1622b7a0adbd01006800210bc7b298fd430a2d34?narHash=sha256-Mmo6HuDXoYIt1Qez5ytc5ejdhUSoej9SqW5hhvNmEeg%3D' (2025-11-08) • Updated input 'gepetto/src-agimus-controller': 'github:agimus-project/agimus_controller/1a4eff6ded0e08b06bfaddb8087ad01a74356c73?narHash=sha256-tJieqjLTKS0QUHmUew8qgjCYTejhMfYMFrndFUV6rJ4%3D' (2025-10-21) → 'github:agimus-project/agimus_controller/09a30869d70c6c2689a2700c83eb73b6541d96f2?narHash=sha256-K05RaiRLm%2BEbCJKPw36c9YwSyhOTOvtDUx1I/HoBx6Q%3D' (2025-11-06) • Updated input 'gepetto/src-franka-description': 'github:agimus-project/franka_description/3cad53b368ea19f10ce1248f4fd781abfae1bdc6?narHash=sha256-D7vkjZ1B9qKecqUCmnpwHcxzZpakvoqp0qAhv4jGwRI%3D' (2025-09-29) → 'github:agimus-project/franka_description/fa4b70f76908c30d08fc9ae4bb77c45d67109311?narHash=sha256-JySeYjg77n6SHstTqyy6PaYBzQcA38vkM6gjTs4Iwt0%3D' (2025-11-04) • Updated input 'gepetto/system-manager': 'github:numtide/system-manager/f6597102b6e053ee6f8d797e1e8bf8e2a3bdd7d7?narHash=sha256-x/E0qSvHP5YHBaf%2BEe2eFr6Ph14upcSEQ1WC0uLrcmQ%3D' (2025-10-23) → 'github:numtide/system-manager/26fb1d50067e63c39be557e4fcedbd1aeec64c2e?narHash=sha256-eax/ncPr9rw0qkcb7JKTyL9Un8BzsEe3Pnor8kG9AcQ%3D' (2025-10-30) • Updated input 'gepetto/treefmt-nix': 'github:numtide/treefmt-nix/f56b1934f5f8fcab8deb5d38d42fd692632b47c2?narHash=sha256-ZRVs8UqikBa4Ki3X4KCnMBtBW0ux1DaT35tgsnB1jM4%3D' (2025-10-20) → 'github:numtide/treefmt-nix/97a30861b13c3731a84e09405414398fbf3e109f?narHash=sha256-aF5fvoZeoXNPxT0bejFUBXeUjXfHLSL7g%2BmjR/p5TEg%3D' (2025-11-06) --- flake.lock | 48 ++++++++++++++++++++++++------------------------ 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/flake.lock b/flake.lock index 3f2ef499..5a3b06ca 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1760948891, - "narHash": "sha256-TmWcdiUUaWk8J4lpjzu4gCGxWY6/Ok7mOK4fIFfBuU4=", + "lastModified": 1762440070, + "narHash": "sha256-xxdepIcb39UJ94+YydGP221rjnpkDZUlykKuF54PsqI=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "864599284fc7c0ba6357ed89ed5e2cd5040f0c04", + "rev": "26d05891e14c88eb4a5d5bee659c0db5afb609d8", "type": "github" }, "original": { @@ -61,11 +61,11 @@ "treefmt-nix": "treefmt-nix" }, "locked": { - "lastModified": 1761305265, - "narHash": "sha256-eEuunrRx0tfi0oq05CuQsV0/U/q6yQYCKUEUbjT0Am8=", + "lastModified": 1762792495, + "narHash": "sha256-rhs3zT1eavw6kjJ3wZSY+FR4jikS1NU6MFWBYNfpQUU=", "owner": "gepetto", "repo": "nix", - "rev": "e82291718d5bc01d145781811db48b4f22eb6ce1", + "rev": "6dccd759d955686bdf6e63006993219478de90df", "type": "github" }, "original": { @@ -80,11 +80,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1760993928, - "narHash": "sha256-eojj3Mggmd8fAHlEsl+c9pbPWwsmQqmn8u/FoYX/az4=", + "lastModified": 1762593451, + "narHash": "sha256-Mmo6HuDXoYIt1Qez5ytc5ejdhUSoej9SqW5hhvNmEeg=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "8a927d104d10114d64528d3731eb7663bea5d843", + "rev": "1622b7a0adbd01006800210bc7b298fd430a2d34", "type": "github" }, "original": { @@ -133,11 +133,11 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1754788789, - "narHash": "sha256-x2rJ+Ovzq0sCMpgfgGaaqgBSwY+LST+WbZ6TytnT9Rk=", + "lastModified": 1761765539, + "narHash": "sha256-b0yj6kfvO8ApcSE+QmA6mUfu8IYG6/uU28OFn4PaC8M=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "a73b9c743612e4244d865a2fdee11865283c04e6", + "rev": "719359f4562934ae99f5443f20aa06c2ffff91fc", "type": "github" }, "original": { @@ -174,11 +174,11 @@ "src-agimus-controller": { "flake": false, "locked": { - "lastModified": 1761049897, - "narHash": "sha256-tJieqjLTKS0QUHmUew8qgjCYTejhMfYMFrndFUV6rJ4=", + "lastModified": 1762444174, + "narHash": "sha256-K05RaiRLm+EbCJKPw36c9YwSyhOTOvtDUx1I/HoBx6Q=", "owner": "agimus-project", "repo": "agimus_controller", - "rev": "1a4eff6ded0e08b06bfaddb8087ad01a74356c73", + "rev": "09a30869d70c6c2689a2700c83eb73b6541d96f2", "type": "github" }, "original": { @@ -206,11 +206,11 @@ "src-franka-description": { "flake": false, "locked": { - "lastModified": 1759137774, - "narHash": "sha256-D7vkjZ1B9qKecqUCmnpwHcxzZpakvoqp0qAhv4jGwRI=", + "lastModified": 1762271824, + "narHash": "sha256-JySeYjg77n6SHstTqyy6PaYBzQcA38vkM6gjTs4Iwt0=", "owner": "agimus-project", "repo": "franka_description", - "rev": "3cad53b368ea19f10ce1248f4fd781abfae1bdc6", + "rev": "fa4b70f76908c30d08fc9ae4bb77c45d67109311", "type": "github" }, "original": { @@ -261,11 +261,11 @@ ] }, "locked": { - "lastModified": 1761207312, - "narHash": "sha256-x/E0qSvHP5YHBaf+Ee2eFr6Ph14upcSEQ1WC0uLrcmQ=", + "lastModified": 1761809512, + "narHash": "sha256-eax/ncPr9rw0qkcb7JKTyL9Un8BzsEe3Pnor8kG9AcQ=", "owner": "numtide", "repo": "system-manager", - "rev": "f6597102b6e053ee6f8d797e1e8bf8e2a3bdd7d7", + "rev": "26fb1d50067e63c39be557e4fcedbd1aeec64c2e", "type": "github" }, "original": { @@ -297,11 +297,11 @@ ] }, "locked": { - "lastModified": 1760945191, - "narHash": "sha256-ZRVs8UqikBa4Ki3X4KCnMBtBW0ux1DaT35tgsnB1jM4=", + "lastModified": 1762410071, + "narHash": "sha256-aF5fvoZeoXNPxT0bejFUBXeUjXfHLSL7g+mjR/p5TEg=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "f56b1934f5f8fcab8deb5d38d42fd692632b47c2", + "rev": "97a30861b13c3731a84e09405414398fbf3e109f", "type": "github" }, "original": { From 019988b83c15fca72fef19cc46057dadfe03833d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 17 Nov 2025 17:12:21 +0000 Subject: [PATCH 038/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.4 → v0.14.5](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.4...v0.14.5) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5188d045..7f43a24b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.4 + rev: v0.14.5 hooks: - id: ruff args: From a8034676807e848feaf742688c4c263a09499cb8 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Mon, 24 Nov 2025 11:54:59 +0000 Subject: [PATCH 039/109] Bump actions/checkout from 5 to 6 Bumps [actions/checkout](https://github.com/actions/checkout) from 5 to 6. - [Release notes](https://github.com/actions/checkout/releases) - [Changelog](https://github.com/actions/checkout/blob/main/CHANGELOG.md) - [Commits](https://github.com/actions/checkout/compare/v5...v6) --- updated-dependencies: - dependency-name: actions/checkout dependency-version: '6' dependency-type: direct:production update-type: version-update:semver-major ... Signed-off-by: dependabot[bot] --- .github/workflows/nix.yml | 2 +- .github/workflows/update-flake-lock.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml index 8f469ead..18849861 100644 --- a/.github/workflows/nix.yml +++ b/.github/workflows/nix.yml @@ -17,7 +17,7 @@ jobs: matrix: os: [ubuntu] # TODO steps: - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - uses: cachix/install-nix-action@v31 - uses: cachix/cachix-action@v16 with: diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml index d32267f0..07c09291 100644 --- a/.github/workflows/update-flake-lock.yml +++ b/.github/workflows/update-flake-lock.yml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Checkout repository - uses: actions/checkout@v5 + uses: actions/checkout@v6 - name: Install Nix uses: DeterminateSystems/nix-installer-action@main - name: Update flake.lock From 6aa32ab95a221f57a6eb1b1f66d1cd82dff3d625 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Nov 2025 17:16:12 +0000 Subject: [PATCH 040/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.5 → v0.14.6](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.5...v0.14.6) - [github.com/pre-commit/mirrors-clang-format: v21.1.5 → v21.1.6](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.5...v21.1.6) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7f43a24b..927cf9df 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.5 + rev: v0.14.6 hooks: - id: ruff args: @@ -18,7 +18,7 @@ repos: hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.5 + rev: v21.1.6 hooks: - id: clang-format args: From c04b2780204f82461314e576dddf96c24a64f893 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 18 Nov 2025 08:54:04 +0100 Subject: [PATCH 041/109] Add security margins helper python script for benchmarks --- src/CMakeLists.txt | 1 + src/pyhpp/manipulation/security_margins.py | 153 +++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 src/pyhpp/manipulation/security_margins.py diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ebf76193..37baa7b6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -70,6 +70,7 @@ python_install_on_site(pyhpp __init__.py) python_install_on_site(pyhpp/manipulation constraint_graph_factory.py) python_install_on_site(pyhpp/core static_stability_constraint_factory.py) +python_install_on_site(pyhpp/manipulation security_margins.py) add_python_library( pyhpp/pinocchio diff --git a/src/pyhpp/manipulation/security_margins.py b/src/pyhpp/manipulation/security_margins.py new file mode 100644 index 00000000..112d74e1 --- /dev/null +++ b/src/pyhpp/manipulation/security_margins.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 CNRS, Airbus SAS +# Author: Florent Lamiraux +# + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are +# met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. + + +class SecurityMargins: + defaultMargin = 0 + separators = ["/"] + + def __init__(self, problem, factory, robotsAndObjects, robot): + self.problem = problem + self.robot = robot + self.factory = factory + self.robotsAndObjects = robotsAndObjects + self.marginMatrix = dict() + self.computeJoints() + self.computeGrippers() + self.computePossibleContacts() + + def computeJoints(self): + self.robotToJoints = dict() + jointNames = self.robot.model().names + for ro in self.robotsAndObjects: + le = len(ro) + self.robotToJoints[ro] = [ + n for n in jointNames + if n[:le] == ro and (len(n) == le or n[le] in self.separators) + ] + self.robotToJoints["universe"] = ["universe"] + self.jointToRobot = dict() + for ro, joints in self.robotToJoints.items(): + for j in joints: + self.jointToRobot[j] = ro + + def computeGrippers(self): + self.gripperToRobot = dict() + self.gripperToJoints = dict() + + for g in self.factory.grippers: + for joint_name in self.robot.model().names: + if g.startswith(joint_name): + self.gripperToRobot[g] = self.jointToRobot.get(joint_name, "unknown") + self.gripperToJoints[g] = [joint_name] + break + + def computePossibleContacts(self): + self.contactSurfaces = {k: [] for k in self.robotToJoints.keys()} + self.possibleContacts = [ + (o1, o2) + for o1, l1 in self.contactSurfaces.items() + for o2, l2 in self.contactSurfaces.items() + if o1 != o2 and len(l1) > 0 and len(l2) > 0 + ] + + def setSecurityMarginBetween(self, obj1, obj2, margin): + self.marginMatrix[frozenset([obj1, obj2])] = margin + + def getSecurityMarginBetween(self, obj1, obj2): + return self.marginMatrix.get(frozenset([obj1, obj2]), self.defaultMargin) + + def getActiveConstraintsAlongEdge(self, edge): + factory = self.factory + graph = factory.graph + + result = graph.getNodesConnectedByTransition(edge) + from_name, to_name = result if isinstance(result, tuple) else (result, result) + + state_from = graph.getState(from_name) + state_to = graph.getState(to_name) + + constraints_from = graph.getNumericalConstraintsForState(state_from) + constraints_graph = graph.getNumericalConstraintsForGraph() + constraints_to = graph.getNumericalConstraintsForState(state_to) + + all_constraint_objects = set(constraints_from + constraints_graph + constraints_to) + + active_names = [] + if hasattr(factory, 'constraints') and hasattr(factory.constraints, 'available_constraints'): + constraint_dict = factory.constraints.available_constraints + for name, constraint_obj in constraint_dict.items(): + if constraint_obj in all_constraint_objects: + active_names.append(name) + + res = {"place": [], "grasp": []} + + for c in active_names: + for o in factory.objects: + if c == "place_" + o: + res["place"].append(o) + for g in factory.grippers: + for o, handle_indices in zip(factory.objects, factory.handlesPerObjects): + for h_idx in handle_indices: + h = factory.handles[h_idx] + if c == g + " grasps " + h: + res["grasp"].append((g, o)) + + return res + + def apply(self): + factory = self.factory + graph = factory.graph + edges = graph.getTransitions() + + for edge in edges: + for i1, ro1 in enumerate([*self.robotsAndObjects, "universe"]): + for i2, ro2 in enumerate([*self.robotsAndObjects, "universe"]): + if i2 < i1: + continue + margin = self.getSecurityMarginBetween(ro1, ro2) + for j1 in self.robotToJoints[ro1]: + for j2 in self.robotToJoints[ro2]: + if j1 != j2: + graph.setSecurityMarginForTransition(edge, j1, j2, margin) + + constraints = self.getActiveConstraintsAlongEdge(edge) + for g, ro1 in constraints["grasp"]: + if g in self.gripperToJoints and ro1 in self.robotToJoints: + for j1 in self.robotToJoints[ro1]: + for j2 in self.gripperToJoints[g]: + graph.setSecurityMarginForTransition(edge, j1, j2, 0) + + for o1 in constraints["place"]: + for o2, o3 in self.possibleContacts: + if o1 == o2: + for j1 in self.robotToJoints[o1]: + for j2 in self.robotToJoints[o3]: + graph.setSecurityMarginForTransition(edge, j1, j2, 0) From e9c06893840c24df3c93a1812a974f0c32726423 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 18 Nov 2025 08:55:38 +0100 Subject: [PATCH 042/109] Add random shortcut path optimizer --- src/pyhpp/core/path-optimizer.cc | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index 4a3b3b12..9559a044 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -32,6 +32,8 @@ #include #include #include +#include +#include using namespace boost::python; @@ -41,7 +43,18 @@ using namespace hpp::core; void exposePathOptimizer() { class_("PathOptimizer", - no_init); + no_init) + .def("problem", &PathOptimizer::problem) + .def("optimize", &PathOptimizer::optimize) + .def("interrupt", &PathOptimizer::interrupt) + .def("maxIterations", &PathOptimizer::maxIterations) + .def("timeOut", &PathOptimizer::timeOut); + + class_, bases, + boost::noncopyable>("RandomShortcut", no_init) + .def("__init__", make_constructor(&pathOptimization::RandomShortcut::create)); + + } } // namespace core } // namespace pyhpp From 3d895e5810478fff0ba5c0536c5b84be46636752 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 18 Nov 2025 08:56:53 +0100 Subject: [PATCH 043/109] Fix static stability constraint factory --- src/pyhpp/core/problem.cc | 16 +++++--------- .../static_stability_constraint_factory.py | 21 +++++++++---------- tests/benchmarks/pyrene-on-the-ground.py | 2 +- 3 files changed, 16 insertions(+), 23 deletions(-) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index b4039063..85d51873 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -236,18 +236,14 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( std::string name(constraintName); hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); auto mask = extract_vector(m); - - const hpp::pinocchio::Transform3s ref2 = M; - + const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M; if (joint1Name && std::strlen(joint1Name) > 0) { hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); - - const hpp::pinocchio::Transform3s ref1 = - hpp::pinocchio::Transform3s::Identity(); + const hpp::pinocchio::Transform3s ref1 = f1.positionInParentJoint(); auto func = GenericTransformation< PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), - f2.joint(), ref1, ref2, mask); + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { @@ -287,14 +283,12 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( auto func = GenericTransformation< PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), - f2.joint(), ref1, ref2, mask); + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { - const hpp::pinocchio::Transform3s Id = - hpp::pinocchio::Transform3s::Identity(); auto func = GenericTransformation::create( - name, robot(), f2.joint(), ref2, Id, mask); + name, robot(), f2.joint(), ref2, M1, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 952d62f5..112a4b0d 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -34,10 +34,9 @@ # # This class provides tools to create static stability constraints class StaticStabilityConstraintsFactory: - def __init__(self, problem): + def __init__(self, problem, robot): self.problem = problem - self.robot = problem.robot() - + self.robot = robot def _getCOM(self, com): from numpy import array @@ -75,7 +74,7 @@ def createSlidingStabilityConstraint( result.append(prefix + "relative-pose") rel_transform = Mr.inverse() * Ml - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], leftAnkle, rightAnkle, @@ -86,7 +85,7 @@ def createSlidingStabilityConstraint( created_constraints[result[-1]] = constraint result.append(prefix + "pose-left-foot") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", leftAnkle, @@ -97,7 +96,7 @@ def createSlidingStabilityConstraint( created_constraints[result[-1]] = constraint result.append(prefix + "pose-left-foot-complement") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", leftAnkle, @@ -112,7 +111,7 @@ def createSlidingStabilityConstraint( # # # Create static stability constraints where the feet are fixed on the ground, def createStaticStabilityConstraint( - self, prefix, comName, leftAnkle, rightAnkle, q0, maskCom=(True,) * 3 + self, prefix, comName, leftAnkle, rightAnkle, q0, maskCom=[True] * 3 ): created_constraints = dict() @@ -133,7 +132,7 @@ def createStaticStabilityConstraint( # Pose of the left foot result.append(prefix + "pose-left-foot") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", leftAnkle, @@ -145,7 +144,7 @@ def createStaticStabilityConstraint( # Pose of the right foot result.append(prefix + "pose-right-foot") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", rightAnkle, @@ -192,7 +191,7 @@ def createAlignedCOMStabilityConstraint( # Pose of the right foot result.append(prefix + "pose-right-foot") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", rightAnkle, @@ -204,7 +203,7 @@ def createAlignedCOMStabilityConstraint( # Pose of the left foot result.append(prefix + "pose-left-foot") - constraint = self.problem.createTransformationConstraint2( + constraint = self.problem.createTransformationConstraint( result[-1], "", leftAnkle, diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 574ad2d8..50e56e6f 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -105,7 +105,7 @@ leftAnkle = "pyrene/leg_left_6_joint" rightAnkle = "pyrene/leg_right_6_joint" -factory = StaticStabilityConstraintsFactory(problem) +factory = StaticStabilityConstraintsFactory(problem, robot) constraints = factory.createSlidingStabilityConstraint( "balance/", "pyrene", leftAnkle, rightAnkle, q0 ) From 4e626ba814f84eafde527ceec57f464730ca5388 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 18 Nov 2025 08:57:30 +0100 Subject: [PATCH 044/109] Fix getNodesConnectedByTransition prototype --- src/pyhpp/manipulation/graph.cc | 9 ++++----- src/pyhpp/manipulation/graph.hh | 3 +-- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 8d00c431..470c1d69 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -407,12 +407,11 @@ bool PyWGraph::isShort(PyWEdgePtr_t edge) { } } -void PyWGraph::getNodesConnectedByTransition(PyWEdgePtr_t edge, - std::string& nodeFrom, - std::string& nodeTo) { +boost::python::tuple PyWGraph::getNodesConnectedByTransition(PyWEdgePtr_t edge) { try { - nodeFrom = edge->obj->stateFrom()->name(); - nodeTo = edge->obj->stateTo()->name(); + std::string nodeFrom = edge->obj->stateFrom()->name(); + std::string nodeTo = edge->obj->stateTo()->name(); + return boost::python::make_tuple(nodeFrom, nodeTo); } catch (const std::exception& exc) { throw std::logic_error(exc.what()); } diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index d1befd58..a01cadae 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -117,8 +117,7 @@ struct PyWGraph { std::string getContainingNode(PyWEdgePtr_t edge); void setShort(PyWEdgePtr_t edge, bool isShort); bool isShort(PyWEdgePtr_t edge); - void getNodesConnectedByTransition(PyWEdgePtr_t edge, std::string& nodeFrom, - std::string& nodeTo); + boost::python::tuple getNodesConnectedByTransition(PyWEdgePtr_t edge); void setWeight(PyWEdgePtr_t edge, int weight); size_t getWeight(PyWEdgePtr_t edge); void setWaypoint(PyWEdgePtr_t waypointEdge, int index, PyWEdgePtr_t edge, From f3c7a4bacd999de75b47a1db04446f9f27a3f488 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 19 Nov 2025 08:52:34 +0100 Subject: [PATCH 045/109] Add StatesPathFinder planner --- src/pyhpp/manipulation/path-planner.cc | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 60e7be92..4b13058a 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,17 @@ struct ManipulationPlanner : public pyhpp::core::PathPlanner { } }; +struct StatesPathFinder : public pyhpp::core::PathPlanner { + StatesPathFinder(const pyhpp::core::Problem& problem) { + hpp::manipulation::RoadmapPtr_t roadmap = + hpp::manipulation::Roadmap::create(problem.obj->distance(), + problem.obj->robot()); + obj = hpp::manipulation::pathPlanner::StatesPathFinder::createWithRoadmap(problem.obj, roadmap); + roadmap->constraintGraph( + problem.asManipulationProblem()->constraintGraph()); + } +}; + // TransitionPlanner implementation TransitionPlanner::TransitionPlanner(const pyhpp::core::Problem& problem) { obj = hpp::manipulation::pathPlanner::TransitionPlanner::createWithRoadmap( @@ -241,6 +253,11 @@ void exposePathPlanners() { "ManipulationPlanner", boost::python::init()); + boost::python::class_>( + "StatesPathFinder", + boost::python::init()); + boost::python::class_>( "EndEffectorTrajectory", From f931e538c64510c7102411e16a147685e3a4f504 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 20 Nov 2025 08:17:28 +0100 Subject: [PATCH 046/109] Bind more path optimizers --- include/pyhpp/manipulation/fwd.hh | 1 + src/CMakeLists.txt | 1 + src/pyhpp/core/path-optimizer.cc | 21 +++++++ src/pyhpp/manipulation/bindings.cc | 3 + src/pyhpp/manipulation/path-optimizer.cc | 74 ++++++++++++++++++++++++ src/pyhpp/manipulation/problem.cc | 1 - 6 files changed, 100 insertions(+), 1 deletion(-) create mode 100644 src/pyhpp/manipulation/path-optimizer.cc diff --git a/include/pyhpp/manipulation/fwd.hh b/include/pyhpp/manipulation/fwd.hh index e94b5c70..3030cbdb 100644 --- a/include/pyhpp/manipulation/fwd.hh +++ b/include/pyhpp/manipulation/fwd.hh @@ -54,6 +54,7 @@ void exposePathPlanners(); void exposeProblem(); void exposePathProjector(); void exposeManipSteeringMethod(); +void exposePathOptimizers(); } // namespace manipulation } // namespace pyhpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 37baa7b6..eae87840 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -160,6 +160,7 @@ add_python_library( pyhpp/manipulation/path-planner.hh pyhpp/manipulation/path-projector.cc pyhpp/manipulation/steering-method.cc + pyhpp/manipulation/path-optimizer.cc LINK_LIBRARIES hpp-manipulation::hpp-manipulation) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index 9559a044..afc45fb0 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -33,6 +33,11 @@ #include #include #include +#include +#include +#include +#include + #include using namespace boost::python; @@ -54,6 +59,22 @@ void exposePathOptimizer() { boost::noncopyable>("RandomShortcut", no_init) .def("__init__", make_constructor(&pathOptimization::RandomShortcut::create)); + class_, bases, + boost::noncopyable>("SimpleShortcut", no_init) + .def("__init__", make_constructor(&pathOptimization::SimpleShortcut::create)); + + class_, bases, + boost::noncopyable>("PartialShortcut", no_init) + .def("__init__", make_constructor(&pathOptimization::PartialShortcut::create)); + + class_, bases, + boost::noncopyable>("SimpleTimeParameterization", no_init) + .def("__init__", make_constructor(&pathOptimization::SimpleTimeParameterization::create)); + + class_, bases, + boost::noncopyable>("RSTimeParameterization", no_init) + .def("__init__", make_constructor(&pathOptimization::RSTimeParameterization::create)); + } } // namespace core diff --git a/src/pyhpp/manipulation/bindings.cc b/src/pyhpp/manipulation/bindings.cc index c6aa9778..202704ab 100644 --- a/src/pyhpp/manipulation/bindings.cc +++ b/src/pyhpp/manipulation/bindings.cc @@ -36,6 +36,7 @@ BOOST_PYTHON_MODULE(bindings) { boost::python::import("pyhpp.pinocchio"); boost::python::import("pyhpp.constraints"); + boost::python::import("pyhpp.core"); pyhpp::manipulation::exposeHandle(); pyhpp::manipulation::exposeProblem(); pyhpp::manipulation::exposeDevice(); @@ -43,4 +44,6 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::manipulation::exposePathProjector(); pyhpp::manipulation::exposeManipSteeringMethod(); pyhpp::manipulation::exposePathPlanners(); + pyhpp::manipulation::exposePathOptimizers(); + } diff --git a/src/pyhpp/manipulation/path-optimizer.cc b/src/pyhpp/manipulation/path-optimizer.cc new file mode 100644 index 00000000..5ccd3de1 --- /dev/null +++ b/src/pyhpp/manipulation/path-optimizer.cc @@ -0,0 +1,74 @@ +// +// Copyright (c) 2025, CNRS +// Authors: Paul Sardin +// +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::python; + + +namespace pyhpp { +namespace manipulation { +using namespace hpp::manipulation; + +template +hpp::core::PathOptimizerPtr_t createGraphOptimizer(const hpp::core::ProblemConstPtr_t& problem) { + return GraphOptimizer::create(problem); +} + +void exposePathOptimizers() { + + class_, + bases, + boost::noncopyable>("RandomShortcut", no_init) + .def("__init__", make_constructor(&pathOptimization::RandomShortcut::create)); + + class_, + bases, + boost::noncopyable>("EnforceTransitionSemantic", no_init) + .def("__init__", make_constructor(&pathOptimization::EnforceTransitionSemantic::create)); + + def("GraphRandomShortcut", + &createGraphOptimizer); + + def("GraphPartialShortcut", + &createGraphOptimizer); +} + +} // namespace manipulation +} // namespace pyhpp diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index 44111b6e..30b609a2 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -121,7 +121,6 @@ void Problem::graphSteeringMethod( // } void exposeProblem() { - boost::python::import("pyhpp.core"); class_>("Problem", init()) .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(Problem, constraintGraph, From ad996a906e11c8cab4f13a065c62e23dcf84c015 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 24 Nov 2025 09:20:12 +0000 Subject: [PATCH 047/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/path-optimizer.cc | 56 +++++++++++-------- src/pyhpp/core/problem.cc | 4 +- .../static_stability_constraint_factory.py | 1 + src/pyhpp/manipulation/bindings.cc | 1 - src/pyhpp/manipulation/graph.cc | 3 +- src/pyhpp/manipulation/path-optimizer.cc | 36 ++++++------ src/pyhpp/manipulation/path-planner.cc | 10 ++-- src/pyhpp/manipulation/security_margins.py | 53 +++++++++++------- 8 files changed, 94 insertions(+), 70 deletions(-) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index afc45fb0..f188d0b4 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -29,16 +29,15 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include -#include -#include #include +#include #include +#include #include - +#include #include +#include +#include using namespace boost::python; @@ -55,27 +54,38 @@ void exposePathOptimizer() { .def("maxIterations", &PathOptimizer::maxIterations) .def("timeOut", &PathOptimizer::timeOut); - class_, bases, - boost::noncopyable>("RandomShortcut", no_init) - .def("__init__", make_constructor(&pathOptimization::RandomShortcut::create)); - - class_, bases, - boost::noncopyable>("SimpleShortcut", no_init) - .def("__init__", make_constructor(&pathOptimization::SimpleShortcut::create)); - - class_, bases, - boost::noncopyable>("PartialShortcut", no_init) - .def("__init__", make_constructor(&pathOptimization::PartialShortcut::create)); + class_, + bases, boost::noncopyable>("RandomShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::RandomShortcut::create)); - class_, bases, - boost::noncopyable>("SimpleTimeParameterization", no_init) - .def("__init__", make_constructor(&pathOptimization::SimpleTimeParameterization::create)); + class_, + bases, boost::noncopyable>("SimpleShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::SimpleShortcut::create)); - class_, bases, - boost::noncopyable>("RSTimeParameterization", no_init) - .def("__init__", make_constructor(&pathOptimization::RSTimeParameterization::create)); + class_, + bases, boost::noncopyable>("PartialShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::PartialShortcut::create)); + class_, + bases, boost::noncopyable>("SimpleTimeParameterization", + no_init) + .def("__init__", + make_constructor( + &pathOptimization::SimpleTimeParameterization::create)); + class_, + bases, boost::noncopyable>("RSTimeParameterization", + no_init) + .def("__init__", + make_constructor(&pathOptimization::RSTimeParameterization::create)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 85d51873..a8376220 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -243,7 +243,7 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( auto func = GenericTransformation< PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), - f2.joint(), ref1, ref2, mask); + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { @@ -283,7 +283,7 @@ hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( auto func = GenericTransformation< PositionBit | OrientationBit | hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), - f2.joint(), ref1, ref2, mask); + f2.joint(), ref1, ref2, mask); return Implicit::create( func, numberOfTrue(mask) * hpp::constraints::EqualToZero); } else { diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py index 112a4b0d..0f8bf5da 100644 --- a/src/pyhpp/core/static_stability_constraint_factory.py +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -37,6 +37,7 @@ class StaticStabilityConstraintsFactory: def __init__(self, problem, robot): self.problem = problem self.robot = robot + def _getCOM(self, com): from numpy import array diff --git a/src/pyhpp/manipulation/bindings.cc b/src/pyhpp/manipulation/bindings.cc index 202704ab..9d280791 100644 --- a/src/pyhpp/manipulation/bindings.cc +++ b/src/pyhpp/manipulation/bindings.cc @@ -45,5 +45,4 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::manipulation::exposeManipSteeringMethod(); pyhpp::manipulation::exposePathPlanners(); pyhpp::manipulation::exposePathOptimizers(); - } diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 470c1d69..a362afa5 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -407,7 +407,8 @@ bool PyWGraph::isShort(PyWEdgePtr_t edge) { } } -boost::python::tuple PyWGraph::getNodesConnectedByTransition(PyWEdgePtr_t edge) { +boost::python::tuple PyWGraph::getNodesConnectedByTransition( + PyWEdgePtr_t edge) { try { std::string nodeFrom = edge->obj->stateFrom()->name(); std::string nodeTo = edge->obj->stateTo()->name(); diff --git a/src/pyhpp/manipulation/path-optimizer.cc b/src/pyhpp/manipulation/path-optimizer.cc index 5ccd3de1..5a761361 100644 --- a/src/pyhpp/manipulation/path-optimizer.cc +++ b/src/pyhpp/manipulation/path-optimizer.cc @@ -29,43 +29,45 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include #include #include -#include -#include +#include #include #include -#include using namespace boost::python; - namespace pyhpp { namespace manipulation { using namespace hpp::manipulation; -template -hpp::core::PathOptimizerPtr_t createGraphOptimizer(const hpp::core::ProblemConstPtr_t& problem) { +template +hpp::core::PathOptimizerPtr_t createGraphOptimizer( + const hpp::core::ProblemConstPtr_t& problem) { return GraphOptimizer::create(problem); } void exposePathOptimizers() { - - class_, - bases, - boost::noncopyable>("RandomShortcut", no_init) - .def("__init__", make_constructor(&pathOptimization::RandomShortcut::create)); + class_, + bases, boost::noncopyable>("RandomShortcut", + no_init) + .def("__init__", + make_constructor(&pathOptimization::RandomShortcut::create)); class_, - bases, - boost::noncopyable>("EnforceTransitionSemantic", no_init) - .def("__init__", make_constructor(&pathOptimization::EnforceTransitionSemantic::create)); + bases, boost::noncopyable>( + "EnforceTransitionSemantic", no_init) + .def("__init__", + make_constructor( + &pathOptimization::EnforceTransitionSemantic::create)); - def("GraphRandomShortcut", + def("GraphRandomShortcut", &createGraphOptimizer); - + def("GraphPartialShortcut", &createGraphOptimizer); } diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 4b13058a..6d6e453f 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -31,8 +31,8 @@ #include <../src/pyhpp/manipulation/path-planner.hh> #include #include -#include #include +#include #include #include #include @@ -57,7 +57,8 @@ struct StatesPathFinder : public pyhpp::core::PathPlanner { hpp::manipulation::RoadmapPtr_t roadmap = hpp::manipulation::Roadmap::create(problem.obj->distance(), problem.obj->robot()); - obj = hpp::manipulation::pathPlanner::StatesPathFinder::createWithRoadmap(problem.obj, roadmap); + obj = hpp::manipulation::pathPlanner::StatesPathFinder::createWithRoadmap( + problem.obj, roadmap); roadmap->constraintGraph( problem.asManipulationProblem()->constraintGraph()); } @@ -254,9 +255,8 @@ void exposePathPlanners() { boost::python::init()); boost::python::class_>( - "StatesPathFinder", - boost::python::init()); + boost::python::bases>( + "StatesPathFinder", boost::python::init()); boost::python::class_>( diff --git a/src/pyhpp/manipulation/security_margins.py b/src/pyhpp/manipulation/security_margins.py index 112d74e1..c05c1c1c 100644 --- a/src/pyhpp/manipulation/security_margins.py +++ b/src/pyhpp/manipulation/security_margins.py @@ -49,7 +49,8 @@ def computeJoints(self): for ro in self.robotsAndObjects: le = len(ro) self.robotToJoints[ro] = [ - n for n in jointNames + n + for n in jointNames if n[:le] == ro and (len(n) == le or n[le] in self.separators) ] self.robotToJoints["universe"] = ["universe"] @@ -61,20 +62,22 @@ def computeJoints(self): def computeGrippers(self): self.gripperToRobot = dict() self.gripperToJoints = dict() - + for g in self.factory.grippers: for joint_name in self.robot.model().names: if g.startswith(joint_name): - self.gripperToRobot[g] = self.jointToRobot.get(joint_name, "unknown") + self.gripperToRobot[g] = self.jointToRobot.get( + joint_name, "unknown" + ) self.gripperToJoints[g] = [joint_name] break def computePossibleContacts(self): self.contactSurfaces = {k: [] for k in self.robotToJoints.keys()} self.possibleContacts = [ - (o1, o2) - for o1, l1 in self.contactSurfaces.items() - for o2, l2 in self.contactSurfaces.items() + (o1, o2) + for o1, l1 in self.contactSurfaces.items() + for o2, l2 in self.contactSurfaces.items() if o1 != o2 and len(l1) > 0 and len(l2) > 0 ] @@ -87,46 +90,52 @@ def getSecurityMarginBetween(self, obj1, obj2): def getActiveConstraintsAlongEdge(self, edge): factory = self.factory graph = factory.graph - + result = graph.getNodesConnectedByTransition(edge) from_name, to_name = result if isinstance(result, tuple) else (result, result) - + state_from = graph.getState(from_name) state_to = graph.getState(to_name) - + constraints_from = graph.getNumericalConstraintsForState(state_from) constraints_graph = graph.getNumericalConstraintsForGraph() constraints_to = graph.getNumericalConstraintsForState(state_to) - - all_constraint_objects = set(constraints_from + constraints_graph + constraints_to) - + + all_constraint_objects = set( + constraints_from + constraints_graph + constraints_to + ) + active_names = [] - if hasattr(factory, 'constraints') and hasattr(factory.constraints, 'available_constraints'): + if hasattr(factory, "constraints") and hasattr( + factory.constraints, "available_constraints" + ): constraint_dict = factory.constraints.available_constraints for name, constraint_obj in constraint_dict.items(): if constraint_obj in all_constraint_objects: active_names.append(name) - + res = {"place": [], "grasp": []} - + for c in active_names: for o in factory.objects: if c == "place_" + o: res["place"].append(o) for g in factory.grippers: - for o, handle_indices in zip(factory.objects, factory.handlesPerObjects): + for o, handle_indices in zip( + factory.objects, factory.handlesPerObjects + ): for h_idx in handle_indices: h = factory.handles[h_idx] if c == g + " grasps " + h: res["grasp"].append((g, o)) - + return res def apply(self): factory = self.factory graph = factory.graph edges = graph.getTransitions() - + for edge in edges: for i1, ro1 in enumerate([*self.robotsAndObjects, "universe"]): for i2, ro2 in enumerate([*self.robotsAndObjects, "universe"]): @@ -136,15 +145,17 @@ def apply(self): for j1 in self.robotToJoints[ro1]: for j2 in self.robotToJoints[ro2]: if j1 != j2: - graph.setSecurityMarginForTransition(edge, j1, j2, margin) - + graph.setSecurityMarginForTransition( + edge, j1, j2, margin + ) + constraints = self.getActiveConstraintsAlongEdge(edge) for g, ro1 in constraints["grasp"]: if g in self.gripperToJoints and ro1 in self.robotToJoints: for j1 in self.robotToJoints[ro1]: for j2 in self.gripperToJoints[g]: graph.setSecurityMarginForTransition(edge, j1, j2, 0) - + for o1 in constraints["place"]: for o2, o3 in self.possibleContacts: if o1 == o2: From ca57cd39343c54ff1ce1e0989a5b73eeace814ff Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 26 Nov 2025 16:31:09 +0100 Subject: [PATCH 048/109] [ConnectedComponent] Add equality comparison between connected components. --- src/pyhpp/core/connected-component.cc | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/core/connected-component.cc b/src/pyhpp/core/connected-component.cc index 35ee18ef..8bc8ee40 100644 --- a/src/pyhpp/core/connected-component.cc +++ b/src/pyhpp/core/connected-component.cc @@ -67,13 +67,18 @@ struct CCWrapper { } return to_python_list(res); } + // Test that raw pointers are the same. + static bool equality(ConnectedComponent& cc1, ConnectedComponent& cc2) { + return &cc1 == &cc2; + } }; void exposeConnectedComponent() { class_( "ConnectedComponent", no_init) .def("nodes", &CCWrapper::nodes) .def("reachableFrom", &CCWrapper::reachableFrom) - .def("reachableTo", &CCWrapper::reachableTo); + .def("reachableTo", &CCWrapper::reachableTo) + .def("__eq__", &CCWrapper::equality); } } // namespace core } // namespace pyhpp From 9dc4a5de146061507a9603f5d7401c29ac699671 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 26 Nov 2025 16:32:07 +0100 Subject: [PATCH 049/109] [Roadmap] Add accessor to connected component of a node. --- src/pyhpp/core/roadmap.cc | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index c3a40f34..3b6cafb1 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -59,6 +59,19 @@ struct RWrapper { roadmap.addNodeAndEdges(nodeFrom, to, path); } + static ConnectedComponentPtr_t connectedComponentOfNode(Roadmap& roadmap, + const ConfigurationIn_t q) { + value_type d; + NodePtr_t node = roadmap.nearestNode(q, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::connectedComponentOfNode: input configuration (" << q + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } + return node->connectedComponent(); + } + static void addNodeAndEdge(Roadmap& roadmap, const ConfigurationIn_t from, ConfigurationIn_t to, const PathPtr_t path) { value_type d; @@ -242,6 +255,7 @@ void exposeRoadmap() { .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, distance) .def("numberConnectedComponents", &RWrapper::numberConnectedComponents) .def("getConnectedComponent", &RWrapper::getConnectedComponent) + .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode) // .def("cost", &RWrapper::cost1) // .def("cost", &RWrapper::cost2, // return_value_policy()) From 5a2b73250ccd73fc600db540940558d457dd91a7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 26 Nov 2025 15:43:03 +0000 Subject: [PATCH 050/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/roadmap.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index 3b6cafb1..e7036c04 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -59,8 +59,8 @@ struct RWrapper { roadmap.addNodeAndEdges(nodeFrom, to, path); } - static ConnectedComponentPtr_t connectedComponentOfNode(Roadmap& roadmap, - const ConfigurationIn_t q) { + static ConnectedComponentPtr_t connectedComponentOfNode( + Roadmap& roadmap, const ConfigurationIn_t q) { value_type d; NodePtr_t node = roadmap.nearestNode(q, d); if (d > 0) { From 16b5e278fd81aedbc8b299ef3902ef151ff03bbb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 1 Dec 2025 17:19:51 +0000 Subject: [PATCH 051/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.6 → v0.14.7](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.6...v0.14.7) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 927cf9df..9040b44d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.6 + rev: v0.14.7 hooks: - id: ruff args: From b03770ac95ea3ecb825dbad445b586e6a7fdd732 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 8 Dec 2025 17:15:52 +0000 Subject: [PATCH 052/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.7 → v0.14.8](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.7...v0.14.8) - [github.com/pre-commit/mirrors-clang-format: v21.1.6 → v21.1.7](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.6...v21.1.7) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9040b44d..3ba100f0 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.7 + rev: v0.14.8 hooks: - id: ruff args: @@ -18,7 +18,7 @@ repos: hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.6 + rev: v21.1.7 hooks: - id: clang-format args: From 3a5839b341ffea5e14379e0d1823a7f008f91e25 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" Date: Sat, 13 Dec 2025 16:21:00 +0000 Subject: [PATCH 053/109] flake.lock: Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Flake lock file updates: • Updated input 'gepetto': 'github:gepetto/nix/6dccd75' (2025-11-10) → 'github:gepetto/nix/a6b0691' (2025-12-13) • Updated input 'gepetto/flake-parts': 'github:hercules-ci/flake-parts/26d0589' (2025-11-06) → 'github:hercules-ci/flake-parts/2cccadc' (2025-11-21) • Updated input 'gepetto/nix-ros-overlay': 'github:lopsided98/nix-ros-overlay/1622b7a' (2025-11-08) → 'github:lopsided98/nix-ros-overlay/804d2ee' (2025-12-02) • Updated input 'gepetto/nix-system-graphics': 'github:soupglasses/nix-system-graphics/9c875e0' (2025-01-21) → 'github:soupglasses/nix-system-graphics/ac37f0f' (2025-11-16) • Updated input 'gepetto/src-agimus-controller': 'github:agimus-project/agimus_controller/09a3086' (2025-11-06) → 'github:agimus-project/agimus_controller/95aac68' (2025-12-04) • Updated input 'gepetto/src-agimus-msgs': 'github:agimus-project/agimus_msgs/e8e48c8' (2025-10-09) → 'github:agimus-project/agimus_msgs/2861569' (2025-11-24) • Updated input 'gepetto/src-franka-description': 'github:agimus-project/franka_description/fa4b70f' (2025-11-04) → 'github:agimus-project/franka_description/3ae2451' (2025-11-28) • Updated input 'gepetto/system-manager': 'github:numtide/system-manager/26fb1d5' (2025-10-30) → 'github:numtide/system-manager/a0343ab' (2025-11-27) • Updated input 'gepetto/treefmt-nix': 'github:numtide/treefmt-nix/97a3086' (2025-11-06) → 'github:numtide/treefmt-nix/5b4ee75' (2025-11-12) --- flake.lock | 54 +++++++++++++++++++++++++++--------------------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/flake.lock b/flake.lock index 5a3b06ca..990049a1 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1762440070, - "narHash": "sha256-xxdepIcb39UJ94+YydGP221rjnpkDZUlykKuF54PsqI=", + "lastModified": 1763759067, + "narHash": "sha256-LlLt2Jo/gMNYAwOgdRQBrsRoOz7BPRkzvNaI/fzXi2Q=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "26d05891e14c88eb4a5d5bee659c0db5afb609d8", + "rev": "2cccadc7357c0ba201788ae99c4dfa90728ef5e0", "type": "github" }, "original": { @@ -61,11 +61,11 @@ "treefmt-nix": "treefmt-nix" }, "locked": { - "lastModified": 1762792495, - "narHash": "sha256-rhs3zT1eavw6kjJ3wZSY+FR4jikS1NU6MFWBYNfpQUU=", + "lastModified": 1765613884, + "narHash": "sha256-/kuN0F5qciGkWMNlroG8ecq91DEDNy75gYe0lTueqRc=", "owner": "gepetto", "repo": "nix", - "rev": "6dccd759d955686bdf6e63006993219478de90df", + "rev": "a6b0691748591a6ae5e07dbbc5abd94ab97cdfa1", "type": "github" }, "original": { @@ -80,11 +80,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1762593451, - "narHash": "sha256-Mmo6HuDXoYIt1Qez5ytc5ejdhUSoej9SqW5hhvNmEeg=", + "lastModified": 1764665924, + "narHash": "sha256-ncGeWPiK6BawJdDZc0ugfeG2yPIGAYsdOFwpXBZ193k=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "1622b7a0adbd01006800210bc7b298fd430a2d34", + "rev": "804d2ee3fd89898fb7b1a02952aa5682e9725ce4", "type": "github" }, "original": { @@ -102,11 +102,11 @@ ] }, "locked": { - "lastModified": 1737457219, - "narHash": "sha256-nX9dxoATDCSQgWw/iv6BngXDJEyHVYYEvHEVQ7Ig3fI=", + "lastModified": 1763296639, + "narHash": "sha256-K9JBscC7ApwCnl0wR0sVkxrKFsoDYVqXN5fOujvyBWA=", "owner": "soupglasses", "repo": "nix-system-graphics", - "rev": "9c875e0c56cf2eb272b9102a4f3e24e4e31629fd", + "rev": "ac37f0f3ec0cb15d63a520918433c794d01d9dac", "type": "github" }, "original": { @@ -174,11 +174,11 @@ "src-agimus-controller": { "flake": false, "locked": { - "lastModified": 1762444174, - "narHash": "sha256-K05RaiRLm+EbCJKPw36c9YwSyhOTOvtDUx1I/HoBx6Q=", + "lastModified": 1764844076, + "narHash": "sha256-GFsuPVDy3TBwTRkDCMn+VcTYN7JQ5zlpSFfxUgAmnkc=", "owner": "agimus-project", "repo": "agimus_controller", - "rev": "09a30869d70c6c2689a2700c83eb73b6541d96f2", + "rev": "95aac68307eab0ec1a85b255b33e79868d53914c", "type": "github" }, "original": { @@ -190,11 +190,11 @@ "src-agimus-msgs": { "flake": false, "locked": { - "lastModified": 1759994370, - "narHash": "sha256-QuYtUR7VTOOtRBrYCxDjSLUP77wh0NlHbMtxZ1nSJFM=", + "lastModified": 1764011280, + "narHash": "sha256-wG0kJUMsDi3IELv5H9mEsizRyssMnvEbTM6LdEqqLvQ=", "owner": "agimus-project", "repo": "agimus_msgs", - "rev": "e8e48c8c7b942cc2d2feba01f5e2d319c7915816", + "rev": "2861569b8f8ef82a932c581a7dbfdb652dc6247b", "type": "github" }, "original": { @@ -206,11 +206,11 @@ "src-franka-description": { "flake": false, "locked": { - "lastModified": 1762271824, - "narHash": "sha256-JySeYjg77n6SHstTqyy6PaYBzQcA38vkM6gjTs4Iwt0=", + "lastModified": 1764344366, + "narHash": "sha256-uk0fh/BoMdhNyS3ssV8Ebn+FYMSXjaKGepTxp6zibt8=", "owner": "agimus-project", "repo": "franka_description", - "rev": "fa4b70f76908c30d08fc9ae4bb77c45d67109311", + "rev": "3ae24519c5ec3a999b0851a4243181ad245a6fa6", "type": "github" }, "original": { @@ -261,11 +261,11 @@ ] }, "locked": { - "lastModified": 1761809512, - "narHash": "sha256-eax/ncPr9rw0qkcb7JKTyL9Un8BzsEe3Pnor8kG9AcQ=", + "lastModified": 1764233885, + "narHash": "sha256-m6etiBLSMajunnbUvEI1Pc/jNu4naImalgQnctVJ1/k=", "owner": "numtide", "repo": "system-manager", - "rev": "26fb1d50067e63c39be557e4fcedbd1aeec64c2e", + "rev": "a0343ab10763fbbfcf8d3b69c8341c18b1b1f215", "type": "github" }, "original": { @@ -297,11 +297,11 @@ ] }, "locked": { - "lastModified": 1762410071, - "narHash": "sha256-aF5fvoZeoXNPxT0bejFUBXeUjXfHLSL7g+mjR/p5TEg=", + "lastModified": 1762938485, + "narHash": "sha256-AlEObg0syDl+Spi4LsZIBrjw+snSVU4T8MOeuZJUJjM=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "97a30861b13c3731a84e09405414398fbf3e109f", + "rev": "5b4ee75aeefd1e2d5a1cc43cf6ba65eba75e83e4", "type": "github" }, "original": { From f193493ff663e013303a537b62710b6d03d31ad9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 15 Dec 2025 17:14:56 +0000 Subject: [PATCH 054/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.8 → v0.14.9](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.8...v0.14.9) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 3ba100f0..d62e4ae4 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.8 + rev: v0.14.9 hooks: - id: ruff args: From ceceacb94704c01cb36adc250fdd0dfa33fad8f3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 22 Dec 2025 17:09:51 +0000 Subject: [PATCH 055/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.9 → v0.14.10](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.9...v0.14.10) - [github.com/pre-commit/mirrors-clang-format: v21.1.7 → v21.1.8](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.7...v21.1.8) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d62e4ae4..5ad1650b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.9 + rev: v0.14.10 hooks: - id: ruff args: @@ -18,7 +18,7 @@ repos: hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.7 + rev: v21.1.8 hooks: - id: clang-format args: From 0723ca03695662ebf34e7f5a1fd707ca101820aa Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 14:03:02 +0100 Subject: [PATCH 056/109] fix gripper-to-joint mapping in SecurityMargins and contact surfaces --- src/pyhpp/manipulation/security_margins.py | 99 +++++++++++++++++----- 1 file changed, 77 insertions(+), 22 deletions(-) diff --git a/src/pyhpp/manipulation/security_margins.py b/src/pyhpp/manipulation/security_margins.py index c05c1c1c..8fcc22e9 100644 --- a/src/pyhpp/manipulation/security_margins.py +++ b/src/pyhpp/manipulation/security_margins.py @@ -45,47 +45,101 @@ def __init__(self, problem, factory, robotsAndObjects, robot): def computeJoints(self): self.robotToJoints = dict() - jointNames = self.robot.model().names + model = self.robot.model() + jointNames = list(model.names) for ro in self.robotsAndObjects: le = len(ro) - self.robotToJoints[ro] = [ - n - for n in jointNames - if n[:le] == ro and (len(n) == le or n[le] in self.separators) - ] + self.robotToJoints[ro] = list( + filter( + lambda n: n[:le] == ro and n[le] in self.separators, + jointNames, + ) + ) self.robotToJoints["universe"] = ["universe"] self.jointToRobot = dict() for ro, joints in self.robotToJoints.items(): for j in joints: self.jointToRobot[j] = ro + def _getChildJoints(self, jointName): + model = self.robot.model() + try: + jointId = model.getJointId(jointName) + except Exception: + return [] + + childNames = [] + if jointId < len(model.children): + for childId in model.children[jointId]: + if childId < len(model.names): + childNames.append(model.names[childId]) + return childNames + + def _getGripperJoint(self, gripperName): + model = self.robot.model() + if model.existFrame(gripperName): + frameId = model.getFrameId(gripperName) + frame = model.frames[frameId] + parentJointId = frame.parentJoint + if parentJointId < len(model.names): + return model.names[parentJointId] + return None + def computeGrippers(self): self.gripperToRobot = dict() self.gripperToJoints = dict() - for g in self.factory.grippers: - for joint_name in self.robot.model().names: - if g.startswith(joint_name): - self.gripperToRobot[g] = self.jointToRobot.get( - joint_name, "unknown" - ) - self.gripperToJoints[g] = [joint_name] - break + j = self._getGripperJoint(g) + if j is not None and j in self.jointToRobot: + self.gripperToRobot[g] = self.jointToRobot[j] + childJoints = self._getChildJoints(j) + self.gripperToJoints[g] = [j] + childJoints + else: + for joint_name in self.robot.model().names: + if g.startswith(joint_name) and joint_name in self.jointToRobot: + self.gripperToRobot[g] = self.jointToRobot[joint_name] + childJoints = self._getChildJoints(joint_name) + self.gripperToJoints[g] = [joint_name] + childJoints + break def computePossibleContacts(self): - self.contactSurfaces = {k: [] for k in self.robotToJoints.keys()} - self.possibleContacts = [ - (o1, o2) - for o1, l1 in self.contactSurfaces.items() - for o2, l2 in self.contactSurfaces.items() - if o1 != o2 and len(l1) > 0 and len(l2) > 0 - ] + self.contactSurfaces = dict() + for k in self.robotToJoints.keys(): + self.contactSurfaces[k] = list() + + for io, obj_name in enumerate(self.factory.objects): + if io < len(self.factory.contactsPerObjects): + for surface_name in self.factory.contactsPerObjects[io]: + if obj_name in self.contactSurfaces: + self.contactSurfaces[obj_name].append(surface_name) + + for surface_name in self.factory.envContacts: + found = False + for ro in list(self.robotsAndObjects) + ["universe"]: + for sep in self.separators: + prefix = ro + sep + if surface_name.startswith(prefix): + if ro in self.contactSurfaces: + self.contactSurfaces[ro].append(surface_name) + found = True + break + if found: + break + if not found: + self.contactSurfaces["universe"].append(surface_name) + + self.possibleContacts = list() + for o1, l1 in self.contactSurfaces.items(): + for o2, l2 in self.contactSurfaces.items(): + if o1 != o2 and len(l1) > 0 and len(l2) > 0: + self.possibleContacts.append((o1, o2)) def setSecurityMarginBetween(self, obj1, obj2, margin): self.marginMatrix[frozenset([obj1, obj2])] = margin def getSecurityMarginBetween(self, obj1, obj2): - return self.marginMatrix.get(frozenset([obj1, obj2]), self.defaultMargin) + key = frozenset([obj1, obj2]) + return self.marginMatrix.get(key, self.defaultMargin) def getActiveConstraintsAlongEdge(self, edge): factory = self.factory @@ -150,6 +204,7 @@ def apply(self): ) constraints = self.getActiveConstraintsAlongEdge(edge) + for g, ro1 in constraints["grasp"]: if g in self.gripperToJoints and ro1 in self.robotToJoints: for j1 in self.robotToJoints[ro1]: From 2a8099564c944ffe9068e0bce465b884fe22acbb Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 24 Nov 2025 10:25:03 +0100 Subject: [PATCH 057/109] Update pyhpp benchmarks for testing --- .../baxter-manipulation-boxes-spf.py | 251 +++++++++++ tests/benchmarks/baxter-manipulation-boxes.py | 248 +++++++++++ tests/benchmarks/construction-set-m-rrt.py | 417 ++++++++++++++++++ tests/benchmarks/pr2-in-iai-kitchen.py | 88 ++++ .../pr2-manipulation-kitchen-spf.py | 162 +++++++ tests/benchmarks/pr2-manipulation-kitchen.py | 158 +++++++ .../pr2-manipulation-two-hand-spf.py | 184 ++++++++ tests/benchmarks/pr2-manipulation-two-hand.py | 181 ++++++++ tests/benchmarks/pyrene-on-the-ground.py | 2 +- tests/benchmarks/romeo-placard-spf.py | 226 ++++++++++ tests/benchmarks/romeo-placard.py | 221 ++++++++++ tests/benchmarks/ur3-spheres-spf.py | 4 +- tests/benchmarks/ur3-spheres.py | 3 +- 13 files changed, 2139 insertions(+), 6 deletions(-) create mode 100644 tests/benchmarks/baxter-manipulation-boxes-spf.py create mode 100644 tests/benchmarks/baxter-manipulation-boxes.py create mode 100644 tests/benchmarks/construction-set-m-rrt.py create mode 100644 tests/benchmarks/pr2-in-iai-kitchen.py create mode 100644 tests/benchmarks/pr2-manipulation-kitchen-spf.py create mode 100644 tests/benchmarks/pr2-manipulation-kitchen.py create mode 100644 tests/benchmarks/pr2-manipulation-two-hand-spf.py create mode 100644 tests/benchmarks/pr2-manipulation-two-hand.py create mode 100644 tests/benchmarks/romeo-placard-spf.py create mode 100644 tests/benchmarks/romeo-placard.py diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py new file mode 100644 index 00000000..c4c648fb --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -0,0 +1,251 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder + +from pyhpp.manipulation import ( + RandomShortcut as ManipRandomShortcut, + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +from pyhpp.constraints import ( + LockedJoint, +) +from pinocchio import SE3, Quaternion + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +baxter_urdf = "package://example-robot-data/robots/baxter_description/urdf/baxter.urdf" +baxter_srdf = "package://example-robot-data/robots/baxter_description/srdf/baxter_manipulation.srdf" +table_urdf = "package://hpp_tutorial/urdf/table.urdf" +table_srdf = "package://hpp_tutorial/srdf/table.srdf" +box_urdf = "package://hpp_environments/urdf/baxter_benchmark/box.urdf" +box_srdf = "package://hpp_environments/srdf/baxter_benchmark/box.srdf" + +# nbBoxes +K = 2 +nBoxPerLine = 2 +grippers = ["baxter/r_gripper", "baxter/l_gripper"] +# Box i will be at box goal[i] place at the end +goal = [1, 0] + +robot = Device("baxter-manip") + +# Load Baxter robot +baxter_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0.926])) +urdf.loadModel(robot, 0, "baxter", "anchor", baxter_urdf, baxter_srdf, baxter_pose) + +# Load table +table_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "table", "anchor", table_urdf, table_srdf, table_pose) + +# Load boxes +boxes = list() +for i in range(K): + boxes.append("box" + str(i)) + box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + boxes[i], + "freeflyer", + box_urdf, + box_srdf, + box_pose, + ) + robot.setJointBounds( + boxes[i] + '/root_joint', + [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + ) + +model = robot.model() + +problem = Problem(robot) +cg = Graph("graph", robot, problem) + +# Set error threshold and max iterations +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +q_init = robot.currentConfiguration() + +# Calculate box positions +rankB = list() +for i in range(K): + joint_id = robot.model().getJointId(boxes[i] + '/root_joint') + rankB.append (robot.model().idx_qs[joint_id]) + +bb = [0.7, 0.8, 0., 0.1] +c = sqrt(2) / 2 +xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) +nbCols = int(K * 1. / nBoxPerLine + 0.5) +ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) +for i in range(K): + iL = i % nBoxPerLine + iC = (i - iL) / nBoxPerLine + x = bb[0] + xstep * iL + y = bb[2] + xstep * iC + q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + +q_goal = q_init[::].copy() +for i in range(K): + r = rankB[i] + rn = rankB[goal[i]] + q_goal[r:r+7] = q_init[rn:rn+7] + +constraints = dict() +graphConstraints = dict() + +jointNames = dict() +jointNames['all'] = robot.model().names +jointNames['baxterRightSide'] = list() +jointNames['baxterLeftSide'] = list() + +for n in jointNames['all']: + if n.startswith("baxter"): + if n.startswith("baxter/left_"): + jointNames['baxterLeftSide'].append(n) + if n.startswith("baxter/right_"): + jointNames['baxterRightSide'].append(n) +# Lock finger joints +lockFingers = ["r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", + ] +for side in ["r", "l"]: + joint_name = "baxter/" + side + "_gripper_r_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([-0.02])) + constraints[side + "_gripper_r_finger"] = cs + graphConstraints[side + "_gripper_r_finger"] = cs + joint_name = "baxter/" + side + "_gripper_l_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([0.02])) + constraints[side + "_gripper_l_finger"] = cs + graphConstraints[side + "_gripper_l_finger"] = cs + + +# Lock head +lockHead = 'head_pan' +joint_name = 'baxter/head_pan' +joint_id = robot.model().getJointId(joint_name) +cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) +constraints[lockHead] = cs +graphConstraints[lockHead] = cs +for n in jointNames["baxterRightSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +# Define handles and contact surfaces +handlesPerObject = list() +handles = list() +objContactSurfaces = list() +for i in range(K): + handlesPerObject.append([boxes[i] + "/handle2"]) + handles.append(boxes[i] + "/handle2") + objContactSurfaces.append([boxes[i] + "/box_surface"]) + +rules = [Rule([".*"], [".*"], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(['table/pancake_table_table_top']) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() +cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise Exception('Init configuration could not be projected.') + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise Exception('Goal configuration could not be projected.') + +print(q_init) +problem.initConfig(q_init_proj) +print(q_init_proj) +print(cg.displayStateConstraints(cg.getState('free'))) + +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +optimizers = { + 'GraphPartialShortcut': GraphPartialShortcut(problem), + 'GraphRandomShortcut': GraphRandomShortcut(problem), + 'PartialShortcut': PartialShortcut(problem), + 'RandomShortcut': RandomShortcut(problem), + 'SimpleShortcut': SimpleShortcut(problem), +} +optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', + 'RandomShortcut', 'SimpleShortcut'] +iOpt = 0 + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + currentOptimizer = optimizers[optimizerNames[iOpt]] + iOpt += 1 + if iOpt == len(optimizerNames): + iOpt = 0 + + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + path = planner.solve() + optimized_path = currentOptimizer.optimize(path) + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/baxter-manipulation-boxes.py b/tests/benchmarks/baxter-manipulation-boxes.py new file mode 100644 index 00000000..c146ce4e --- /dev/null +++ b/tests/benchmarks/baxter-manipulation-boxes.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.core import Dichotomy +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner + +from pyhpp.manipulation import ( + RandomShortcut as ManipRandomShortcut, + EnforceTransitionSemantic, + GraphRandomShortcut, + GraphPartialShortcut, +) +from pyhpp.core import ( + RandomShortcut, + PartialShortcut, + SimpleShortcut, +) + +from pyhpp.constraints import ( + LockedJoint, +) +from pinocchio import SE3, Quaternion + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +baxter_urdf = "package://example-robot-data/robots/baxter_description/urdf/baxter.urdf" +baxter_srdf = "package://example-robot-data/robots/baxter_description/srdf/baxter_manipulation.srdf" +table_urdf = "package://hpp_tutorial/urdf/table.urdf" +table_srdf = "package://hpp_tutorial/srdf/table.srdf" +box_urdf = "package://hpp_environments/urdf/baxter_benchmark/box.urdf" +box_srdf = "package://hpp_environments/srdf/baxter_benchmark/box.srdf" + +# nbBoxes +K = 2 +nBoxPerLine = 2 +grippers = ["baxter/r_gripper", "baxter/l_gripper"] +# Box i will be at box goal[i] place at the end +goal = [1, 0] + +robot = Device("baxter-manip") + +# Load Baxter robot +baxter_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0.926])) +urdf.loadModel(robot, 0, "baxter", "anchor", baxter_urdf, baxter_srdf, baxter_pose) + +# Load table +table_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "table", "anchor", table_urdf, table_srdf, table_pose) + +# Load boxes +boxes = list() +for i in range(K): + boxes.append("box" + str(i)) + box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + boxes[i], + "freeflyer", + box_urdf, + box_srdf, + box_pose, + ) + robot.setJointBounds( + boxes[i] + '/root_joint', + [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + ) + +tmp = boxes[0] +boxes[0] = boxes[1] +boxes[1] = tmp + +model = robot.model() + +problem = Problem(robot) +cg = Graph("graph", robot, problem) + +# Set error threshold and max iterations +cg.errorThreshold(1e-3) +cg.maxIterations(40) +problem.pathValidation = Dichotomy(robot, 0.0) + +q_init = robot.currentConfiguration() + +# Calculate box positions +rankB = list() +for i in range(K): + joint_id = robot.model().getJointId(boxes[i] + '/root_joint') + rankB.append (robot.model().idx_qs[joint_id]) + +bb = [0.7, 0.8, 0., 0.1] +c = sqrt(2) / 2 +xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) +nbCols = int(K * 1. / nBoxPerLine + 0.5) +ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) +for i in range(K): + iL = i % nBoxPerLine + iC = (i - iL) / nBoxPerLine + x = bb[0] + xstep * iL + y = bb[2] + xstep * iC + q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + +q_goal = q_init[::].copy() +for i in range(K): + r = rankB[i] + rn = rankB[goal[i]] + q_goal[r:r+7] = q_init[rn:rn+7] + +constraints = dict() +graphConstraints = dict() + +jointNames = dict() +jointNames['all'] = robot.model().names +jointNames['baxterRightSide'] = list() +jointNames['baxterLeftSide'] = list() + +for n in jointNames['all']: + if n.startswith("baxter"): + if n.startswith("baxter/left_"): + jointNames['baxterLeftSide'].append(n) + if n.startswith("baxter/right_"): + jointNames['baxterRightSide'].append(n) +# Lock finger joints +lockFingers = ["r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", + ] +for side in ["r", "l"]: + joint_name = "baxter/" + side + "_gripper_r_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([-0.02])) + constraints[side + "_gripper_r_finger"] = cs + graphConstraints[side + "_gripper_r_finger"] = cs + joint_name = "baxter/" + side + "_gripper_l_finger_joint" + cs = LockedJoint(robot, joint_name, np.array([0.02])) + constraints[side + "_gripper_l_finger"] = cs + graphConstraints[side + "_gripper_l_finger"] = cs + + +# Lock head +lockHead = 'head_pan' +joint_name = 'baxter/head_pan' +joint_id = robot.model().getJointId(joint_name) +cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) +constraints[lockHead] = cs +graphConstraints[lockHead] = cs +for n in jointNames["baxterRightSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +for n in jointNames["baxterLeftSide"]: + cs = LockedJoint(robot, n, np.array([0.])) + constraints[n] = cs + +# Define handles and contact surfaces +handlesPerObject = list() +handles = list() +objContactSurfaces = list() +for i in range(K): + handlesPerObject.append([boxes[i] + "/handle2"]) + handles.append(boxes[i] + "/handle2") + objContactSurfaces.append([boxes[i] + "/box_surface"]) + +rules = [Rule([".*"], [".*"], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(['table/pancake_table_table_top']) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() +cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise Exception('Init configuration could not be projected.') + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise Exception('Goal configuration could not be projected.') + +problem.initConfig(q_init_proj) + +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = ManipulationPlanner(problem) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") +optimizers = { + 'GraphPartialShortcut': GraphPartialShortcut(problem), + 'GraphRandomShortcut': GraphRandomShortcut(problem), + 'PartialShortcut': PartialShortcut(problem), + 'RandomShortcut': RandomShortcut(problem), + 'SimpleShortcut': SimpleShortcut(problem), +} +optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', + 'RandomShortcut', 'SimpleShortcut'] +iOpt = 0 + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + currentOptimizer = optimizers[optimizerNames[iOpt]] + iOpt += 1 + if iOpt == len(optimizerNames): + iOpt = 0 + + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + path = planner.solve() + optimized_path = currentOptimizer.optimize(path) + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/construction-set-m-rrt.py b/tests/benchmarks/construction-set-m-rrt.py new file mode 100644 index 00000000..bf23ba81 --- /dev/null +++ b/tests/benchmarks/construction-set-m-rrt.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import pi, sqrt +import numpy as np +import datetime as dt +import re +import typing as T + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Straight, Progressive, ProgressiveProjector +from pyhpp.constraints import ( + Transformation, + ComparisonTypes, + ComparisonType, + Implicit, + LockedJoint, +) +from pinocchio import SE3, Quaternion + +from pyhpp.manipulation.security_margins import SecurityMargins + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +parser.add_argument('--bigGraph', action='store_true', + help="Whether constraint graph is generated with all the possible states. " + "If unspecified, constraint graph only has a few states traversed by " + "one safe path.") +args = parser.parse_args() + + +class StateName(object): + noGrasp = 'free' + + def __init__(self, grasps): + if isinstance(grasps, set): + self.grasps = grasps.copy() + elif isinstance(grasps, str): + if grasps == self.noGrasp: + self.grasps = set() + else: + g1 = [s.strip(' ') for s in grasps.split(':')] + self.grasps = set([tuple(s.split(' grasps ')) for s in g1]) + else: + raise TypeError('expecting a set of pairs (gripper, handle) or a string') + + def __str__(self): + if self.grasps == set(): + return 'free' + res = "" + for g in self.grasps: + res += g[0] + " grasps " + g[1] + " : " + return res[:-3] + + def __eq__(self, other): + return self.grasps == other.grasps + + def __ne__(self, other): + return not self.__eq__(other) + + +ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" +ur3_srdf = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" +cylinder_08_urdf = "package://hpp_environments/urdf/construction_set/cylinder_08.urdf" +cylinder_08_srdf = "package://hpp_environments/srdf/construction_set/cylinder_08.srdf" +sphere_urdf = "package://hpp_environments/urdf/construction_set/sphere.urdf" +sphere_srdf = "package://hpp_environments/srdf/construction_set/sphere.srdf" +ground_urdf = "package://hpp_environments/urdf/construction_set/ground.urdf" +ground_srdf = "package://hpp_environments/srdf/construction_set/ground.srdf" + +nSphere = 2 +nCylinder = 2 + +robot = Device('2ur5-sphere') + +r0_pose = SE3(rotation=np.identity(3), translation=np.array([-.25, 0, 0])) +urdf.loadModel(robot, 0, "r0", "anchor", ur3_urdf, ur3_srdf, r0_pose) + +r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([.25, 0, 0])) +urdf.loadModel(robot, 0, "r1", "anchor", ur3_urdf, ur3_srdf, r1_pose) + +robot.setJointBounds('r0/shoulder_pan_joint', [-pi, 4]) +robot.setJointBounds('r1/shoulder_pan_joint', [-pi, 4]) +robot.setJointBounds('r0/shoulder_lift_joint', [-pi, 0]) +robot.setJointBounds('r1/shoulder_lift_joint', [-pi, 0]) +robot.setJointBounds('r0/elbow_joint', [-2.6, 2.6]) +robot.setJointBounds('r1/elbow_joint', [-2.6, 2.6]) + +ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "ground", "anchor", ground_urdf, ground_srdf, ground_pose) + +objects = list() +for i in range(nSphere): + sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, f'sphere{i}', "freeflyer", sphere_urdf, sphere_srdf, sphere_pose) + robot.setJointBounds( + f'sphere{i}/root_joint', + [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, + -1.0001, 1.0001, -1.0001, 1.0001] + ) + objects.append(f'sphere{i}') + +for i in range(nCylinder): + cylinder_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, f'cylinder{i}', "freeflyer", cylinder_08_urdf, cylinder_08_srdf, cylinder_pose) + robot.setJointBounds( + f'cylinder{i}/root_joint', + [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, + -1.0001, 1.0001, -1.0001, 1.0001] + ) + objects.append(f'cylinder{i}') + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("assembly", robot, problem) +cg.errorThreshold(1e-4) +cg.maxIterations(40) + +constraints = dict() + +for i in range(nSphere): + placementName = f"place_sphere{i}" + Id = SE3.Identity() + spherePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"sphere{i}/root_joint") + + pc = Transformation( + placementName, robot, joint, Id, spherePlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", robot, joint, Id, spherePlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) + constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, + ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"sphere{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_sphere{i}" + spherePrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, robot, joint, Id, spherePrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +for i in range(nCylinder): + placementName = f"place_cylinder{i}" + Id = SE3.Identity() + cylinderPlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"cylinder{i}/root_joint") + + pc = Transformation( + placementName, robot, joint, Id, cylinderPlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", robot, joint, Id, cylinderPlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) + constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, + ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"cylinder{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_cylinder{i}" + cylinderPrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, robot, joint, Id, cylinderPrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +grippers = [f'cylinder{i}/magnet0' for i in range(nCylinder)] +grippers += [f'cylinder{i}/magnet1' for i in range(nCylinder)] +grippers += [f"r{i}/gripper" for i in range(2)] + +handlesPerObjects = [[f'sphere{i}/handle', f'sphere{i}/magnet'] for i in range(nSphere)] +handlesPerObjects += [[f'cylinder{i}/handle'] for i in range(nCylinder)] + +shapesPerObject = [[] for o in objects] + +allHandles = [handle for objHandles in handlesPerObjects for handle in objHandles] + + +def makeRule(grasps): + _grippers = list() + _handles = list() + for (g, h) in grasps: + _grippers.append(g) + _handles.append(h) + for g in grippers: + if not g in _grippers: + _grippers.append(g) + _handles += (len(_grippers) - len(_handles)) * ['^$'] + return Rule(grippers=_grippers, handles=_handles, link=True) + + +def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: + gRegex = re.compile(g) + hRegex = [re.compile(handle) for handle in h] + forbiddenList: T.List[Rule] = list() + idForbidGrippers = [i for i in range(len(grippers)) if gRegex.match(grippers[i])] + forbidHandles = [handle for handle in allHandles + if not any([handlePattern.match(handle) for handlePattern in hRegex])] + for id in idForbidGrippers: + for handle in forbidHandles: + _handles = [handle if i == id else '.*' for i in range(len(grippers))] + forbiddenList.append(Rule(grippers=grippers, handles=_handles, link=False)) + return forbiddenList + + +q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0] +q0_r1 = q0_r0[::] + +q0_spheres = list() +i = 0 +y = 0.04 +while i < nSphere: + q0_spheres.append([-.1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0_cylinders = list() +i = 0 +y = -0.04 +while i < nCylinder: + q0_cylinders.append([0.45 + .1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0 = np.array(q0_r0 + q0_r1 + sum(q0_spheres, []) + sum(q0_cylinders, [])) +if not args.bigGraph: + nodes = list() + rules = list() + grasps = set() + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r0/gripper', 'sphere0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r1/gripper', 'cylinder0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('cylinder0/magnet0', 'sphere0/magnet')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r0/gripper', 'sphere0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('r0/gripper', 'sphere1/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(('cylinder0/magnet1', 'sphere1/magnet')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r0/gripper', 'sphere1/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(('r1/gripper', 'cylinder0/handle')) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", + "cylinder0", "cylinder1"], robot) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + nodes.append(nodes[-1]) + +else: + rules = list() + + rules.extend(forbidExcept('^r0/gripper$', ['^sphere\\d*/handle$'])) + rules.extend(forbidExcept('^r1/gripper$', ['^cylinder0*/handle$'])) + rules.extend(forbidExcept('^cylinder0/magnet\\d*$', ['^sphere\\d*/magnet$'])) + rules.extend(forbidExcept('^cylinder[1-9][0-9]*.*$', ['^$'])) + + rules.append(Rule(grippers=grippers, handles=[".*"] * len(grippers), link=True)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", + "cylinder0", "cylinder1"], robot) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + +assert (nCylinder == 2 and nSphere == 2) +c = sqrt(2) / 2 +q_goal = np.array(q0_r0 + q0_r1 + [-0.06202136144745322, -0.15, 0.025, c, 0, -c, 0, + 0.06202136144745322, -0.15, 0.025, c, 0, c, 0, + 0, -0.15, 0.025, 0, 0, 0, 1, + 0.5, -0.08, 0.025, 0, 0, 0, 1]) + +problem.initConfig(q0) +problem.addGoalConfig(q_goal) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q0) + problem.addGoalConfig(q_goal) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py new file mode 100644 index 00000000..29ffa922 --- /dev/null +++ b/tests/benchmarks/pr2-in-iai-kitchen.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import numpy as np +import datetime as dt + +from pyhpp.core import Problem, DiffusingPlanner, Dichotomy, Straight +from pyhpp.pinocchio import Device, urdf +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" + +robot = Device('pr2') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +robot.setJointBounds("pr2/root_joint", [-4, -3, -5, -3, -2, 2, -2, 2]) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen", "anchor", kitchen_urdf, "", kitchen_pose) + +model = robot.model() + +q_init = robot.currentConfiguration() +q_goal = q_init.copy() + +q_init[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId('pr2/torso_lift_joint')] +q_init[rank] = 0.2 + +q_goal[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId('pr2/l_shoulder_lift_joint')] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId('pr2/r_shoulder_lift_joint')] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId('pr2/l_elbow_flex_joint')] +q_goal[rank] = -0.5 +rank = model.idx_qs[model.getJointId('pr2/r_elbow_flex_joint')] +q_goal[rank] = -0.5 + +problem = Problem(robot) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.steeringMethod = Straight(problem) + +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) +planner = DiffusingPlanner(problem) +planner.maxIterations(5000) +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py new file mode 100644 index 00000000..51fab7d6 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt, pi +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +q_init[rank] = 0.5 + +q_goal = q_init.copy() +q_init[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] + +q_goal[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +grippers = ['pr2/l_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle']] +envSurfaces = ['kitchen_area/pancake_table_table_top', + 'kitchen_area/white_counter_top_sink'] +objContactSurfaces = [['box/box_surface']] + +rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), + Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) + +cg.setWeight(cg.getTransition('Loop | f'), 1) +cg.setWeight(cg.getTransition('Loop | 0-0'), 1) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(problem.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py new file mode 100644 index 00000000..d9be8726 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt, pi +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +q_init[rank] = 0.5 + +q_goal = q_init.copy() +q_init[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] + +q_goal[0:2] = [-3.2, -4] +rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +q_goal[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +q_goal[rank] = -0.5 +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +grippers = ['pr2/l_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle']] +envSurfaces = ['kitchen_area/pancake_table_table_top', + 'kitchen_area/white_counter_top_sink'] +objContactSurfaces = [['box/box_surface']] + +rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), + Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) + +cg.setWeight(cg.getTransition('Loop | f'), 1) +cg.setWeight(cg.getTransition('Loop | 0-0'), 1) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py new file mode 100644 index 00000000..d3713b50 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +q_init[0:4] = [-3.2, -4, 1, 0] + +rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 + +q_goal = q_init.copy() + +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +lockrhand = ['r_l_finger', 'r_r_finger'] +cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) +constraints['r_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) +constraints['r_r_finger'] = cs + +lockhands = lockrhand + locklhand + +lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] +cs = LockedJoint(robot, 'pr2/head_pan_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) +constraints['head_pan'] = cs +cs = LockedJoint(robot, 'pr2/head_tilt_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) +constraints['head_tilt'] = cs +cs = LockedJoint(robot, 'pr2/torso_lift_joint', + np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) +constraints['torso'] = cs +cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', + np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) +constraints['laser'] = cs + +lockPlanar = ['lockplanar'] +cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) +constraints['lockplanar'] = cs + +lockAll = lockhands + lockHeadAndTorso + lockPlanar + +grippers = ['pr2/l_gripper', 'pr2/r_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle', 'box/handle2']] +objContactSurfaces = [['box/box_surface']] +envSurfaces = ['kitchen_area/pancake_table_table_top'] + +rules = [Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in lockAll]) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.2 +) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py new file mode 100644 index 00000000..dcf14cc3 --- /dev/null +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -0,0 +1,181 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +from math import sqrt +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import LockedJoint +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +box_urdf = "package://hpp_tutorial/urdf/box.urdf" +box_srdf = "package://hpp_tutorial/srdf/box.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" + +robot = Device('pr2-box') + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +box_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) + +robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) +robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph('graph', robot, problem) +cg.errorThreshold(1e-3) +cg.maxIterations(40) + +constraints = dict() + +c = sqrt(2) / 2 + +q_init = robot.currentConfiguration() +q_init[0:4] = [-3.2, -4, 1, 0] + +rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +q_init[rank] = 0.5 +rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +q_init[rank] = 0.2 + +q_goal = q_init.copy() + +rank = robot.rankInConfiguration['box/root_joint'] +q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration['box/root_joint'] +q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] + +locklhand = ['l_l_finger', 'l_r_finger'] +cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) +constraints['l_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) +constraints['l_r_finger'] = cs + +lockrhand = ['r_l_finger', 'r_r_finger'] +cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) +constraints['r_l_finger'] = cs +cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) +constraints['r_r_finger'] = cs + +lockhands = lockrhand + locklhand + +lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] +cs = LockedJoint(robot, 'pr2/head_pan_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) +constraints['head_pan'] = cs +cs = LockedJoint(robot, 'pr2/head_tilt_joint', + np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) +constraints['head_tilt'] = cs +cs = LockedJoint(robot, 'pr2/torso_lift_joint', + np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) +constraints['torso'] = cs +cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', + np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) +constraints['laser'] = cs + +lockPlanar = ['lockplanar'] +rank = robot.rankInConfiguration['pr2/root_joint'] +cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) +constraints['lockplanar'] = cs + +lockAll = lockhands + lockHeadAndTorso + lockPlanar + +grippers = ['pr2/l_gripper', 'pr2/r_gripper'] +boxes = ['box'] +handlesPerObject = [['box/handle', 'box/handle2']] +objContactSurfaces = [['box/box_surface']] +envSurfaces = ['kitchen_area/pancake_table_table_top'] + +rules = [Rule([""], [""], True)] + +factory = ConstraintGraphFactory(cg, constraints) +factory.setGrippers(grippers) +factory.environmentContacts(envSurfaces) +factory.setObjects(boxes, handlesPerObject, objContactSurfaces) +factory.setRules(rules) +factory.generate() + +cg.addNumericalConstraintsToGraph([constraints[c] for c in lockAll]) + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.2 +) + +cg.initialize() + +res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +if not res: + raise RuntimeError("Failed to project initial configuration") + +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration") + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 + +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file diff --git a/tests/benchmarks/pyrene-on-the-ground.py b/tests/benchmarks/pyrene-on-the-ground.py index 50e56e6f..b02d6de4 100755 --- a/tests/benchmarks/pyrene-on-the-ground.py +++ b/tests/benchmarks/pyrene-on-the-ground.py @@ -19,7 +19,7 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) parser.add_argument("--display", action="store_true") parser.add_argument("--run", action="store_true") args = parser.parse_args() diff --git a/tests/benchmarks/romeo-placard-spf.py b/tests/benchmarks/romeo-placard-spf.py new file mode 100644 index 00000000..72a72627 --- /dev/null +++ b/tests/benchmarks/romeo-placard-spf.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import time +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" +romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +placard_urdf = "package://hpp_environments/urdf/placard.urdf" +placard_srdf = "package://hpp_environments/srdf/placard.srdf" + +robot = Device('romeo-placard') + +# Load Romeo robot +romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) + +robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) + +# Load placard +placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) + +robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) + +model = robot.model() + +problem = Problem(robot) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("graph", robot, problem) +cg.errorThreshold(2e-4) +cg.maxIterations(40) + +constraints = dict() +graphConstraints = dict() + +leftHandOpen = { + "LHand": 1, + "LFinger12": 1.06, + "LFinger13": 1.06, + "LFinger21": 1.06, + "LFinger22": 1.06, + "LFinger23": 1.06, + "LFinger31": 1.06, + "LFinger32": 1.06, + "LFinger33": 1.06, + "LThumb1": 0, + "LThumb2": 1.06, + "LThumb3": 1.06, +} + +rightHandOpen = { + "RHand": 1, + "RFinger12": 1.06, + "RFinger13": 1.06, + "RFinger21": 1.06, + "RFinger22": 1.06, + "RFinger23": 1.06, + "RFinger31": 1.06, + "RFinger32": 1.06, + "RFinger33": 1.06, + "RThumb1": 0, + "RThumb2": 1.06, + "RThumb3": 1.06, +} + + +# Lock left hand +locklhand = list() +for j, v in leftHandOpen.items(): + joint_name = 'romeo/' + j + locklhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +# Lock right hand +lockrhand = list() +for j, v in rightHandOpen.items(): + joint_name = 'romeo/' + j + lockrhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +lockHands = lockrhand + locklhand + +# Create static stability constraint +q = robot.currentConfiguration() +placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] +q[placard_rank:placard_rank+3] = [.4, 0, 1.2] + +problem.addPartialCom("romeo", ["romeo/root_joint"]) + +leftAnkle = 'romeo/LAnkleRoll' +rightAnkle = 'romeo/RAnkleRoll' + +factory = StaticStabilityConstraintsFactory(problem, robot) +balanceConstraintsDict = factory.createStaticStabilityConstraint( + "balance/", "romeo", leftAnkle, rightAnkle, q +) + +balanceConstraints = [ + balanceConstraintsDict.get('balance/pose-left-foot'), + balanceConstraintsDict.get('balance/pose-right-foot'), + balanceConstraintsDict.get('balance/relative-com'), +] +balanceConstraints = [c for c in balanceConstraints if c is not None] + +for name, constraint in balanceConstraintsDict.items(): + if constraint not in balanceConstraints: + constraints[name] = constraint + graphConstraints[name] = constraint + +# Build graph +grippers = ['romeo/r_hand', 'romeo/l_hand'] +handlesPerObjects = [['placard/low', 'placard/high']] + +rules = [ + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["", "placard/high"], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", "placard/high"], True), +] + +factory_cg = ConstraintGraphFactory(cg, constraints) +factory_cg.setGrippers(grippers) +factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setRules(rules) +factory_cg.generate() + +# Add balance constraints and locked hands to graph +all_graph_constraints = list(graphConstraints.values()) + balanceConstraints +cg.addNumericalConstraintsToGraph(all_graph_constraints) +cg.initialize() + +# Define initial and final configurations +q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_init = q_goal.copy() +q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] + +n = 'romeo/l_hand grasps placard/low' +state = cg.getState(n) +res, q_init_proj, err = cg.applyStateConstraints(state, q_init) +if not res: + raise RuntimeError("Failed to project initial configuration.") +res, q_goal_proj, err = cg.applyStateConstraints(state, q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration.") + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 +) + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +planner = StatesPathFinder(problem) +planner.maxIterations(5000) + +problem.setParameter("StatesPathFinder/innerPlannerTimeOut", 0.0) +problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) +problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) +# Run benchmark +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + planner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(planner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") + + diff --git a/tests/benchmarks/romeo-placard.py b/tests/benchmarks/romeo-placard.py new file mode 100644 index 00000000..8d7aed44 --- /dev/null +++ b/tests/benchmarks/romeo-placard.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python + +from argparse import ArgumentParser +import time +import numpy as np +import datetime as dt + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pinocchio import SE3 + +parser = ArgumentParser() +parser.add_argument('-N', default=0, type=int) +parser.add_argument('--display', action='store_true') +parser.add_argument('--run', action='store_true') +args = parser.parse_args() + +# Robot and object file paths +romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" +romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +placard_urdf = "package://hpp_environments/urdf/placard.urdf" +placard_srdf = "package://hpp_environments/srdf/placard.srdf" + +robot = Device('romeo-placard') + +# Load Romeo robot +romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) + +robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) + +# Load placard +placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) + +robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) + +model = robot.model() + +problem = Problem(robot) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("graph", robot, problem) +cg.errorThreshold(2e-4) +cg.maxIterations(40) + +constraints = dict() +graphConstraints = dict() + +leftHandOpen = { + "LHand": 1, + "LFinger12": 1.06, + "LFinger13": 1.06, + "LFinger21": 1.06, + "LFinger22": 1.06, + "LFinger23": 1.06, + "LFinger31": 1.06, + "LFinger32": 1.06, + "LFinger33": 1.06, + "LThumb1": 0, + "LThumb2": 1.06, + "LThumb3": 1.06, +} + +rightHandOpen = { + "RHand": 1, + "RFinger12": 1.06, + "RFinger13": 1.06, + "RFinger21": 1.06, + "RFinger22": 1.06, + "RFinger23": 1.06, + "RFinger31": 1.06, + "RFinger32": 1.06, + "RFinger33": 1.06, + "RThumb1": 0, + "RThumb2": 1.06, + "RThumb3": 1.06, +} + + +# Lock left hand +locklhand = list() +for j, v in leftHandOpen.items(): + joint_name = 'romeo/' + j + locklhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +# Lock right hand +lockrhand = list() +for j, v in rightHandOpen.items(): + joint_name = 'romeo/' + j + lockrhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +lockHands = lockrhand + locklhand + +# Create static stability constraint +q = robot.currentConfiguration() +placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] +q[placard_rank:placard_rank+3] = [.4, 0, 1.2] + +problem.addPartialCom("romeo", ["romeo/root_joint"]) + +leftAnkle = 'romeo/LAnkleRoll' +rightAnkle = 'romeo/RAnkleRoll' + +factory = StaticStabilityConstraintsFactory(problem, robot) +balanceConstraintsDict = factory.createStaticStabilityConstraint( + "balance/", "romeo", leftAnkle, rightAnkle, q +) + +balanceConstraints = [ + balanceConstraintsDict.get('balance/pose-left-foot'), + balanceConstraintsDict.get('balance/pose-right-foot'), + balanceConstraintsDict.get('balance/relative-com'), +] +balanceConstraints = [c for c in balanceConstraints if c is not None] + +for name, constraint in balanceConstraintsDict.items(): + if constraint not in balanceConstraints: + constraints[name] = constraint + graphConstraints[name] = constraint + +# Build graph +grippers = ['romeo/r_hand', 'romeo/l_hand'] +handlesPerObjects = [['placard/low', 'placard/high']] + +rules = [ + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["", "placard/high"], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", "placard/high"], True), +] + +factory_cg = ConstraintGraphFactory(cg, constraints) +factory_cg.setGrippers(grippers) +factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setRules(rules) +factory_cg.generate() + +# Add balance constraints and locked hands to graph +all_graph_constraints = list(graphConstraints.values()) + balanceConstraints +cg.addNumericalConstraintsToGraph(all_graph_constraints) +cg.initialize() + +# Define initial and final configurations +q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_init = q_goal.copy() +q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] + +n = 'romeo/l_hand grasps placard/low' +state = cg.getState(n) +res, q_init_proj, err = cg.applyStateConstraints(state, q_init) +if not res: + raise RuntimeError("Failed to project initial configuration.") +res, q_goal_proj, err = cg.applyStateConstraints(state, q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration.") + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 +) + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +# Run benchmark +totalTime = dt.timedelta(0) +totalNumberNodes = 0 +success = 0 +for i in range(args.N): + try: + manipulationPlanner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init_proj) + problem.addGoalConfig(q_goal_proj) + t1 = dt.datetime.now() + manipulationPlanner.solve() + t2 = dt.datetime.now() + except Exception as e: + print(f"Failed to plan path: {e}") + else: + success += 1 + totalTime += t2 - t1 + print(t2 - t1) + n = len(manipulationPlanner.roadmap().nodes()) + totalNumberNodes += n + print("Number nodes: " + str(n)) + +if args.N != 0: + print("#" * 20) + print(f"Number of rounds: {args.N}") + print(f"Number of successes: {success}") + print(f"Success rate: {success / args.N * 100}%") + if success > 0: + print(f"Average time per success: {totalTime.total_seconds() / success}") + print(f"Average number nodes per success: {totalNumberNodes / success}") + + diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/benchmarks/ur3-spheres-spf.py index 0586f645..07ea8f8f 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/benchmarks/ur3-spheres-spf.py @@ -31,7 +31,7 @@ from pinocchio import SE3, Quaternion parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) args = parser.parse_args() # Robot and environment file paths @@ -309,8 +309,6 @@ for i in range(args.N): try: planner.roadmap().clear() - # TODO: Verify if resetGoalConfigs equivalent is needed - # In old API: ps.resetGoalConfigs() t1 = dt.datetime.now() planner.solve() t2 = dt.datetime.now() diff --git a/tests/benchmarks/ur3-spheres.py b/tests/benchmarks/ur3-spheres.py index 3b2591fa..debb2704 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/benchmarks/ur3-spheres.py @@ -26,7 +26,7 @@ from argparse import ArgumentParser parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser.add_argument("-N", default=0, type=int) args = parser.parse_args() # Robot and environment file paths ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" @@ -289,7 +289,6 @@ t2 = dt.datetime.now() except Exception as e: print(f"Failed to plan path: {e}") - break else: success += 1 totalTime += t2 - t1 From e5825e8b7d1b8929253a5c21ba00ae082797b828 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:13:17 +0100 Subject: [PATCH 058/109] Fix test files referencing bad urdf file --- tests/benchmarks/baxter-manipulation-boxes-spf.py | 3 --- tests/benchmarks/pr2-manipulation-kitchen-spf.py | 4 ++-- tests/benchmarks/pr2-manipulation-kitchen.py | 2 +- tests/benchmarks/pr2-manipulation-two-hand-spf.py | 2 +- tests/benchmarks/pr2-manipulation-two-hand.py | 2 +- tests/graph_factory.py | 2 +- 6 files changed, 6 insertions(+), 9 deletions(-) diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py index c4c648fb..f635221b 100644 --- a/tests/benchmarks/baxter-manipulation-boxes-spf.py +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -183,10 +183,7 @@ if not res: raise Exception('Goal configuration could not be projected.') -print(q_init) problem.initConfig(q_init_proj) -print(q_init_proj) -print(cg.displayStateConstraints(cg.getState('free'))) problem.addGoalConfig(q_goal_proj) problem.constraintGraph(cg) diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py index 51fab7d6..f7a45314 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen-spf.py +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') @@ -148,7 +148,7 @@ success += 1 totalTime += t2 - t1 print(t2 - t1) - n = len(problem.roadmap().nodes()) + n = len(planner.roadmap().nodes()) totalNumberNodes += n print("Number nodes: " + str(n)) diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py index d9be8726..50685a85 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen.py +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py index d3713b50..85c2a857 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand-spf.py +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py index dcf14cc3..b4fad231 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand.py +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -21,7 +21,7 @@ pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device('pr2-box') diff --git a/tests/graph_factory.py b/tests/graph_factory.py index 215addba..c580b353 100644 --- a/tests/graph_factory.py +++ b/tests/graph_factory.py @@ -13,7 +13,7 @@ ) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" robot = Device("pr2-box") From f1bc304238e3cd0c6c734dbb026377f946975082 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:14:41 +0100 Subject: [PATCH 059/109] Add getGraphCapsule to allow external modules to extract cpp pointer --- src/pyhpp/manipulation/graph.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index a362afa5..b7ffc82c 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -1212,6 +1212,23 @@ ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, ImplicitPtr_t constraint = h->createPreGrasp(g, c, name); return constraint; } + +static void graphCapsuleDestructor(PyObject* capsule) { + auto* ptr = static_cast( + PyCapsule_GetPointer(capsule, "hpp.manipulation.graph.GraphPtr")); + if (ptr) { + delete ptr; + } +} + +/// Create a PyCapsule containing a copy of the GraphPtr_t shared_ptr +/// This allows external modules to extract the native C++ pointer +PyObject* getGraphCapsule(const PyWGraph& self) { + auto* ptr = new GraphPtr_t(self.obj); + return PyCapsule_New(ptr, "hpp.manipulation.graph.GraphPtr", + graphCapsuleDestructor); +} + // ============================================================================= // Boost.Python bindings // ============================================================================= @@ -1230,6 +1247,7 @@ void exposeGraph() { "Graph", init()) + .def("_get_native_graph", &getGraphCapsule) .def_readwrite("robot", &PyWGraph::robot) // Configuration methods .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, From 9e90e34f4f3fef209e3a1a20eac579b4843249f9 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 7 Jan 2026 09:27:53 +0100 Subject: [PATCH 060/109] Add file to test benchmarks examples easily --- tests/benchmarks/test_benchmarks.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 tests/benchmarks/test_benchmarks.py diff --git a/tests/benchmarks/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py new file mode 100644 index 00000000..582f7734 --- /dev/null +++ b/tests/benchmarks/test_benchmarks.py @@ -0,0 +1,16 @@ +#!/usr/bin/env python3 +import subprocess +import sys +from pathlib import Path + +def main(): + folder = Path(sys.argv[1]) if len(sys.argv) > 1 else Path(".") + + for script in sorted(folder.glob("*.py")): + if script.name == Path(__file__).name: + continue + print(f"\n{'='*60}\nRunning: {script.name}\n{'='*60}") + subprocess.run([sys.executable, str(script), "-N", "1"]) + +if __name__ == "__main__": + main() \ No newline at end of file From 453571afab6fa9210af40b9c880a64e0fca5c168 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 7 Jan 2026 08:33:17 +0000 Subject: [PATCH 061/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- .../baxter-manipulation-boxes-spf.py | 90 +++-- tests/benchmarks/baxter-manipulation-boxes.py | 89 +++-- tests/benchmarks/construction-set-m-rrt.py | 366 ++++++++++++------ tests/benchmarks/pr2-in-iai-kitchen.py | 20 +- .../pr2-manipulation-kitchen-spf.py | 89 +++-- tests/benchmarks/pr2-manipulation-kitchen.py | 89 +++-- .../pr2-manipulation-two-hand-spf.py | 118 +++--- tests/benchmarks/pr2-manipulation-two-hand.py | 120 +++--- tests/benchmarks/romeo-placard-spf.py | 135 +++++-- tests/benchmarks/romeo-placard.py | 135 +++++-- tests/benchmarks/test_benchmarks.py | 8 +- 11 files changed, 803 insertions(+), 456 deletions(-) diff --git a/tests/benchmarks/baxter-manipulation-boxes-spf.py b/tests/benchmarks/baxter-manipulation-boxes-spf.py index f635221b..b05f2ac7 100644 --- a/tests/benchmarks/baxter-manipulation-boxes-spf.py +++ b/tests/benchmarks/baxter-manipulation-boxes-spf.py @@ -9,7 +9,6 @@ from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder from pyhpp.manipulation import ( - RandomShortcut as ManipRandomShortcut, GraphRandomShortcut, GraphPartialShortcut, ) @@ -22,12 +21,12 @@ from pyhpp.constraints import ( LockedJoint, ) -from pinocchio import SE3, Quaternion +from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths @@ -70,8 +69,7 @@ box_pose, ) robot.setJointBounds( - boxes[i] + '/root_joint', - [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] ) model = robot.model() @@ -88,47 +86,48 @@ # Calculate box positions rankB = list() for i in range(K): - joint_id = robot.model().getJointId(boxes[i] + '/root_joint') - rankB.append (robot.model().idx_qs[joint_id]) + joint_id = robot.model().getJointId(boxes[i] + "/root_joint") + rankB.append(robot.model().idx_qs[joint_id]) -bb = [0.7, 0.8, 0., 0.1] +bb = [0.7, 0.8, 0.0, 0.1] c = sqrt(2) / 2 xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) -nbCols = int(K * 1. / nBoxPerLine + 0.5) +nbCols = int(K * 1.0 / nBoxPerLine + 0.5) ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) for i in range(K): iL = i % nBoxPerLine iC = (i - iL) / nBoxPerLine x = bb[0] + xstep * iL y = bb[2] + xstep * iC - q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c] q_goal = q_init[::].copy() for i in range(K): r = rankB[i] rn = rankB[goal[i]] - q_goal[r:r+7] = q_init[rn:rn+7] + q_goal[r : r + 7] = q_init[rn : rn + 7] constraints = dict() graphConstraints = dict() jointNames = dict() -jointNames['all'] = robot.model().names -jointNames['baxterRightSide'] = list() -jointNames['baxterLeftSide'] = list() +jointNames["all"] = robot.model().names +jointNames["baxterRightSide"] = list() +jointNames["baxterLeftSide"] = list() -for n in jointNames['all']: +for n in jointNames["all"]: if n.startswith("baxter"): if n.startswith("baxter/left_"): - jointNames['baxterLeftSide'].append(n) + jointNames["baxterLeftSide"].append(n) if n.startswith("baxter/right_"): - jointNames['baxterRightSide'].append(n) + jointNames["baxterRightSide"].append(n) # Lock finger joints -lockFingers = ["r_gripper_l_finger", - "r_gripper_r_finger", - "l_gripper_l_finger", - "l_gripper_r_finger", - ] +lockFingers = [ + "r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", +] for side in ["r", "l"]: joint_name = "baxter/" + side + "_gripper_r_finger_joint" cs = LockedJoint(robot, joint_name, np.array([-0.02])) @@ -141,18 +140,18 @@ # Lock head -lockHead = 'head_pan' -joint_name = 'baxter/head_pan' +lockHead = "head_pan" +joint_name = "baxter/head_pan" joint_id = robot.model().getJointId(joint_name) cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) constraints[lockHead] = cs graphConstraints[lockHead] = cs for n in jointNames["baxterRightSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs for n in jointNames["baxterLeftSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs # Define handles and contact surfaces @@ -168,20 +167,20 @@ factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) -factory.environmentContacts(['table/pancake_table_table_top']) +factory.environmentContacts(["table/pancake_table_table_top"]) factory.setObjects(boxes, handlesPerObject, objContactSurfaces) factory.setRules(rules) factory.generate() -cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.addNumericalConstraintsToGraph(list(graphConstraints.values())) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: - raise Exception('Init configuration could not be projected.') + raise Exception("Init configuration could not be projected.") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: - raise Exception('Goal configuration could not be projected.') + raise Exception("Goal configuration could not be projected.") problem.initConfig(q_init_proj) @@ -200,14 +199,19 @@ problem.addConfigValidation("CollisionValidation") optimizers = { - 'GraphPartialShortcut': GraphPartialShortcut(problem), - 'GraphRandomShortcut': GraphRandomShortcut(problem), - 'PartialShortcut': PartialShortcut(problem), - 'RandomShortcut': RandomShortcut(problem), - 'SimpleShortcut': SimpleShortcut(problem), + "GraphPartialShortcut": GraphPartialShortcut(problem), + "GraphRandomShortcut": GraphRandomShortcut(problem), + "PartialShortcut": PartialShortcut(problem), + "RandomShortcut": RandomShortcut(problem), + "SimpleShortcut": SimpleShortcut(problem), } -optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', - 'RandomShortcut', 'SimpleShortcut'] +optimizerNames = [ + "GraphPartialShortcut", + "GraphRandomShortcut", + "PartialShortcut", + "RandomShortcut", + "SimpleShortcut", +] iOpt = 0 totalTime = dt.timedelta(0) @@ -219,7 +223,7 @@ iOpt += 1 if iOpt == len(optimizerNames): iOpt = 0 - + try: planner.roadmap().clear() problem.resetGoalConfigs() @@ -245,4 +249,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/baxter-manipulation-boxes.py b/tests/benchmarks/baxter-manipulation-boxes.py index c146ce4e..b434bb7a 100644 --- a/tests/benchmarks/baxter-manipulation-boxes.py +++ b/tests/benchmarks/baxter-manipulation-boxes.py @@ -10,8 +10,6 @@ from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner from pyhpp.manipulation import ( - RandomShortcut as ManipRandomShortcut, - EnforceTransitionSemantic, GraphRandomShortcut, GraphPartialShortcut, ) @@ -24,12 +22,12 @@ from pyhpp.constraints import ( LockedJoint, ) -from pinocchio import SE3, Quaternion +from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths @@ -72,8 +70,7 @@ box_pose, ) robot.setJointBounds( - boxes[i] + '/root_joint', - [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] + boxes[i] + "/root_joint", [-1, 0.5, -1, 2, 0.6, 1.9, -1, 1, -1, 1, -1, 1, -1, 1] ) tmp = boxes[0] @@ -95,47 +92,48 @@ # Calculate box positions rankB = list() for i in range(K): - joint_id = robot.model().getJointId(boxes[i] + '/root_joint') - rankB.append (robot.model().idx_qs[joint_id]) + joint_id = robot.model().getJointId(boxes[i] + "/root_joint") + rankB.append(robot.model().idx_qs[joint_id]) -bb = [0.7, 0.8, 0., 0.1] +bb = [0.7, 0.8, 0.0, 0.1] c = sqrt(2) / 2 xstep = (bb[1] - bb[0]) / (nBoxPerLine - 1) if nBoxPerLine > 1 else (bb[1] - bb[0]) -nbCols = int(K * 1. / nBoxPerLine + 0.5) +nbCols = int(K * 1.0 / nBoxPerLine + 0.5) ystep = (bb[3] - bb[2]) / (nbCols - 1) if nbCols > 1 else (bb[3] - bb[2]) for i in range(K): iL = i % nBoxPerLine iC = (i - iL) / nBoxPerLine x = bb[0] + xstep * iL y = bb[2] + xstep * iC - q_init[rankB[i]:rankB[i]+7] = [x, y, 0.746, 0, -c, 0, c] + q_init[rankB[i] : rankB[i] + 7] = [x, y, 0.746, 0, -c, 0, c] q_goal = q_init[::].copy() for i in range(K): r = rankB[i] rn = rankB[goal[i]] - q_goal[r:r+7] = q_init[rn:rn+7] + q_goal[r : r + 7] = q_init[rn : rn + 7] constraints = dict() graphConstraints = dict() jointNames = dict() -jointNames['all'] = robot.model().names -jointNames['baxterRightSide'] = list() -jointNames['baxterLeftSide'] = list() +jointNames["all"] = robot.model().names +jointNames["baxterRightSide"] = list() +jointNames["baxterLeftSide"] = list() -for n in jointNames['all']: +for n in jointNames["all"]: if n.startswith("baxter"): if n.startswith("baxter/left_"): - jointNames['baxterLeftSide'].append(n) + jointNames["baxterLeftSide"].append(n) if n.startswith("baxter/right_"): - jointNames['baxterRightSide'].append(n) + jointNames["baxterRightSide"].append(n) # Lock finger joints -lockFingers = ["r_gripper_l_finger", - "r_gripper_r_finger", - "l_gripper_l_finger", - "l_gripper_r_finger", - ] +lockFingers = [ + "r_gripper_l_finger", + "r_gripper_r_finger", + "l_gripper_l_finger", + "l_gripper_r_finger", +] for side in ["r", "l"]: joint_name = "baxter/" + side + "_gripper_r_finger_joint" cs = LockedJoint(robot, joint_name, np.array([-0.02])) @@ -148,18 +146,18 @@ # Lock head -lockHead = 'head_pan' -joint_name = 'baxter/head_pan' +lockHead = "head_pan" +joint_name = "baxter/head_pan" joint_id = robot.model().getJointId(joint_name) cs = LockedJoint(robot, joint_name, np.array([q_init[robot.model().idx_qs[joint_id]]])) constraints[lockHead] = cs graphConstraints[lockHead] = cs for n in jointNames["baxterRightSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs for n in jointNames["baxterLeftSide"]: - cs = LockedJoint(robot, n, np.array([0.])) + cs = LockedJoint(robot, n, np.array([0.0])) constraints[n] = cs # Define handles and contact surfaces @@ -175,20 +173,20 @@ factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) -factory.environmentContacts(['table/pancake_table_table_top']) +factory.environmentContacts(["table/pancake_table_table_top"]) factory.setObjects(boxes, handlesPerObject, objContactSurfaces) factory.setRules(rules) factory.generate() -cg.addNumericalConstraintsToGraph( list(graphConstraints.values())) +cg.addNumericalConstraintsToGraph(list(graphConstraints.values())) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: - raise Exception('Init configuration could not be projected.') + raise Exception("Init configuration could not be projected.") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: - raise Exception('Goal configuration could not be projected.') + raise Exception("Goal configuration could not be projected.") problem.initConfig(q_init_proj) @@ -200,14 +198,19 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") optimizers = { - 'GraphPartialShortcut': GraphPartialShortcut(problem), - 'GraphRandomShortcut': GraphRandomShortcut(problem), - 'PartialShortcut': PartialShortcut(problem), - 'RandomShortcut': RandomShortcut(problem), - 'SimpleShortcut': SimpleShortcut(problem), + "GraphPartialShortcut": GraphPartialShortcut(problem), + "GraphRandomShortcut": GraphRandomShortcut(problem), + "PartialShortcut": PartialShortcut(problem), + "RandomShortcut": RandomShortcut(problem), + "SimpleShortcut": SimpleShortcut(problem), } -optimizerNames = ['GraphPartialShortcut', 'GraphRandomShortcut', 'PartialShortcut', - 'RandomShortcut', 'SimpleShortcut'] +optimizerNames = [ + "GraphPartialShortcut", + "GraphRandomShortcut", + "PartialShortcut", + "RandomShortcut", + "SimpleShortcut", +] iOpt = 0 totalTime = dt.timedelta(0) @@ -219,7 +222,7 @@ iOpt += 1 if iOpt == len(optimizerNames): iOpt = 0 - + try: planner.roadmap().clear() problem.resetGoalConfigs() diff --git a/tests/benchmarks/construction-set-m-rrt.py b/tests/benchmarks/construction-set-m-rrt.py index bf23ba81..ee9b2259 100644 --- a/tests/benchmarks/construction-set-m-rrt.py +++ b/tests/benchmarks/construction-set-m-rrt.py @@ -22,19 +22,22 @@ from pyhpp.manipulation.security_margins import SecurityMargins parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') -parser.add_argument('--bigGraph', action='store_true', +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") +parser.add_argument( + "--bigGraph", + action="store_true", help="Whether constraint graph is generated with all the possible states. " - "If unspecified, constraint graph only has a few states traversed by " - "one safe path.") + "If unspecified, constraint graph only has a few states traversed by " + "one safe path.", +) args = parser.parse_args() class StateName(object): - noGrasp = 'free' - + noGrasp = "free" + def __init__(self, grasps): if isinstance(grasps, set): self.grasps = grasps.copy() @@ -42,22 +45,22 @@ def __init__(self, grasps): if grasps == self.noGrasp: self.grasps = set() else: - g1 = [s.strip(' ') for s in grasps.split(':')] - self.grasps = set([tuple(s.split(' grasps ')) for s in g1]) + g1 = [s.strip(" ") for s in grasps.split(":")] + self.grasps = set([tuple(s.split(" grasps ")) for s in g1]) else: - raise TypeError('expecting a set of pairs (gripper, handle) or a string') - + raise TypeError("expecting a set of pairs (gripper, handle) or a string") + def __str__(self): if self.grasps == set(): - return 'free' + return "free" res = "" for g in self.grasps: res += g[0] + " grasps " + g[1] + " : " return res[:-3] - + def __eq__(self, other): return self.grasps == other.grasps - + def __ne__(self, other): return not self.__eq__(other) @@ -74,20 +77,20 @@ def __ne__(self, other): nSphere = 2 nCylinder = 2 -robot = Device('2ur5-sphere') +robot = Device("2ur5-sphere") -r0_pose = SE3(rotation=np.identity(3), translation=np.array([-.25, 0, 0])) +r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) urdf.loadModel(robot, 0, "r0", "anchor", ur3_urdf, ur3_srdf, r0_pose) -r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([.25, 0, 0])) +r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([0.25, 0, 0])) urdf.loadModel(robot, 0, "r1", "anchor", ur3_urdf, ur3_srdf, r1_pose) -robot.setJointBounds('r0/shoulder_pan_joint', [-pi, 4]) -robot.setJointBounds('r1/shoulder_pan_joint', [-pi, 4]) -robot.setJointBounds('r0/shoulder_lift_joint', [-pi, 0]) -robot.setJointBounds('r1/shoulder_lift_joint', [-pi, 0]) -robot.setJointBounds('r0/elbow_joint', [-2.6, 2.6]) -robot.setJointBounds('r1/elbow_joint', [-2.6, 2.6]) +robot.setJointBounds("r0/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r1/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r0/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r1/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r0/elbow_joint", [-2.6, 2.6]) +robot.setJointBounds("r1/elbow_joint", [-2.6, 2.6]) ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "ground", "anchor", ground_urdf, ground_srdf, ground_pose) @@ -95,23 +98,61 @@ def __ne__(self, other): objects = list() for i in range(nSphere): sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) - urdf.loadModel(robot, 0, f'sphere{i}', "freeflyer", sphere_urdf, sphere_srdf, sphere_pose) + urdf.loadModel( + robot, 0, f"sphere{i}", "freeflyer", sphere_urdf, sphere_srdf, sphere_pose + ) robot.setJointBounds( - f'sphere{i}/root_joint', - [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, - -1.0001, 1.0001, -1.0001, 1.0001] + f"sphere{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], ) - objects.append(f'sphere{i}') + objects.append(f"sphere{i}") for i in range(nCylinder): cylinder_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) - urdf.loadModel(robot, 0, f'cylinder{i}', "freeflyer", cylinder_08_urdf, cylinder_08_srdf, cylinder_pose) + urdf.loadModel( + robot, + 0, + f"cylinder{i}", + "freeflyer", + cylinder_08_urdf, + cylinder_08_srdf, + cylinder_pose, + ) robot.setJointBounds( - f'cylinder{i}/root_joint', - [-1., 1., -1., 1., -.1, 1., -1.0001, 1.0001, -1.0001, 1.0001, - -1.0001, 1.0001, -1.0001, 1.0001] + f"cylinder{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], ) - objects.append(f'cylinder{i}') + objects.append(f"cylinder{i}") model = robot.model() @@ -130,42 +171,68 @@ def __ne__(self, other): Id = SE3.Identity() spherePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) joint = model.getJointId(f"sphere{i}/root_joint") - + pc = Transformation( - placementName, robot, joint, Id, spherePlacement, + placementName, + robot, + joint, + Id, + spherePlacement, [False, False, True, True, True, False], ) cts = ComparisonTypes() - cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) constraints[placementName] = Implicit(pc, cts, [True, True, True]) - + pc_complement = Transformation( - placementName + "/complement", robot, joint, Id, spherePlacement, + placementName + "/complement", + robot, + joint, + Id, + spherePlacement, [True, True, False, False, False, True], ) cts_complement = ComparisonTypes() - cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) - constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) - + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + cts_hold = ComparisonTypes() cts_hold[:] = ( - ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, - ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, ) constraints[placementName + "/hold"] = LockedJoint( robot, f"sphere{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold ) - + cg.registerConstraints( constraints[placementName], constraints[placementName + "/complement"], constraints[placementName + "/hold"], ) - + preplacementName = f"preplace_sphere{i}" spherePrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) pc_pre = Transformation( - preplacementName, robot, joint, Id, spherePrePlacement, + preplacementName, + robot, + joint, + Id, + spherePrePlacement, [False, False, True, True, True, False], ) constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) @@ -175,52 +242,78 @@ def __ne__(self, other): Id = SE3.Identity() cylinderPlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) joint = model.getJointId(f"cylinder{i}/root_joint") - + pc = Transformation( - placementName, robot, joint, Id, cylinderPlacement, + placementName, + robot, + joint, + Id, + cylinderPlacement, [False, False, True, True, True, False], ) cts = ComparisonTypes() - cts[:] = (ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.EqualToZero) + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) constraints[placementName] = Implicit(pc, cts, [True, True, True]) - + pc_complement = Transformation( - placementName + "/complement", robot, joint, Id, cylinderPlacement, + placementName + "/complement", + robot, + joint, + Id, + cylinderPlacement, [True, True, False, False, False, True], ) cts_complement = ComparisonTypes() - cts_complement[:] = (ComparisonType.Equality, ComparisonType.Equality, ComparisonType.Equality) - constraints[placementName + "/complement"] = Implicit(pc_complement, cts_complement, [True, True, True]) - + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + cts_hold = ComparisonTypes() cts_hold[:] = ( - ComparisonType.Equality, ComparisonType.Equality, ComparisonType.EqualToZero, - ComparisonType.EqualToZero, ComparisonType.EqualToZero, ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, ) constraints[placementName + "/hold"] = LockedJoint( robot, f"cylinder{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold ) - + cg.registerConstraints( constraints[placementName], constraints[placementName + "/complement"], constraints[placementName + "/hold"], ) - + preplacementName = f"preplace_cylinder{i}" cylinderPrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) pc_pre = Transformation( - preplacementName, robot, joint, Id, cylinderPrePlacement, + preplacementName, + robot, + joint, + Id, + cylinderPrePlacement, [False, False, True, True, True, False], ) constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) -grippers = [f'cylinder{i}/magnet0' for i in range(nCylinder)] -grippers += [f'cylinder{i}/magnet1' for i in range(nCylinder)] +grippers = [f"cylinder{i}/magnet0" for i in range(nCylinder)] +grippers += [f"cylinder{i}/magnet1" for i in range(nCylinder)] grippers += [f"r{i}/gripper" for i in range(2)] -handlesPerObjects = [[f'sphere{i}/handle', f'sphere{i}/magnet'] for i in range(nSphere)] -handlesPerObjects += [[f'cylinder{i}/handle'] for i in range(nCylinder)] +handlesPerObjects = [[f"sphere{i}/handle", f"sphere{i}/magnet"] for i in range(nSphere)] +handlesPerObjects += [[f"cylinder{i}/handle"] for i in range(nCylinder)] shapesPerObject = [[] for o in objects] @@ -230,13 +323,13 @@ def __ne__(self, other): def makeRule(grasps): _grippers = list() _handles = list() - for (g, h) in grasps: + for g, h in grasps: _grippers.append(g) _handles.append(h) for g in grippers: - if not g in _grippers: + if g not in _grippers: _grippers.append(g) - _handles += (len(_grippers) - len(_handles)) * ['^$'] + _handles += (len(_grippers) - len(_handles)) * ["^$"] return Rule(grippers=_grippers, handles=_handles, link=True) @@ -245,23 +338,26 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: hRegex = [re.compile(handle) for handle in h] forbiddenList: T.List[Rule] = list() idForbidGrippers = [i for i in range(len(grippers)) if gRegex.match(grippers[i])] - forbidHandles = [handle for handle in allHandles - if not any([handlePattern.match(handle) for handlePattern in hRegex])] + forbidHandles = [ + handle + for handle in allHandles + if not any([handlePattern.match(handle) for handlePattern in hRegex]) + ] for id in idForbidGrippers: for handle in forbidHandles: - _handles = [handle if i == id else '.*' for i in range(len(grippers))] + _handles = [handle if i == id else ".*" for i in range(len(grippers))] forbiddenList.append(Rule(grippers=grippers, handles=_handles, link=False)) return forbiddenList -q0_r0 = [pi/6, -pi/2, pi/2, 0, 0, 0] +q0_r0 = [pi / 6, -pi / 2, pi / 2, 0, 0, 0] q0_r1 = q0_r0[::] q0_spheres = list() i = 0 y = 0.04 while i < nSphere: - q0_spheres.append([-.1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + q0_spheres.append([-0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -269,7 +365,7 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: i = 0 y = -0.04 while i < nCylinder: - q0_cylinders.append([0.45 + .1*(i/2), -.12 + y, 0.025, 0, 0, 0, 1]) + q0_cylinders.append([0.45 + 0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) i += 1 y = -y @@ -280,103 +376,141 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: grasps = set() nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r0/gripper', 'sphere0/handle')) + + grasps.add(("r0/gripper", "sphere0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r1/gripper', 'cylinder0/handle')) + + grasps.add(("r1/gripper", "cylinder0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('cylinder0/magnet0', 'sphere0/magnet')) + + grasps.add(("cylinder0/magnet0", "sphere0/magnet")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r0/gripper', 'sphere0/handle')) + + grasps.remove(("r0/gripper", "sphere0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('r0/gripper', 'sphere1/handle')) + + grasps.add(("r0/gripper", "sphere1/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.add(('cylinder0/magnet1', 'sphere1/magnet')) + + grasps.add(("cylinder0/magnet1", "sphere1/magnet")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r0/gripper', 'sphere1/handle')) + + grasps.remove(("r0/gripper", "sphere1/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - - grasps.remove(('r1/gripper', 'cylinder0/handle')) + + grasps.remove(("r1/gripper", "cylinder0/handle")) nodes.append(StateName(grasps)) rules.append(makeRule(grasps=grasps)) - + problem.steeringMethod = Straight(problem) problem.pathValidation = Progressive(robot, 0.02) problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, shapesPerObject) factory.setRules(rules) factory.generate() - - sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", - "cylinder0", "cylinder1"], robot) + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) sm.defaultMargin = 0.02 sm.apply() - + cg.initialize() - + problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + nodes.append(nodes[-1]) else: rules = list() - - rules.extend(forbidExcept('^r0/gripper$', ['^sphere\\d*/handle$'])) - rules.extend(forbidExcept('^r1/gripper$', ['^cylinder0*/handle$'])) - rules.extend(forbidExcept('^cylinder0/magnet\\d*$', ['^sphere\\d*/magnet$'])) - rules.extend(forbidExcept('^cylinder[1-9][0-9]*.*$', ['^$'])) - + + rules.extend(forbidExcept("^r0/gripper$", ["^sphere\\d*/handle$"])) + rules.extend(forbidExcept("^r1/gripper$", ["^cylinder0*/handle$"])) + rules.extend(forbidExcept("^cylinder0/magnet\\d*$", ["^sphere\\d*/magnet$"])) + rules.extend(forbidExcept("^cylinder[1-9][0-9]*.*$", ["^$"])) + rules.append(Rule(grippers=grippers, handles=[".*"] * len(grippers), link=True)) - + problem.steeringMethod = Straight(problem) problem.pathValidation = Progressive(robot, 0.02) problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) - + factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) factory.setObjects(objects, handlesPerObjects, shapesPerObject) factory.setRules(rules) factory.generate() - - sm = SecurityMargins(problem, factory, ["r0", "r1", "sphere0", "sphere1", - "cylinder0", "cylinder1"], robot) + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) sm.defaultMargin = 0.02 sm.apply() - + cg.initialize() - + problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.05 ) -assert (nCylinder == 2 and nSphere == 2) +assert nCylinder == 2 and nSphere == 2 c = sqrt(2) / 2 -q_goal = np.array(q0_r0 + q0_r1 + [-0.06202136144745322, -0.15, 0.025, c, 0, -c, 0, - 0.06202136144745322, -0.15, 0.025, c, 0, c, 0, - 0, -0.15, 0.025, 0, 0, 0, 1, - 0.5, -0.08, 0.025, 0, 0, 0, 1]) +q_goal = np.array( + q0_r0 + + q0_r1 + + [ + -0.06202136144745322, + -0.15, + 0.025, + c, + 0, + -c, + 0, + 0.06202136144745322, + -0.15, + 0.025, + c, + 0, + c, + 0, + 0, + -0.15, + 0.025, + 0, + 0, + 0, + 1, + 0.5, + -0.08, + 0.025, + 0, + 0, + 0, + 1, + ] +) problem.initConfig(q0) problem.addGoalConfig(q_goal) @@ -414,4 +548,4 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-in-iai-kitchen.py b/tests/benchmarks/pr2-in-iai-kitchen.py index 29ffa922..f7695ebf 100644 --- a/tests/benchmarks/pr2-in-iai-kitchen.py +++ b/tests/benchmarks/pr2-in-iai-kitchen.py @@ -9,16 +9,16 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" -robot = Device('pr2') +robot = Device("pr2") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -34,17 +34,17 @@ q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = model.idx_qs[model.getJointId('pr2/torso_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/torso_lift_joint")] q_init[rank] = 0.2 q_goal[0:2] = [-3.2, -4] -rank = model.idx_qs[model.getJointId('pr2/l_shoulder_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/l_shoulder_lift_joint")] q_goal[rank] = 0.5 -rank = model.idx_qs[model.getJointId('pr2/r_shoulder_lift_joint')] +rank = model.idx_qs[model.getJointId("pr2/r_shoulder_lift_joint")] q_goal[rank] = 0.5 -rank = model.idx_qs[model.getJointId('pr2/l_elbow_flex_joint')] +rank = model.idx_qs[model.getJointId("pr2/l_elbow_flex_joint")] q_goal[rank] = -0.5 -rank = model.idx_qs[model.getJointId('pr2/r_elbow_flex_joint')] +rank = model.idx_qs[model.getJointId("pr2/r_elbow_flex_joint")] q_goal[rank] = -0.5 problem = Problem(robot) @@ -85,4 +85,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-kitchen-spf.py b/tests/benchmarks/pr2-manipulation-kitchen-spf.py index f7a45314..2a9bb924 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen-spf.py +++ b/tests/benchmarks/pr2-manipulation-kitchen-spf.py @@ -1,7 +1,7 @@ #!/usr/bin/env python from argparse import ArgumentParser -from math import sqrt, pi +from math import sqrt import numpy as np import datetime as dt @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,10 +35,14 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) +robot.setJointBounds( + "box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2] +) model = robot.model() @@ -44,7 +50,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -53,45 +59,46 @@ c = sqrt(2) / 2 q_init = robot.currentConfiguration() -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -4, 0.9, 0, c, 0, c] q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/l_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/l_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/r_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/r_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] - -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs - -grippers = ['pr2/l_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle']] -envSurfaces = ['kitchen_area/pancake_table_table_top', - 'kitchen_area/white_counter_top_sink'] -objContactSurfaces = [['box/box_surface']] - -rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), - Rule([""], [""], True)] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs + +grippers = ["pr2/l_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle"]] +envSurfaces = [ + "kitchen_area/pancake_table_table_top", + "kitchen_area/white_counter_top_sink", +] +objContactSurfaces = [["box/box_surface"]] + +rules = [Rule(["pr2/l_gripper"], ["box/handle"], True), Rule([""], [""], True)] factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) @@ -102,19 +109,19 @@ cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) -cg.setWeight(cg.getTransition('Loop | f'), 1) -cg.setWeight(cg.getTransition('Loop | 0-0'), 1) +cg.setWeight(cg.getTransition("Loop | f"), 1) +cg.setWeight(cg.getTransition("Loop | 0-0"), 1) problem.steeringMethod = Straight(problem) problem.pathValidation = Dichotomy(robot, 0.0) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -159,4 +166,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-kitchen.py b/tests/benchmarks/pr2-manipulation-kitchen.py index 50685a85..10d18e7d 100644 --- a/tests/benchmarks/pr2-manipulation-kitchen.py +++ b/tests/benchmarks/pr2-manipulation-kitchen.py @@ -1,7 +1,7 @@ #!/usr/bin/env python from argparse import ArgumentParser -from math import sqrt, pi +from math import sqrt import numpy as np import datetime as dt @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,10 +35,14 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2]) +robot.setJointBounds( + "box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2] +) model = robot.model() @@ -44,7 +50,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -53,45 +59,46 @@ c = sqrt(2) / 2 q_init = robot.currentConfiguration() -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 q_goal = q_init.copy() q_init[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -4, 0.9, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -4, 0.9, 0, c, 0, c] q_goal[0:2] = [-3.2, -4] -rank = robot.rankInConfiguration['pr2/l_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/l_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/l_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['pr2/r_shoulder_lift_joint'] +rank = robot.rankInConfiguration["pr2/r_shoulder_lift_joint"] q_goal[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_elbow_flex_joint'] +rank = robot.rankInConfiguration["pr2/r_elbow_flex_joint"] q_goal[rank] = -0.5 -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-4.8, -4.8, 0.9, 0, c, 0, c] - -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs - -grippers = ['pr2/l_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle']] -envSurfaces = ['kitchen_area/pancake_table_table_top', - 'kitchen_area/white_counter_top_sink'] -objContactSurfaces = [['box/box_surface']] - -rules = [Rule(['pr2/l_gripper'], ['box/handle'], True), - Rule([""], [""], True)] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-4.8, -4.8, 0.9, 0, c, 0, c] + +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs + +grippers = ["pr2/l_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle"]] +envSurfaces = [ + "kitchen_area/pancake_table_table_top", + "kitchen_area/white_counter_top_sink", +] +objContactSurfaces = [["box/box_surface"]] + +rules = [Rule(["pr2/l_gripper"], ["box/handle"], True), Rule([""], [""], True)] factory = ConstraintGraphFactory(cg, constraints) factory.setGrippers(grippers) @@ -102,19 +109,19 @@ cg.addNumericalConstraintsToGraph([constraints[c] for c in locklhand]) -cg.setWeight(cg.getTransition('Loop | f'), 1) -cg.setWeight(cg.getTransition('Loop | 0-0'), 1) +cg.setWeight(cg.getTransition("Loop | f"), 1) +cg.setWeight(cg.getTransition("Loop | 0-0"), 1) problem.steeringMethod = Straight(problem) problem.pathValidation = Dichotomy(robot, 0.0) cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -155,4 +162,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-two-hand-spf.py b/tests/benchmarks/pr2-manipulation-two-hand-spf.py index 85c2a857..495d6680 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand-spf.py +++ b/tests/benchmarks/pr2-manipulation-two-hand-spf.py @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,7 +35,9 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) @@ -44,7 +48,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -55,63 +59,75 @@ q_init = robot.currentConfiguration() q_init[0:4] = [-3.2, -4, 1, 0] -rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 q_goal = q_init.copy() -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs -lockrhand = ['r_l_finger', 'r_r_finger'] -cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) -constraints['r_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) -constraints['r_r_finger'] = cs +lockrhand = ["r_l_finger", "r_r_finger"] +cs = LockedJoint(robot, "pr2/r_gripper_l_finger_joint", np.array([0.5])) +constraints["r_l_finger"] = cs +cs = LockedJoint(robot, "pr2/r_gripper_r_finger_joint", np.array([0.5])) +constraints["r_r_finger"] = cs lockhands = lockrhand + locklhand -lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] -cs = LockedJoint(robot, 'pr2/head_pan_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) -constraints['head_pan'] = cs -cs = LockedJoint(robot, 'pr2/head_tilt_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) -constraints['head_tilt'] = cs -cs = LockedJoint(robot, 'pr2/torso_lift_joint', - np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) -constraints['torso'] = cs -cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', - np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) -constraints['laser'] = cs - -lockPlanar = ['lockplanar'] -cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) -constraints['lockplanar'] = cs +lockHeadAndTorso = ["head_pan", "head_tilt", "torso", "laser"] +cs = LockedJoint( + robot, + "pr2/head_pan_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_pan_joint"]]]), +) +constraints["head_pan"] = cs +cs = LockedJoint( + robot, + "pr2/head_tilt_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_tilt_joint"]]]), +) +constraints["head_tilt"] = cs +cs = LockedJoint( + robot, + "pr2/torso_lift_joint", + np.array([q_init[robot.rankInConfiguration["pr2/torso_lift_joint"]]]), +) +constraints["torso"] = cs +cs = LockedJoint( + robot, + "pr2/laser_tilt_mount_joint", + np.array([q_init[robot.rankInConfiguration["pr2/laser_tilt_mount_joint"]]]), +) +constraints["laser"] = cs + +lockPlanar = ["lockplanar"] +cs = LockedJoint(robot, "pr2/root_joint", np.array([-3.2, -4, 1, 0])) +constraints["lockplanar"] = cs lockAll = lockhands + lockHeadAndTorso + lockPlanar -grippers = ['pr2/l_gripper', 'pr2/r_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle', 'box/handle2']] -objContactSurfaces = [['box/box_surface']] -envSurfaces = ['kitchen_area/pancake_table_table_top'] +grippers = ["pr2/l_gripper", "pr2/r_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle", "box/handle2"]] +objContactSurfaces = [["box/box_surface"]] +envSurfaces = ["kitchen_area/pancake_table_table_top"] rules = [Rule([""], [""], True)] @@ -132,11 +148,11 @@ cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -181,4 +197,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/pr2-manipulation-two-hand.py b/tests/benchmarks/pr2-manipulation-two-hand.py index b4fad231..0465e9ca 100644 --- a/tests/benchmarks/pr2-manipulation-two-hand.py +++ b/tests/benchmarks/pr2-manipulation-two-hand.py @@ -12,19 +12,21 @@ from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +pr2_srdf = ( + "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" +) box_urdf = "package://hpp_tutorial/urdf/box.urdf" box_srdf = "package://hpp_tutorial/srdf/box.srdf" kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" -robot = Device('pr2-box') +robot = Device("pr2-box") pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) @@ -33,7 +35,9 @@ urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose) +urdf.loadModel( + robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose +) robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) @@ -44,7 +48,7 @@ problem.clearConfigValidations() problem.addConfigValidation("CollisionValidation") -cg = Graph('graph', robot, problem) +cg = Graph("graph", robot, problem) cg.errorThreshold(1e-3) cg.maxIterations(40) @@ -55,64 +59,76 @@ q_init = robot.currentConfiguration() q_init[0:4] = [-3.2, -4, 1, 0] -rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/r_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_l_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/l_gripper_r_finger_joint'] +rank = robot.rankInConfiguration["pr2/l_gripper_r_finger_joint"] q_init[rank] = 0.5 -rank = robot.rankInConfiguration['pr2/torso_lift_joint'] +rank = robot.rankInConfiguration["pr2/torso_lift_joint"] q_init[rank] = 0.2 q_goal = q_init.copy() -rank = robot.rankInConfiguration['box/root_joint'] -q_init[rank:rank+7] = [-2.5, -3.6, 0.76, 0, c, 0, c] -rank = robot.rankInConfiguration['box/root_joint'] -q_goal[rank:rank+7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_init[rank : rank + 7] = [-2.5, -3.6, 0.76, 0, c, 0, c] +rank = robot.rankInConfiguration["box/root_joint"] +q_goal[rank : rank + 7] = [-2.5, -4.4, 0.76, 0, -c, 0, c] -locklhand = ['l_l_finger', 'l_r_finger'] -cs = LockedJoint(robot, 'pr2/l_gripper_l_finger_joint', np.array([0.5])) -constraints['l_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/l_gripper_r_finger_joint', np.array([0.5])) -constraints['l_r_finger'] = cs +locklhand = ["l_l_finger", "l_r_finger"] +cs = LockedJoint(robot, "pr2/l_gripper_l_finger_joint", np.array([0.5])) +constraints["l_l_finger"] = cs +cs = LockedJoint(robot, "pr2/l_gripper_r_finger_joint", np.array([0.5])) +constraints["l_r_finger"] = cs -lockrhand = ['r_l_finger', 'r_r_finger'] -cs = LockedJoint(robot, 'pr2/r_gripper_l_finger_joint', np.array([0.5])) -constraints['r_l_finger'] = cs -cs = LockedJoint(robot, 'pr2/r_gripper_r_finger_joint', np.array([0.5])) -constraints['r_r_finger'] = cs +lockrhand = ["r_l_finger", "r_r_finger"] +cs = LockedJoint(robot, "pr2/r_gripper_l_finger_joint", np.array([0.5])) +constraints["r_l_finger"] = cs +cs = LockedJoint(robot, "pr2/r_gripper_r_finger_joint", np.array([0.5])) +constraints["r_r_finger"] = cs lockhands = lockrhand + locklhand -lockHeadAndTorso = ['head_pan', 'head_tilt', 'torso', 'laser'] -cs = LockedJoint(robot, 'pr2/head_pan_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_pan_joint']]])) -constraints['head_pan'] = cs -cs = LockedJoint(robot, 'pr2/head_tilt_joint', - np.array([q_init[robot.rankInConfiguration['pr2/head_tilt_joint']]])) -constraints['head_tilt'] = cs -cs = LockedJoint(robot, 'pr2/torso_lift_joint', - np.array([q_init[robot.rankInConfiguration['pr2/torso_lift_joint']]])) -constraints['torso'] = cs -cs = LockedJoint(robot, 'pr2/laser_tilt_mount_joint', - np.array([q_init[robot.rankInConfiguration['pr2/laser_tilt_mount_joint']]])) -constraints['laser'] = cs - -lockPlanar = ['lockplanar'] -rank = robot.rankInConfiguration['pr2/root_joint'] -cs = LockedJoint(robot, 'pr2/root_joint', np.array([-3.2, -4, 1, 0])) -constraints['lockplanar'] = cs +lockHeadAndTorso = ["head_pan", "head_tilt", "torso", "laser"] +cs = LockedJoint( + robot, + "pr2/head_pan_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_pan_joint"]]]), +) +constraints["head_pan"] = cs +cs = LockedJoint( + robot, + "pr2/head_tilt_joint", + np.array([q_init[robot.rankInConfiguration["pr2/head_tilt_joint"]]]), +) +constraints["head_tilt"] = cs +cs = LockedJoint( + robot, + "pr2/torso_lift_joint", + np.array([q_init[robot.rankInConfiguration["pr2/torso_lift_joint"]]]), +) +constraints["torso"] = cs +cs = LockedJoint( + robot, + "pr2/laser_tilt_mount_joint", + np.array([q_init[robot.rankInConfiguration["pr2/laser_tilt_mount_joint"]]]), +) +constraints["laser"] = cs + +lockPlanar = ["lockplanar"] +rank = robot.rankInConfiguration["pr2/root_joint"] +cs = LockedJoint(robot, "pr2/root_joint", np.array([-3.2, -4, 1, 0])) +constraints["lockplanar"] = cs lockAll = lockhands + lockHeadAndTorso + lockPlanar -grippers = ['pr2/l_gripper', 'pr2/r_gripper'] -boxes = ['box'] -handlesPerObject = [['box/handle', 'box/handle2']] -objContactSurfaces = [['box/box_surface']] -envSurfaces = ['kitchen_area/pancake_table_table_top'] +grippers = ["pr2/l_gripper", "pr2/r_gripper"] +boxes = ["box"] +handlesPerObject = [["box/handle", "box/handle2"]] +objContactSurfaces = [["box/box_surface"]] +envSurfaces = ["kitchen_area/pancake_table_table_top"] rules = [Rule([""], [""], True)] @@ -133,11 +149,11 @@ cg.initialize() -res, q_init_proj, err = cg.applyStateConstraints(cg.getState('free'), q_init) +res, q_init_proj, err = cg.applyStateConstraints(cg.getState("free"), q_init) if not res: raise RuntimeError("Failed to project initial configuration") -res, q_goal_proj, err = cg.applyStateConstraints(cg.getState('free'), q_goal) +res, q_goal_proj, err = cg.applyStateConstraints(cg.getState("free"), q_goal) if not res: raise RuntimeError("Failed to project goal configuration") @@ -178,4 +194,4 @@ print(f"Success rate: {success / args.N * 100}%") if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") \ No newline at end of file + print(f"Average number nodes per success: {totalNumberNodes / success}") diff --git a/tests/benchmarks/romeo-placard-spf.py b/tests/benchmarks/romeo-placard-spf.py index 72a72627..100535fa 100644 --- a/tests/benchmarks/romeo-placard-spf.py +++ b/tests/benchmarks/romeo-placard-spf.py @@ -1,44 +1,53 @@ #!/usr/bin/env python from argparse import ArgumentParser -import time import numpy as np import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule from pyhpp.manipulation import Device, Graph, Problem, urdf, StatesPathFinder from pyhpp.core import Dichotomy, Straight, ProgressiveProjector -from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.constraints import LockedJoint from pyhpp.core.static_stability_constraint_factory import ( StaticStabilityConstraintsFactory, ) from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" -romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +romeo_srdf_moveit = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +) placard_urdf = "package://hpp_environments/urdf/placard.urdf" placard_srdf = "package://hpp_environments/srdf/placard.srdf" -robot = Device('romeo-placard') +robot = Device("romeo-placard") # Load Romeo robot romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) +urdf.loadModel( + robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose +) -robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "romeo/root_joint", [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) # Load placard placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) +urdf.loadModel( + robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose +) -robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "placard/root_joint", [-1, 1, -1, 1, 0, 1.5, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) model = robot.model() @@ -88,7 +97,7 @@ # Lock left hand locklhand = list() for j, v in leftHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j locklhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -101,7 +110,7 @@ # Lock right hand lockrhand = list() for j, v in rightHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j lockrhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -115,13 +124,13 @@ # Create static stability constraint q = robot.currentConfiguration() -placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] -q[placard_rank:placard_rank+3] = [.4, 0, 1.2] +placard_rank = model.idx_qs[model.getJointId("placard/root_joint")] +q[placard_rank : placard_rank + 3] = [0.4, 0, 1.2] problem.addPartialCom("romeo", ["romeo/root_joint"]) -leftAnkle = 'romeo/LAnkleRoll' -rightAnkle = 'romeo/RAnkleRoll' +leftAnkle = "romeo/LAnkleRoll" +rightAnkle = "romeo/RAnkleRoll" factory = StaticStabilityConstraintsFactory(problem, robot) balanceConstraintsDict = factory.createStaticStabilityConstraint( @@ -129,9 +138,9 @@ ) balanceConstraints = [ - balanceConstraintsDict.get('balance/pose-left-foot'), - balanceConstraintsDict.get('balance/pose-right-foot'), - balanceConstraintsDict.get('balance/relative-com'), + balanceConstraintsDict.get("balance/pose-left-foot"), + balanceConstraintsDict.get("balance/pose-right-foot"), + balanceConstraintsDict.get("balance/relative-com"), ] balanceConstraints = [c for c in balanceConstraints if c is not None] @@ -141,8 +150,8 @@ graphConstraints[name] = constraint # Build graph -grippers = ['romeo/r_hand', 'romeo/l_hand'] -handlesPerObjects = [['placard/low', 'placard/high']] +grippers = ["romeo/r_hand", "romeo/l_hand"] +handlesPerObjects = [["placard/low", "placard/high"]] rules = [ Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), @@ -152,7 +161,7 @@ factory_cg = ConstraintGraphFactory(cg, constraints) factory_cg.setGrippers(grippers) -factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setObjects(["placard"], handlesPerObjects, [[]]) factory_cg.setRules(rules) factory_cg.generate() @@ -162,11 +171,83 @@ cg.initialize() # Define initial and final configurations -q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_goal = np.array( + [ + -0.003429678026293006, + 7.761615492429529e-05, + 0.8333148411182841, + -0.08000440760954532, + 0.06905332841243099, + -0.09070086400314036, + 0.9902546570793265, + 0.2097693637044623, + 0.19739743868699455, + -0.6079135018296973, + 0.8508704420155889, + -0.39897628829947995, + -0.05274298289004072, + 0.20772797293264825, + 0.1846394290733244, + -0.49824886682709824, + 0.5042013065348324, + -0.16158420369261683, + -0.039828502509861335, + -0.3827070014985058, + -0.24118425356319423, + 1.0157846623463191, + 0.5637424355124602, + -1.3378817283780955, + -1.3151786907256797, + -0.392409481224193, + 0.11332560818107676, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.35936687035487364, + -0.32595302056157444, + -0.33115291290191723, + 0.20387672048126043, + 0.9007626913161502, + -0.39038645767349395, + 0.31725226129015516, + 1.5475253831101246, + -0.0104572058777634, + 0.32681856374063933, + 0.24476959944940427, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.412075621240969, + 0.020809907186176854, + 1.056724788359247, + 0.0, + 0.0, + 0.0, + 1.0, + ] +) q_init = q_goal.copy() -q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] +q_init[placard_rank + 3 : placard_rank + 7] = [0, 0, 1, 0] -n = 'romeo/l_hand grasps placard/low' +n = "romeo/l_hand grasps placard/low" state = cg.getState(n) res, q_init_proj, err = cg.applyStateConstraints(state, q_init) if not res: @@ -222,5 +303,3 @@ if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") print(f"Average number nodes per success: {totalNumberNodes / success}") - - diff --git a/tests/benchmarks/romeo-placard.py b/tests/benchmarks/romeo-placard.py index 8d7aed44..b5bcfcbf 100644 --- a/tests/benchmarks/romeo-placard.py +++ b/tests/benchmarks/romeo-placard.py @@ -1,44 +1,53 @@ #!/usr/bin/env python from argparse import ArgumentParser -import time import numpy as np import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner from pyhpp.core import Dichotomy, Straight, ProgressiveProjector -from pyhpp.constraints import Transformation, LockedJoint +from pyhpp.constraints import LockedJoint from pyhpp.core.static_stability_constraint_factory import ( StaticStabilityConstraintsFactory, ) from pinocchio import SE3 parser = ArgumentParser() -parser.add_argument('-N', default=0, type=int) -parser.add_argument('--display', action='store_true') -parser.add_argument('--run', action='store_true') +parser.add_argument("-N", default=0, type=int) +parser.add_argument("--display", action="store_true") +parser.add_argument("--run", action="store_true") args = parser.parse_args() # Robot and object file paths romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" -romeo_srdf_moveit = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +romeo_srdf_moveit = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +) placard_urdf = "package://hpp_environments/urdf/placard.urdf" placard_srdf = "package://hpp_environments/srdf/placard.srdf" -robot = Device('romeo-placard') +robot = Device("romeo-placard") # Load Romeo robot romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose) +urdf.loadModel( + robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose +) -robot.setJointBounds('romeo/root_joint', [-1, 1, -1, 1, 0, 2, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "romeo/root_joint", [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) # Load placard placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose) +urdf.loadModel( + robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose +) -robot.setJointBounds('placard/root_joint', [-1, 1, -1, 1, 0, 1.5, -2., 2, -2., 2, -2., 2, -2., 2]) +robot.setJointBounds( + "placard/root_joint", [-1, 1, -1, 1, 0, 1.5, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) model = robot.model() @@ -88,7 +97,7 @@ # Lock left hand locklhand = list() for j, v in leftHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j locklhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -101,7 +110,7 @@ # Lock right hand lockrhand = list() for j, v in rightHandOpen.items(): - joint_name = 'romeo/' + j + joint_name = "romeo/" + j lockrhand.append(joint_name) if type(v) is float or type(v) is int: val = np.array([v]) @@ -115,13 +124,13 @@ # Create static stability constraint q = robot.currentConfiguration() -placard_rank = model.idx_qs[model.getJointId('placard/root_joint')] -q[placard_rank:placard_rank+3] = [.4, 0, 1.2] +placard_rank = model.idx_qs[model.getJointId("placard/root_joint")] +q[placard_rank : placard_rank + 3] = [0.4, 0, 1.2] problem.addPartialCom("romeo", ["romeo/root_joint"]) -leftAnkle = 'romeo/LAnkleRoll' -rightAnkle = 'romeo/RAnkleRoll' +leftAnkle = "romeo/LAnkleRoll" +rightAnkle = "romeo/RAnkleRoll" factory = StaticStabilityConstraintsFactory(problem, robot) balanceConstraintsDict = factory.createStaticStabilityConstraint( @@ -129,9 +138,9 @@ ) balanceConstraints = [ - balanceConstraintsDict.get('balance/pose-left-foot'), - balanceConstraintsDict.get('balance/pose-right-foot'), - balanceConstraintsDict.get('balance/relative-com'), + balanceConstraintsDict.get("balance/pose-left-foot"), + balanceConstraintsDict.get("balance/pose-right-foot"), + balanceConstraintsDict.get("balance/relative-com"), ] balanceConstraints = [c for c in balanceConstraints if c is not None] @@ -141,8 +150,8 @@ graphConstraints[name] = constraint # Build graph -grippers = ['romeo/r_hand', 'romeo/l_hand'] -handlesPerObjects = [['placard/low', 'placard/high']] +grippers = ["romeo/r_hand", "romeo/l_hand"] +handlesPerObjects = [["placard/low", "placard/high"]] rules = [ Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), @@ -152,7 +161,7 @@ factory_cg = ConstraintGraphFactory(cg, constraints) factory_cg.setGrippers(grippers) -factory_cg.setObjects(['placard'], handlesPerObjects, [[]]) +factory_cg.setObjects(["placard"], handlesPerObjects, [[]]) factory_cg.setRules(rules) factory_cg.generate() @@ -162,11 +171,83 @@ cg.initialize() # Define initial and final configurations -q_goal = np.array([-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841, -0.08000440760954532, 0.06905332841243099, -0.09070086400314036, 0.9902546570793265, 0.2097693637044623, 0.19739743868699455, -0.6079135018296973, 0.8508704420155889, -0.39897628829947995, -0.05274298289004072, 0.20772797293264825, 0.1846394290733244, -0.49824886682709824, 0.5042013065348324, -0.16158420369261683, -0.039828502509861335, -0.3827070014985058, -0.24118425356319423, 1.0157846623463191, 0.5637424355124602, -1.3378817283780955, -1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.35936687035487364, -0.32595302056157444, -0.33115291290191723, 0.20387672048126043, 0.9007626913161502, -0.39038645767349395, 0.31725226129015516, 1.5475253831101246, -0.0104572058777634, 0.32681856374063933, 0.24476959944940427, 1.06, 1.06, 1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06, 0.412075621240969, 0.020809907186176854, 1.056724788359247, 0.0, 0.0, 0.0, 1.0]) +q_goal = np.array( + [ + -0.003429678026293006, + 7.761615492429529e-05, + 0.8333148411182841, + -0.08000440760954532, + 0.06905332841243099, + -0.09070086400314036, + 0.9902546570793265, + 0.2097693637044623, + 0.19739743868699455, + -0.6079135018296973, + 0.8508704420155889, + -0.39897628829947995, + -0.05274298289004072, + 0.20772797293264825, + 0.1846394290733244, + -0.49824886682709824, + 0.5042013065348324, + -0.16158420369261683, + -0.039828502509861335, + -0.3827070014985058, + -0.24118425356319423, + 1.0157846623463191, + 0.5637424355124602, + -1.3378817283780955, + -1.3151786907256797, + -0.392409481224193, + 0.11332560818107676, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.35936687035487364, + -0.32595302056157444, + -0.33115291290191723, + 0.20387672048126043, + 0.9007626913161502, + -0.39038645767349395, + 0.31725226129015516, + 1.5475253831101246, + -0.0104572058777634, + 0.32681856374063933, + 0.24476959944940427, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.412075621240969, + 0.020809907186176854, + 1.056724788359247, + 0.0, + 0.0, + 0.0, + 1.0, + ] +) q_init = q_goal.copy() -q_init[placard_rank+3:placard_rank+7] = [0, 0, 1, 0] +q_init[placard_rank + 3 : placard_rank + 7] = [0, 0, 1, 0] -n = 'romeo/l_hand grasps placard/low' +n = "romeo/l_hand grasps placard/low" state = cg.getState(n) res, q_init_proj, err = cg.applyStateConstraints(state, q_init) if not res: @@ -217,5 +298,3 @@ if success > 0: print(f"Average time per success: {totalTime.total_seconds() / success}") print(f"Average number nodes per success: {totalNumberNodes / success}") - - diff --git a/tests/benchmarks/test_benchmarks.py b/tests/benchmarks/test_benchmarks.py index 582f7734..48079382 100644 --- a/tests/benchmarks/test_benchmarks.py +++ b/tests/benchmarks/test_benchmarks.py @@ -3,14 +3,16 @@ import sys from pathlib import Path + def main(): folder = Path(sys.argv[1]) if len(sys.argv) > 1 else Path(".") - + for script in sorted(folder.glob("*.py")): if script.name == Path(__file__).name: continue - print(f"\n{'='*60}\nRunning: {script.name}\n{'='*60}") + print(f"\n{'=' * 60}\nRunning: {script.name}\n{'=' * 60}") subprocess.run([sys.executable, str(script), "-N", "1"]) + if __name__ == "__main__": - main() \ No newline at end of file + main() From 33d1db8e9bd9b3d8876afd58a8a96886e3d3fe54 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 9 Jan 2026 10:48:20 +0000 Subject: [PATCH 062/109] [Path] Bind method reverse. --- src/pyhpp/core/path.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pyhpp/core/path.cc b/src/pyhpp/core/path.cc index 454739e8..66d55d77 100644 --- a/src/pyhpp/core/path.cc +++ b/src/pyhpp/core/path.cc @@ -160,6 +160,7 @@ void exposePath() { .def("timeRange", static_cast(&Path::timeRange), return_internal_reference<>()) + .def("reverse", &Path::reverse) .PYHPP_DEFINE_METHOD_INTERNAL_REF(Path, paramRange) .PYHPP_DEFINE_METHOD(Path, length) .PYHPP_DEFINE_METHOD(Path, initial) From fa88845384e32f1f048a700dd5364f76dbdf8812 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 9 Jan 2026 10:48:47 +0000 Subject: [PATCH 063/109] [TransitionPlanner] Bind method addPathOptimizer. --- src/pyhpp/manipulation/path-planner.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 6d6e453f..45c42e6b 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -247,7 +247,9 @@ void exposePathPlanners() { &TransitionPlanner::setReedsAndSheppSteeringMethod) .def("pathProjector", &TransitionPlanner::pathProjector) .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) - .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer); + .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer, + "Add a path optimizer\n\n Note: the input path optimizer should have been constructed" + " with\n the inner problem of this class."); boost::python::class_>( From 97a48578fb1c96ac9cf8d3f5381eed1feb81e630 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 9 Jan 2026 10:49:29 +0000 Subject: [PATCH 064/109] [core] Bind SplineGradientBased_bezier optimizers. --- src/pyhpp/core/path-optimizer.cc | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index f188d0b4..85d50be0 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -86,6 +87,28 @@ void exposePathOptimizer() { no_init) .def("__init__", make_constructor(&pathOptimization::RSTimeParameterization::create)); + + class_, + std::shared_ptr >, + bases, boost::noncopyable>("SplineGradientBased_bezier1", no_init) + .def("__init__", + make_constructor(&pathOptimization::SplineGradientBased::create) + ); + + class_, + std::shared_ptr >, + bases, boost::noncopyable>("SplineGradientBased_bezier3", no_init) + .def("__init__", + make_constructor(&pathOptimization::SplineGradientBased::create) + ); + + class_, + std::shared_ptr >, + bases, boost::noncopyable>("SplineGradientBased_bezier5", no_init) + .def("__init__", + make_constructor(&pathOptimization::SplineGradientBased::create) + ); + } } // namespace core } // namespace pyhpp From 59678f47ef174842a0f3e84e26aa9fe5ccc3a5c2 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 9 Jan 2026 10:51:51 +0000 Subject: [PATCH 065/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/path-optimizer.cc | 34 ++++++++++++++------------ src/pyhpp/manipulation/path-planner.cc | 3 ++- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index 85d50be0..1c153d49 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -89,26 +89,28 @@ void exposePathOptimizer() { make_constructor(&pathOptimization::RSTimeParameterization::create)); class_, - std::shared_ptr >, - bases, boost::noncopyable>("SplineGradientBased_bezier1", no_init) - .def("__init__", - make_constructor(&pathOptimization::SplineGradientBased::create) - ); + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier1", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 1>::create)); class_, - std::shared_ptr >, - bases, boost::noncopyable>("SplineGradientBased_bezier3", no_init) - .def("__init__", - make_constructor(&pathOptimization::SplineGradientBased::create) - ); + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier3", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 3>::create)); class_, - std::shared_ptr >, - bases, boost::noncopyable>("SplineGradientBased_bezier5", no_init) - .def("__init__", - make_constructor(&pathOptimization::SplineGradientBased::create) - ); - + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier5", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 5>::create)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 45c42e6b..382f9eff 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -248,7 +248,8 @@ void exposePathPlanners() { .def("pathProjector", &TransitionPlanner::pathProjector) .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer, - "Add a path optimizer\n\n Note: the input path optimizer should have been constructed" + "Add a path optimizer\n\n Note: the input path optimizer should " + "have been constructed" " with\n the inner problem of this class."); boost::python::class_ Date: Mon, 12 Jan 2026 17:12:19 +0000 Subject: [PATCH 066/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.10 → v0.14.11](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.10...v0.14.11) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5ad1650b..304fa8f0 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.10 + rev: v0.14.11 hooks: - id: ruff args: From 07121935928956b699abfda6a0c4f4337c96033e Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" Date: Tue, 13 Jan 2026 16:28:41 +0000 Subject: [PATCH 067/109] flake.lock: Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Flake lock file updates: • Updated input 'gepetto': 'github:gepetto/nix/a6b0691' (2025-12-13) → 'github:gepetto/nix/d1226cd' (2026-01-11) • Updated input 'gepetto/flake-parts': 'github:hercules-ci/flake-parts/2cccadc' (2025-11-21) → 'github:hercules-ci/flake-parts/250481a' (2026-01-05) • Updated input 'gepetto/flake-parts/nixpkgs-lib': 'github:nix-community/nixpkgs.lib/719359f' (2025-10-29) → 'github:nix-community/nixpkgs.lib/2075416' (2025-12-14) • Updated input 'gepetto/nix-ros-overlay': 'github:lopsided98/nix-ros-overlay/804d2ee' (2025-12-02) → 'github:lopsided98/nix-ros-overlay/cbd23fc' (2026-01-10) • Updated input 'gepetto/src-agimus-controller': 'github:agimus-project/agimus_controller/95aac68' (2025-12-04) → 'github:agimus-project/agimus_controller/5ef41cd' (2025-12-17) • Updated input 'gepetto/src-agimus-msgs': 'github:agimus-project/agimus_msgs/2861569' (2025-11-24) → 'github:agimus-project/agimus_msgs/9f3bcea' (2026-01-09) • Updated input 'gepetto/src-franka-description': 'github:agimus-project/franka_description/3ae2451' (2025-11-28) → 'github:agimus-project/franka_description/2bdc8da' (2025-12-17) • Updated input 'gepetto/system-manager': 'github:numtide/system-manager/a0343ab' (2025-11-27) → 'github:numtide/system-manager/2646bbc' (2026-01-10) • Updated input 'gepetto/treefmt-nix': 'github:numtide/treefmt-nix/5b4ee75' (2025-11-12) → 'github:numtide/treefmt-nix/0c445aa' (2026-01-10) --- flake.lock | 54 +++++++++++++++++++++++++++--------------------------- 1 file changed, 27 insertions(+), 27 deletions(-) diff --git a/flake.lock b/flake.lock index 990049a1..f0240a3d 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1763759067, - "narHash": "sha256-LlLt2Jo/gMNYAwOgdRQBrsRoOz7BPRkzvNaI/fzXi2Q=", + "lastModified": 1767609335, + "narHash": "sha256-feveD98mQpptwrAEggBQKJTYbvwwglSbOv53uCfH9PY=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "2cccadc7357c0ba201788ae99c4dfa90728ef5e0", + "rev": "250481aafeb741edfe23d29195671c19b36b6dca", "type": "github" }, "original": { @@ -61,11 +61,11 @@ "treefmt-nix": "treefmt-nix" }, "locked": { - "lastModified": 1765613884, - "narHash": "sha256-/kuN0F5qciGkWMNlroG8ecq91DEDNy75gYe0lTueqRc=", + "lastModified": 1768097331, + "narHash": "sha256-RRY9GSpSiWfzo7P/v/R9xLJPdsvUzQdpuNq5gwAdFsM=", "owner": "gepetto", "repo": "nix", - "rev": "a6b0691748591a6ae5e07dbbc5abd94ab97cdfa1", + "rev": "d1226cdb8ae6e489c28eb2d39dacf7fc905fce95", "type": "github" }, "original": { @@ -80,11 +80,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1764665924, - "narHash": "sha256-ncGeWPiK6BawJdDZc0ugfeG2yPIGAYsdOFwpXBZ193k=", + "lastModified": 1768042480, + "narHash": "sha256-MY81n7N8mn6We+PxGBq9eJJweg/j0vgEDhKLp9gs4IM=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "804d2ee3fd89898fb7b1a02952aa5682e9725ce4", + "rev": "cbd23fc7766de218051c79d4df6848303a915ee0", "type": "github" }, "original": { @@ -133,11 +133,11 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1761765539, - "narHash": "sha256-b0yj6kfvO8ApcSE+QmA6mUfu8IYG6/uU28OFn4PaC8M=", + "lastModified": 1765674936, + "narHash": "sha256-k00uTP4JNfmejrCLJOwdObYC9jHRrr/5M/a/8L2EIdo=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "719359f4562934ae99f5443f20aa06c2ffff91fc", + "rev": "2075416fcb47225d9b68ac469a5c4801a9c4dd85", "type": "github" }, "original": { @@ -174,11 +174,11 @@ "src-agimus-controller": { "flake": false, "locked": { - "lastModified": 1764844076, - "narHash": "sha256-GFsuPVDy3TBwTRkDCMn+VcTYN7JQ5zlpSFfxUgAmnkc=", + "lastModified": 1765937017, + "narHash": "sha256-aAURRGwKom7pssu48I1io8giykUMNftNGTv72YIH5n8=", "owner": "agimus-project", "repo": "agimus_controller", - "rev": "95aac68307eab0ec1a85b255b33e79868d53914c", + "rev": "5ef41cd0a64feeaafdb7bdebe0f0308fa07bf734", "type": "github" }, "original": { @@ -190,11 +190,11 @@ "src-agimus-msgs": { "flake": false, "locked": { - "lastModified": 1764011280, - "narHash": "sha256-wG0kJUMsDi3IELv5H9mEsizRyssMnvEbTM6LdEqqLvQ=", + "lastModified": 1767943618, + "narHash": "sha256-zOaDYbuG46SzNCjBibzIgsIkZkPpg7wjxTQ1ETVVe+g=", "owner": "agimus-project", "repo": "agimus_msgs", - "rev": "2861569b8f8ef82a932c581a7dbfdb652dc6247b", + "rev": "9f3bcea94b5824b06530bc006b9afd2590a37db0", "type": "github" }, "original": { @@ -206,11 +206,11 @@ "src-franka-description": { "flake": false, "locked": { - "lastModified": 1764344366, - "narHash": "sha256-uk0fh/BoMdhNyS3ssV8Ebn+FYMSXjaKGepTxp6zibt8=", + "lastModified": 1765937047, + "narHash": "sha256-mTIQJnQLZN3UMyPq7Btu6SYWv8JuXMSjdkzXbRdGEh4=", "owner": "agimus-project", "repo": "franka_description", - "rev": "3ae24519c5ec3a999b0851a4243181ad245a6fa6", + "rev": "2bdc8da187040d2e85da9d5d302698ff30de3d11", "type": "github" }, "original": { @@ -261,11 +261,11 @@ ] }, "locked": { - "lastModified": 1764233885, - "narHash": "sha256-m6etiBLSMajunnbUvEI1Pc/jNu4naImalgQnctVJ1/k=", + "lastModified": 1768082042, + "narHash": "sha256-6HjFn+tJi2C4xByHD0CZwPSnRHgKZNVqVCYRDdVHvSA=", "owner": "numtide", "repo": "system-manager", - "rev": "a0343ab10763fbbfcf8d3b69c8341c18b1b1f215", + "rev": "2646bbcb730fbd2a38d3d20948373ec5cd6271ef", "type": "github" }, "original": { @@ -297,11 +297,11 @@ ] }, "locked": { - "lastModified": 1762938485, - "narHash": "sha256-AlEObg0syDl+Spi4LsZIBrjw+snSVU4T8MOeuZJUJjM=", + "lastModified": 1768031762, + "narHash": "sha256-b2gJDJfi+TbA7Hu2sKip+1mWqya0GJaWrrXQjpbOVTU=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "5b4ee75aeefd1e2d5a1cc43cf6ba65eba75e83e4", + "rev": "0c445aa21b01fd1d4bb58927f7b268568af87b20", "type": "github" }, "original": { From 795692c5e29ec3bffb83031e583e571b0819b5a9 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 9 Jan 2026 07:35:02 +0100 Subject: [PATCH 068/109] 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 b7ffc82c..0337bcb5 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 62981cf19d745c67cf67d1ec80c3576dfe59adc2 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 09:41:34 +0100 Subject: [PATCH 069/109] 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 0f8bf5da..cc9010bd 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 7a31888fa8f6233d80b655a6459c45618ffcf48a Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 09:45:07 +0100 Subject: [PATCH 070/109] 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 c2b38627..11616a3d 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 cc9010bd..95c8acee 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 be168d4b..bd0d9274 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 b05f2ac7..00000000 --- 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 b434bb7a..00000000 --- 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 f7695ebf..00000000 --- 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 2a9bb924..00000000 --- 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 10d18e7d..00000000 --- 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 495d6680..00000000 --- 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 0465e9ca..00000000 --- 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 b02d6de4..00000000 --- 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 100535fa..00000000 --- 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 48079382..00000000 --- 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 d9d1b091..00000000 --- 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 35463043..00000000 --- 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 c9ad45da..00000000 --- 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 a6190d10..00000000 --- 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 f76c53a6..00000000 --- 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 c580b353..00000000 --- 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 26f1cbe4..00000000 --- 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 e69de29b..00000000 diff --git a/tests/integration/benchmark_utils.py b/tests/integration/benchmark_utils.py new file mode 100644 index 00000000..b331d72d --- /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 ee9b2259..789b551a 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 00000000..b4508b8f --- /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 b5bcfcbf..9e18d5c8 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 00000000..8122c5b8 --- /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 07ea8f8f..1df22205 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 debb2704..9efa9ec0 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 1a0875ae..00000000 --- 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 9cda9a76..00000000 --- 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 4e88bf04..00000000 --- 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 929b63b3..00000000 --- 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 58cca6f8..00000000 --- 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 287f18a0..00000000 --- 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 c5b465d6..00000000 --- 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 6bf61717..00000000 --- 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 081b7665..00000000 --- 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 a280fb04..00000000 --- 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 70d11782..00000000 --- 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 00000000..b9e59ae1 --- /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 00000000..293535c7 --- /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 00000000..3513deec --- /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 00000000..b0130f07 --- /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 00000000..92ec41ce --- /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 00000000..efbb15bc --- /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 00000000..c61ac168 --- /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 00000000..d4de5776 --- /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 00000000..b184e58b --- /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 00000000..2cb45154 --- /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 00000000..10c34aad --- /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 00000000..a5744c93 --- /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 00000000..d1fb11b7 --- /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 00000000..a65c4c2d --- /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 00000000..c6b35ba4 --- /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 00000000..231fbdf5 --- /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 00000000..394c054a --- /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 00000000..5d350b3f --- /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 00000000..9c3d631d --- /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 4e1c4024737a8599633828b90f3fc0240ab8cb73 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 071/109] [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 11616a3d..4a74eed2 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 95c8acee..ab8c7427 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 bd0d9274..678a5f29 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 b331d72d..670b45c4 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 789b551a..c2f55b6b 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 b4508b8f..9897b49f 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 9e18d5c8..cc481f7e 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 8122c5b8..84ee36b0 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 3513deec..fdfd4cf3 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 b0130f07..420805ff 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 92ec41ce..456f1afd 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 efbb15bc..d823798e 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 c61ac168..6259ca4e 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 d4de5776..c1e2073c 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 b184e58b..2e8821c1 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 2cb45154..9fa863ed 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 10c34aad..588f8834 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 a5744c93..5acc1fe2 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 d1fb11b7..d92d1cef 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 a65c4c2d..e0bfa714 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 c6b35ba4..2d9d020a 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 231fbdf5..a56d3165 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 394c054a..444075f4 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 5d350b3f..77948c8f 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 9c3d631d..0842c354 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 936a40e08dbb3a4b71eb61bac5da4fb54bc51128 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 14 Jan 2026 10:24:23 +0100 Subject: [PATCH 072/109] 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 ab8c7427..4ed45183 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 74b1180e4436968f0ea68249992ef70cff13a3c3 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 073/109] [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 4ed45183..cc9010bd 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 From 1af352ac2f6b976c799caabc75c5571304ecf46d Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 16 Jan 2026 08:35:34 +0100 Subject: [PATCH 074/109] Add path serialization --- src/pyhpp/core/path/vector.cc | 37 ++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 6388da39..89c83d10 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -30,9 +30,12 @@ #include #include #include +#include #include #include #include +#include +#include using namespace boost::python; @@ -41,6 +44,31 @@ namespace core { namespace path { using namespace hpp::core; +void savePathVector(PathVectorPtr_t pathVector, const std::string& filename) { + if (!pathVector) { + throw std::invalid_argument("Cannot save null PathVector"); + } + std::ofstream ofs(filename, std::ios::binary); + if (!ofs.is_open()) { + throw std::runtime_error("Failed to open file for writing: " + filename); + } + hpp::serialization::binary_oarchive ar(ofs); + ar.initialize(); + ar << hpp::serialization::make_nvp("pathVector", pathVector); +} + +PathVectorPtr_t loadPathVector(const std::string& filename) { + std::ifstream ifs(filename, std::ios::binary); + if (!ifs.is_open()) { + throw std::runtime_error("Failed to open file for reading: " + filename); + } + hpp::serialization::binary_iarchive ar(ifs); + ar.initialize(); + PathVectorPtr_t pathVector; + ar >> hpp::serialization::make_nvp("pathVector", pathVector); + return pathVector; +} + void exposeVector() { class_, boost::noncopyable>("Vector", no_init) @@ -52,7 +80,14 @@ void exposeVector() { .PYHPP_DEFINE_METHOD(PathVector, rankAtParam) .PYHPP_DEFINE_METHOD(PathVector, appendPath) .PYHPP_DEFINE_METHOD(PathVector, concatenate) - .PYHPP_DEFINE_METHOD(PathVector, flatten); + .PYHPP_DEFINE_METHOD(PathVector, flatten) + + // Serialization methods (binary format) + .def("save", &savePathVector, + "Save PathVector to file (binary format)") + .def("load", &loadPathVector, + "Load PathVector from file (binary format)") + .staticmethod("load"); class_("Vectors").def( vector_indexing_suite()); From d83e21efcb88e1ad3bb232e2fcadb61117807aec Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 16 Jan 2026 08:54:45 +0100 Subject: [PATCH 075/109] Fix auto generating pyhpp doc based on doxygen --- doc/doxygen_xml_parser.py | 12 +++-- src/pyhpp/constraints/by-substitution.cc | 9 +++- .../constraints/explicit-constraint-set.cc | 5 +- src/pyhpp/constraints/implicit.cc | 21 ++++++-- src/pyhpp/constraints/iterative-solver.cc | 5 +- src/pyhpp/core/constraint.cc | 41 +++++++++++---- src/pyhpp/core/node.cc | 21 +++++--- src/pyhpp/core/path.cc | 32 +++++++----- src/pyhpp/core/roadmap.cc | 51 ++++++++++--------- 9 files changed, 129 insertions(+), 68 deletions(-) diff --git a/doc/doxygen_xml_parser.py b/doc/doxygen_xml_parser.py index 7079c793..7d374531 100644 --- a/doc/doxygen_xml_parser.py +++ b/doc/doxygen_xml_parser.py @@ -22,14 +22,16 @@ def classDoc(self, name): class ClassDoc: def __init__(self, filename): self.tree = etree.parse(filename) - self.compound = self.tree.find("/compounddef") + self.compound = self.tree.find("./compounddef") self.classname = self.compound.find("compoundname").text.strip() @staticmethod def _getDoc(el): b = el.find("briefdescription") d = el.find("detaileddescription") - return etree.tostring(b, method="text").strip(), d.text.strip() + brief = etree.tostring(b, method="text", encoding="unicode").strip() + detailed = d.text.strip() if d.text else "" + return brief, detailed def _getMember(self, sectionKind, memberDefKind, name): # return self.compound.xpath ("sectiondef[@kind='" + sectionKind @@ -76,14 +78,16 @@ def getClassMethodDoc(self, methodname): ] else: args = [] - args += [el.text.strip() for el in member.xpath("param/declname")] + args += [el.text.strip() for el in member.xpath("param/declname") if el.text] for parameters in dd.xpath("para/parameterlist/parameteritem"): pargs = [ el.text.strip() for el in parameters.find("parameternamelist").findall("parametername") + if el.text ] pns = " ".join(pargs) - pd = parameters.find("parameterdescription").find("para").text.strip() + para_el = parameters.find("parameterdescription").find("para") + pd = para_el.text.strip() if para_el is not None and para_el.text else "" d += "\n:param " + pns + ":" + pd return b, d, args diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 4a74eed2..55a7d882 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -52,15 +54,18 @@ void exposeBySubstitution() { .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) .value("SUCCESS", HierarchicalIterative::SUCCESS); + // DocClass(solver::BySubstitution) class_ >( "BySubstitution", init()) .def("explicitConstraintSetHasChanged", - &BySubstitution::explicitConstraintSetHasChanged) + &BySubstitution::explicitConstraintSetHasChanged, + DocClassMethod(explicitConstraintSetHasChanged)) .def("solve", &BySubstitution_solve) .def("explicitConstraintSet", static_cast( &BySubstitution::explicitConstraintSet), - return_internal_reference<>()) + return_internal_reference<>(), + DocClassMethod(explicitConstraintSet)) .add_property("errorThreshold", static_cast( &BySubstitution::errorThreshold), diff --git a/src/pyhpp/constraints/explicit-constraint-set.cc b/src/pyhpp/constraints/explicit-constraint-set.cc index 26578a83..032577ea 100644 --- a/src/pyhpp/constraints/explicit-constraint-set.cc +++ b/src/pyhpp/constraints/explicit-constraint-set.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -39,10 +41,11 @@ namespace constraints { using namespace hpp::constraints; void exposeExplicitConstraintSet() { + // DocClass(ExplicitConstraintSet) class_("ExplicitConstraintSet", init()) .def("__str__", &to_str) - .def("add", &ExplicitConstraintSet::add); + .def("add", &ExplicitConstraintSet::add, DocClassMethod(add)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/implicit.cc b/src/pyhpp/constraints/implicit.cc index fca3a0dc..72aba304 100644 --- a/src/pyhpp/constraints/implicit.cc +++ b/src/pyhpp/constraints/implicit.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -53,13 +55,22 @@ void exposeImplicit() { .value("EqualToZero", EqualToZero) .value("Superior", Superior) .value("Inferior", Inferior); + // DocClass(Implicit) class_("Implicit", no_init) .def("__init__", make_constructor(&Implicit::create)) - .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(Implicit, comparisonType, - const ComparisonTypes_t&) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Implicit, function) - .def("parameterSize", &Implicit::parameterSize) - .def("rightHandSideSize", &Implicit::rightHandSideSize) + .def("comparisonType", + static_cast( + &Implicit::comparisonType), + return_internal_reference<>()) + .def("comparisonType", + static_cast( + &Implicit::comparisonType)) + .def("function", &Implicit::function, return_internal_reference<>(), + DocClassMethod(function)) + .def("parameterSize", &Implicit::parameterSize, + DocClassMethod(parameterSize)) + .def("rightHandSideSize", &Implicit::rightHandSideSize, + DocClassMethod(rightHandSideSize)) .def("getFunctionOutputSize", &getFunctionOutputSize) .staticmethod("getFunctionOutputSize"); } diff --git a/src/pyhpp/constraints/iterative-solver.cc b/src/pyhpp/constraints/iterative-solver.cc index 6325ea48..f9e01d4c 100644 --- a/src/pyhpp/constraints/iterative-solver.cc +++ b/src/pyhpp/constraints/iterative-solver.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -48,10 +50,11 @@ void exposeHierarchicalIterativeSolver() { class_("ComparisonTypes") .def(vector_indexing_suite()); + // DocClass(solver::HierarchicalIterative) class_("HierarchicalIterative", init()) .def("__str__", &to_str) - .def("add", &HierarchicalIterative::add) + .def("add", &HierarchicalIterative::add, DocClassMethod(add)) .add_property( "errorThreshold", diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 60ec1d9c..86fc8e13 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -85,41 +87,58 @@ static void setRightHandSideOfConstraint( } void exposeConstraint() { + // DocClass(Constraint) class_("Constraint", no_init) .def("__str__", &to_str_from_operator) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Constraint, name) - .PYHPP_DEFINE_METHOD(CWrapper, apply) + .def("name", &Constraint::name, return_internal_reference<>(), + DocClassMethod(name)) + .def("apply", &CWrapper::apply) .def("isSatisfied", &CWrapper::isSatisfied1) .def("isSatisfied", &CWrapper::isSatisfied2) - .PYHPP_DEFINE_METHOD(CWrapper, copy); + .def("copy", &CWrapper::copy); + // DocClass(ConstraintSet) class_ >("ConstraintSet", no_init) .def("__init__", make_constructor(&createConstraintSet)) - .PYHPP_DEFINE_METHOD(ConstraintSet, addConstraint) - .PYHPP_DEFINE_METHOD(ConstraintSet, configProjector); + .def("addConstraint", &ConstraintSet::addConstraint, + DocClassMethod(addConstraint)) + .def("configProjector", &ConstraintSet::configProjector, + DocClassMethod(configProjector)); + // DocClass(ConfigProjector) class_ >("ConfigProjector", no_init) .def("__init__", make_constructor(&createConfigProjector)) .def("solver", static_cast( &ConfigProjector::solver), - return_internal_reference<>()) - .def("add", &ConfigProjector::add) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, lastIsOptional, bool) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, maxIterations, size_type) + return_internal_reference<>(), DocClassMethod(solver)) + .def("add", &ConfigProjector::add, DocClassMethod(add)) + .def("lastIsOptional", + static_cast( + &ConfigProjector::lastIsOptional)) + .def("lastIsOptional", + static_cast( + &ConfigProjector::lastIsOptional)) + .def("maxIterations", + static_cast( + &ConfigProjector::maxIterations)) + .def("maxIterations", + static_cast( + &ConfigProjector::maxIterations)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) - .PYHPP_DEFINE_METHOD(ConfigProjector, residualError) + .def("residualError", &ConfigProjector::residualError, + DocClassMethod(residualError)) .def("setRightHandSideFromConfig", &rightHandSideFromConfig) .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint) .def("sigma", &ConfigProjector::sigma, - return_value_policy()); + return_value_policy(), DocClassMethod(sigma)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/node.cc b/src/pyhpp/core/node.cc index d702fe20..1ffc5c56 100644 --- a/src/pyhpp/core/node.cc +++ b/src/pyhpp/core/node.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -42,23 +44,26 @@ namespace core { using namespace hpp::core; void exposeNode() { + // DocClass(Node) class_, boost::noncopyable>("Node", no_init) .def(init()) .def(init()) - .PYHPP_DEFINE_METHOD(Node, addOutEdge) - .PYHPP_DEFINE_METHOD(Node, addInEdge) + .def("addOutEdge", &Node::addOutEdge, DocClassMethod(addOutEdge)) + .def("addInEdge", &Node::addInEdge, DocClassMethod(addInEdge)) .def("connectedComponent", static_cast( &Node::connectedComponent)) .def("connectedComponent", static_cast( &Node::connectedComponent)) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, outEdges) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, inEdges) - .PYHPP_DEFINE_METHOD(Node, isOutNeighbor) - .PYHPP_DEFINE_METHOD(Node, isInNeighbor) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, configuration) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, print); + .def("outEdges", &Node::outEdges, return_internal_reference<>(), + DocClassMethod(outEdges)) + .def("inEdges", &Node::inEdges, return_internal_reference<>(), + DocClassMethod(inEdges)) + .def("isOutNeighbor", &Node::isOutNeighbor, DocClassMethod(isOutNeighbor)) + .def("isInNeighbor", &Node::isInNeighbor, DocClassMethod(isInNeighbor)) + .def("configuration", &Node::configuration, return_internal_reference<>(), + DocClassMethod(configuration)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/path.cc b/src/pyhpp/core/path.cc index 66d55d77..9892e68c 100644 --- a/src/pyhpp/core/path.cc +++ b/src/pyhpp/core/path.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -141,6 +143,7 @@ struct PathWrap : PathWrapper, wrapper { }; void exposePath() { + // DocClass(Path) class_, boost::noncopyable>("Path", no_init) .def("__str__", &to_str_from_operator) @@ -148,26 +151,29 @@ void exposePath() { .def("__call__", &PathWrap::py_call2) .def("eval", &PathWrap::py_call1) .def("eval", &PathWrap::py_call2) - .PYHPP_DEFINE_METHOD(PathWrap, derivative) + .def("derivative", &PathWrap::derivative) - .def("copy", static_cast(&Path::copy)) + .def("copy", static_cast(&Path::copy), + DocClassMethod(copy)) .def("extract", static_cast(&Path::extract)) + const>(&Path::extract), + DocClassMethod(extract)) .def("extract", static_cast( &Path::extract)) - // .PYHPP_DEFINE_METHOD (Path, timeRange) .def("timeRange", static_cast(&Path::timeRange), - return_internal_reference<>()) - .def("reverse", &Path::reverse) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Path, paramRange) - .PYHPP_DEFINE_METHOD(Path, length) - .PYHPP_DEFINE_METHOD(Path, initial) - .PYHPP_DEFINE_METHOD(Path, end) - .PYHPP_DEFINE_METHOD(PathWrap, constraints) - .PYHPP_DEFINE_METHOD(Path, outputSize) - .PYHPP_DEFINE_METHOD(Path, outputDerivativeSize); + return_internal_reference<>(), DocClassMethod(timeRange)) + .def("reverse", &Path::reverse, DocClassMethod(reverse)) + .def("paramRange", &Path::paramRange, return_internal_reference<>(), + DocClassMethod(paramRange)) + .def("length", &Path::length, DocClassMethod(length)) + .def("initial", &Path::initial, DocClassMethod(initial)) + .def("end", &Path::end, DocClassMethod(end)) + .def("constraints", &PathWrap::constraints) + .def("outputSize", &Path::outputSize, DocClassMethod(outputSize)) + .def("outputDerivativeSize", &Path::outputDerivativeSize, + DocClassMethod(outputDerivativeSize)); class_, hpp::shared_ptr, boost::noncopyable>( "PathWrap", no_init) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index e7036c04..3e846343 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -33,6 +33,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -218,49 +220,52 @@ struct RWrapper { }; void exposeRoadmap() { + // DocClass(Roadmap) class_("Roadmap", no_init) .def("__init__", make_constructor(&Roadmap::create)) .def("__str__", &to_str) - .PYHPP_DEFINE_METHOD(Roadmap, clear) - .PYHPP_DEFINE_METHOD1(RWrapper, addNode, - return_value_policy()) + .def("clear", &Roadmap::clear, DocClassMethod(clear)) + .def("addNode", &RWrapper::addNode, + return_value_policy()) .def("nearestNode", &RWrapper::nearestNode1) .def("nearestNode", &RWrapper::nearestNode2) .def("nearestNode", &RWrapper::nearestNode3) .def("nearestNode", &RWrapper::nearestNode4) .def("nearestNodes", &RWrapper::nearestNodes1) .def("nearestNodes", &RWrapper::nearestNodes2) - .PYHPP_DEFINE_METHOD(Roadmap, nodesWithinBall) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdges, - return_value_policy()) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdge, - return_value_policy()) + .def("nodesWithinBall", &Roadmap::nodesWithinBall, + DocClassMethod(nodesWithinBall)) + .def("addNodeAndEdges", &RWrapper::addNodeAndEdges, + return_value_policy()) + .def("addNodeAndEdge", &RWrapper::addNodeAndEdge, + return_value_policy()) .def("addEdge", &RWrapper::addEdge) .def("addEdge", &RWrapper::addEdge2) - .PYHPP_DEFINE_METHOD(Roadmap, addEdges) + .def("addEdges", &Roadmap::addEdges, DocClassMethod(addEdges)) .def("merge", - static_cast(&Roadmap::merge)) - .PYHPP_DEFINE_METHOD(Roadmap, insertPathVector) - .PYHPP_DEFINE_METHOD1(Roadmap, addGoalNode, - return_value_policy()) - .PYHPP_DEFINE_METHOD(Roadmap, resetGoalNodes) - .PYHPP_DEFINE_METHOD(Roadmap, pathExists) + static_cast(&Roadmap::merge), + DocClassMethod(merge)) + .def("insertPathVector", &Roadmap::insertPathVector, + DocClassMethod(insertPathVector)) + .def("addGoalNode", &Roadmap::addGoalNode, + return_value_policy(), + DocClassMethod(addGoalNode)) + .def("resetGoalNodes", &Roadmap::resetGoalNodes, + DocClassMethod(resetGoalNodes)) + .def("pathExists", &Roadmap::pathExists, DocClassMethod(pathExists)) .def("nodes", &RWrapper::nodes) .def("nodesConnectedComponent", &RWrapper::nodesConnectedComponent) .def("initNode", &RWrapper::initNode1) .def("initNode", &RWrapper::initNode2, return_value_policy()) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, goalNodes) + .def("goalNodes", &Roadmap::goalNodes, return_internal_reference<>(), + DocClassMethod(goalNodes)) .def("connectedComponents", &RWrapper::connectedComponents) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, distance) + .def("distance", &Roadmap::distance, return_internal_reference<>(), + DocClassMethod(distance)) .def("numberConnectedComponents", &RWrapper::numberConnectedComponents) .def("getConnectedComponent", &RWrapper::getConnectedComponent) - .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode) - // .def("cost", &RWrapper::cost1) - // .def("cost", &RWrapper::cost2, - // return_value_policy()) - - ; + .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode); } } // namespace core } // namespace pyhpp From 78ad5366d8c8469a22b48ef13b9cba58b788af7d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 16 Jan 2026 08:03:22 +0000 Subject: [PATCH 076/109] [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 +-- src/pyhpp/core/constraint.cc | 20 ++++++++------------ src/pyhpp/core/path/vector.cc | 10 ++++------ 3 files changed, 13 insertions(+), 20 deletions(-) diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 55a7d882..ee7ef79a 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -64,8 +64,7 @@ void exposeBySubstitution() { .def("explicitConstraintSet", static_cast( &BySubstitution::explicitConstraintSet), - return_internal_reference<>(), - DocClassMethod(explicitConstraintSet)) + return_internal_reference<>(), DocClassMethod(explicitConstraintSet)) .add_property("errorThreshold", static_cast( &BySubstitution::errorThreshold), diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 86fc8e13..4e76df84 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -115,18 +115,14 @@ void exposeConstraint() { &ConfigProjector::solver), return_internal_reference<>(), DocClassMethod(solver)) .def("add", &ConfigProjector::add, DocClassMethod(add)) - .def("lastIsOptional", - static_cast( - &ConfigProjector::lastIsOptional)) - .def("lastIsOptional", - static_cast( - &ConfigProjector::lastIsOptional)) - .def("maxIterations", - static_cast( - &ConfigProjector::maxIterations)) - .def("maxIterations", - static_cast( - &ConfigProjector::maxIterations)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 89c83d10..d64d5ea2 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -29,12 +29,12 @@ #include #include +#include #include -#include #include +#include #include #include -#include #include using namespace boost::python; @@ -83,10 +83,8 @@ void exposeVector() { .PYHPP_DEFINE_METHOD(PathVector, flatten) // Serialization methods (binary format) - .def("save", &savePathVector, - "Save PathVector to file (binary format)") - .def("load", &loadPathVector, - "Load PathVector from file (binary format)") + .def("save", &savePathVector, "Save PathVector to file (binary format)") + .def("load", &loadPathVector, "Load PathVector from file (binary format)") .staticmethod("load"); class_("Vectors").def( From a5557a0c57ada33e476086c727ba1a54fb546f33 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 19 Jan 2026 17:17:23 +0000 Subject: [PATCH 077/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.11 → v0.14.13](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.11...v0.14.13) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 304fa8f0..cc1646b0 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.11 + rev: v0.14.13 hooks: - id: ruff args: From fdffbdc7be860b099cd1fb22bf944c958b22b905 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 16 Jan 2026 08:35:34 +0100 Subject: [PATCH 078/109] Add path serialization --- src/pyhpp/core/path/vector.cc | 37 ++++++++++++++++++++++++++++++++++- 1 file changed, 36 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 6388da39..89c83d10 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -30,9 +30,12 @@ #include #include #include +#include #include #include #include +#include +#include using namespace boost::python; @@ -41,6 +44,31 @@ namespace core { namespace path { using namespace hpp::core; +void savePathVector(PathVectorPtr_t pathVector, const std::string& filename) { + if (!pathVector) { + throw std::invalid_argument("Cannot save null PathVector"); + } + std::ofstream ofs(filename, std::ios::binary); + if (!ofs.is_open()) { + throw std::runtime_error("Failed to open file for writing: " + filename); + } + hpp::serialization::binary_oarchive ar(ofs); + ar.initialize(); + ar << hpp::serialization::make_nvp("pathVector", pathVector); +} + +PathVectorPtr_t loadPathVector(const std::string& filename) { + std::ifstream ifs(filename, std::ios::binary); + if (!ifs.is_open()) { + throw std::runtime_error("Failed to open file for reading: " + filename); + } + hpp::serialization::binary_iarchive ar(ifs); + ar.initialize(); + PathVectorPtr_t pathVector; + ar >> hpp::serialization::make_nvp("pathVector", pathVector); + return pathVector; +} + void exposeVector() { class_, boost::noncopyable>("Vector", no_init) @@ -52,7 +80,14 @@ void exposeVector() { .PYHPP_DEFINE_METHOD(PathVector, rankAtParam) .PYHPP_DEFINE_METHOD(PathVector, appendPath) .PYHPP_DEFINE_METHOD(PathVector, concatenate) - .PYHPP_DEFINE_METHOD(PathVector, flatten); + .PYHPP_DEFINE_METHOD(PathVector, flatten) + + // Serialization methods (binary format) + .def("save", &savePathVector, + "Save PathVector to file (binary format)") + .def("load", &loadPathVector, + "Load PathVector from file (binary format)") + .staticmethod("load"); class_("Vectors").def( vector_indexing_suite()); From b630cef17fb4adafe32666569268d1083f12c2b2 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 16 Jan 2026 08:54:45 +0100 Subject: [PATCH 079/109] Fix auto generating pyhpp doc based on doxygen --- doc/doxygen_xml_parser.py | 12 +++-- src/pyhpp/constraints/by-substitution.cc | 9 +++- .../constraints/explicit-constraint-set.cc | 5 +- src/pyhpp/constraints/implicit.cc | 21 ++++++-- src/pyhpp/constraints/iterative-solver.cc | 5 +- src/pyhpp/core/constraint.cc | 41 +++++++++++---- src/pyhpp/core/node.cc | 21 +++++--- src/pyhpp/core/path.cc | 32 +++++++----- src/pyhpp/core/roadmap.cc | 51 ++++++++++--------- 9 files changed, 129 insertions(+), 68 deletions(-) diff --git a/doc/doxygen_xml_parser.py b/doc/doxygen_xml_parser.py index 7079c793..7d374531 100644 --- a/doc/doxygen_xml_parser.py +++ b/doc/doxygen_xml_parser.py @@ -22,14 +22,16 @@ def classDoc(self, name): class ClassDoc: def __init__(self, filename): self.tree = etree.parse(filename) - self.compound = self.tree.find("/compounddef") + self.compound = self.tree.find("./compounddef") self.classname = self.compound.find("compoundname").text.strip() @staticmethod def _getDoc(el): b = el.find("briefdescription") d = el.find("detaileddescription") - return etree.tostring(b, method="text").strip(), d.text.strip() + brief = etree.tostring(b, method="text", encoding="unicode").strip() + detailed = d.text.strip() if d.text else "" + return brief, detailed def _getMember(self, sectionKind, memberDefKind, name): # return self.compound.xpath ("sectiondef[@kind='" + sectionKind @@ -76,14 +78,16 @@ def getClassMethodDoc(self, methodname): ] else: args = [] - args += [el.text.strip() for el in member.xpath("param/declname")] + args += [el.text.strip() for el in member.xpath("param/declname") if el.text] for parameters in dd.xpath("para/parameterlist/parameteritem"): pargs = [ el.text.strip() for el in parameters.find("parameternamelist").findall("parametername") + if el.text ] pns = " ".join(pargs) - pd = parameters.find("parameterdescription").find("para").text.strip() + para_el = parameters.find("parameterdescription").find("para") + pd = para_el.text.strip() if para_el is not None and para_el.text else "" d += "\n:param " + pns + ":" + pd return b, d, args diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 4a74eed2..55a7d882 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -52,15 +54,18 @@ void exposeBySubstitution() { .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) .value("SUCCESS", HierarchicalIterative::SUCCESS); + // DocClass(solver::BySubstitution) class_ >( "BySubstitution", init()) .def("explicitConstraintSetHasChanged", - &BySubstitution::explicitConstraintSetHasChanged) + &BySubstitution::explicitConstraintSetHasChanged, + DocClassMethod(explicitConstraintSetHasChanged)) .def("solve", &BySubstitution_solve) .def("explicitConstraintSet", static_cast( &BySubstitution::explicitConstraintSet), - return_internal_reference<>()) + return_internal_reference<>(), + DocClassMethod(explicitConstraintSet)) .add_property("errorThreshold", static_cast( &BySubstitution::errorThreshold), diff --git a/src/pyhpp/constraints/explicit-constraint-set.cc b/src/pyhpp/constraints/explicit-constraint-set.cc index 26578a83..032577ea 100644 --- a/src/pyhpp/constraints/explicit-constraint-set.cc +++ b/src/pyhpp/constraints/explicit-constraint-set.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -39,10 +41,11 @@ namespace constraints { using namespace hpp::constraints; void exposeExplicitConstraintSet() { + // DocClass(ExplicitConstraintSet) class_("ExplicitConstraintSet", init()) .def("__str__", &to_str) - .def("add", &ExplicitConstraintSet::add); + .def("add", &ExplicitConstraintSet::add, DocClassMethod(add)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/implicit.cc b/src/pyhpp/constraints/implicit.cc index fca3a0dc..72aba304 100644 --- a/src/pyhpp/constraints/implicit.cc +++ b/src/pyhpp/constraints/implicit.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -53,13 +55,22 @@ void exposeImplicit() { .value("EqualToZero", EqualToZero) .value("Superior", Superior) .value("Inferior", Inferior); + // DocClass(Implicit) class_("Implicit", no_init) .def("__init__", make_constructor(&Implicit::create)) - .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(Implicit, comparisonType, - const ComparisonTypes_t&) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Implicit, function) - .def("parameterSize", &Implicit::parameterSize) - .def("rightHandSideSize", &Implicit::rightHandSideSize) + .def("comparisonType", + static_cast( + &Implicit::comparisonType), + return_internal_reference<>()) + .def("comparisonType", + static_cast( + &Implicit::comparisonType)) + .def("function", &Implicit::function, return_internal_reference<>(), + DocClassMethod(function)) + .def("parameterSize", &Implicit::parameterSize, + DocClassMethod(parameterSize)) + .def("rightHandSideSize", &Implicit::rightHandSideSize, + DocClassMethod(rightHandSideSize)) .def("getFunctionOutputSize", &getFunctionOutputSize) .staticmethod("getFunctionOutputSize"); } diff --git a/src/pyhpp/constraints/iterative-solver.cc b/src/pyhpp/constraints/iterative-solver.cc index 6325ea48..f9e01d4c 100644 --- a/src/pyhpp/constraints/iterative-solver.cc +++ b/src/pyhpp/constraints/iterative-solver.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -48,10 +50,11 @@ void exposeHierarchicalIterativeSolver() { class_("ComparisonTypes") .def(vector_indexing_suite()); + // DocClass(solver::HierarchicalIterative) class_("HierarchicalIterative", init()) .def("__str__", &to_str) - .def("add", &HierarchicalIterative::add) + .def("add", &HierarchicalIterative::add, DocClassMethod(add)) .add_property( "errorThreshold", diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 60ec1d9c..86fc8e13 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -85,41 +87,58 @@ static void setRightHandSideOfConstraint( } void exposeConstraint() { + // DocClass(Constraint) class_("Constraint", no_init) .def("__str__", &to_str_from_operator) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Constraint, name) - .PYHPP_DEFINE_METHOD(CWrapper, apply) + .def("name", &Constraint::name, return_internal_reference<>(), + DocClassMethod(name)) + .def("apply", &CWrapper::apply) .def("isSatisfied", &CWrapper::isSatisfied1) .def("isSatisfied", &CWrapper::isSatisfied2) - .PYHPP_DEFINE_METHOD(CWrapper, copy); + .def("copy", &CWrapper::copy); + // DocClass(ConstraintSet) class_ >("ConstraintSet", no_init) .def("__init__", make_constructor(&createConstraintSet)) - .PYHPP_DEFINE_METHOD(ConstraintSet, addConstraint) - .PYHPP_DEFINE_METHOD(ConstraintSet, configProjector); + .def("addConstraint", &ConstraintSet::addConstraint, + DocClassMethod(addConstraint)) + .def("configProjector", &ConstraintSet::configProjector, + DocClassMethod(configProjector)); + // DocClass(ConfigProjector) class_ >("ConfigProjector", no_init) .def("__init__", make_constructor(&createConfigProjector)) .def("solver", static_cast( &ConfigProjector::solver), - return_internal_reference<>()) - .def("add", &ConfigProjector::add) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, lastIsOptional, bool) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, maxIterations, size_type) + return_internal_reference<>(), DocClassMethod(solver)) + .def("add", &ConfigProjector::add, DocClassMethod(add)) + .def("lastIsOptional", + static_cast( + &ConfigProjector::lastIsOptional)) + .def("lastIsOptional", + static_cast( + &ConfigProjector::lastIsOptional)) + .def("maxIterations", + static_cast( + &ConfigProjector::maxIterations)) + .def("maxIterations", + static_cast( + &ConfigProjector::maxIterations)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) - .PYHPP_DEFINE_METHOD(ConfigProjector, residualError) + .def("residualError", &ConfigProjector::residualError, + DocClassMethod(residualError)) .def("setRightHandSideFromConfig", &rightHandSideFromConfig) .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint) .def("sigma", &ConfigProjector::sigma, - return_value_policy()); + return_value_policy(), DocClassMethod(sigma)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/node.cc b/src/pyhpp/core/node.cc index d702fe20..1ffc5c56 100644 --- a/src/pyhpp/core/node.cc +++ b/src/pyhpp/core/node.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -42,23 +44,26 @@ namespace core { using namespace hpp::core; void exposeNode() { + // DocClass(Node) class_, boost::noncopyable>("Node", no_init) .def(init()) .def(init()) - .PYHPP_DEFINE_METHOD(Node, addOutEdge) - .PYHPP_DEFINE_METHOD(Node, addInEdge) + .def("addOutEdge", &Node::addOutEdge, DocClassMethod(addOutEdge)) + .def("addInEdge", &Node::addInEdge, DocClassMethod(addInEdge)) .def("connectedComponent", static_cast( &Node::connectedComponent)) .def("connectedComponent", static_cast( &Node::connectedComponent)) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, outEdges) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, inEdges) - .PYHPP_DEFINE_METHOD(Node, isOutNeighbor) - .PYHPP_DEFINE_METHOD(Node, isInNeighbor) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, configuration) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, print); + .def("outEdges", &Node::outEdges, return_internal_reference<>(), + DocClassMethod(outEdges)) + .def("inEdges", &Node::inEdges, return_internal_reference<>(), + DocClassMethod(inEdges)) + .def("isOutNeighbor", &Node::isOutNeighbor, DocClassMethod(isOutNeighbor)) + .def("isInNeighbor", &Node::isInNeighbor, DocClassMethod(isInNeighbor)) + .def("configuration", &Node::configuration, return_internal_reference<>(), + DocClassMethod(configuration)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/path.cc b/src/pyhpp/core/path.cc index 66d55d77..9892e68c 100644 --- a/src/pyhpp/core/path.cc +++ b/src/pyhpp/core/path.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -141,6 +143,7 @@ struct PathWrap : PathWrapper, wrapper { }; void exposePath() { + // DocClass(Path) class_, boost::noncopyable>("Path", no_init) .def("__str__", &to_str_from_operator) @@ -148,26 +151,29 @@ void exposePath() { .def("__call__", &PathWrap::py_call2) .def("eval", &PathWrap::py_call1) .def("eval", &PathWrap::py_call2) - .PYHPP_DEFINE_METHOD(PathWrap, derivative) + .def("derivative", &PathWrap::derivative) - .def("copy", static_cast(&Path::copy)) + .def("copy", static_cast(&Path::copy), + DocClassMethod(copy)) .def("extract", static_cast(&Path::extract)) + const>(&Path::extract), + DocClassMethod(extract)) .def("extract", static_cast( &Path::extract)) - // .PYHPP_DEFINE_METHOD (Path, timeRange) .def("timeRange", static_cast(&Path::timeRange), - return_internal_reference<>()) - .def("reverse", &Path::reverse) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Path, paramRange) - .PYHPP_DEFINE_METHOD(Path, length) - .PYHPP_DEFINE_METHOD(Path, initial) - .PYHPP_DEFINE_METHOD(Path, end) - .PYHPP_DEFINE_METHOD(PathWrap, constraints) - .PYHPP_DEFINE_METHOD(Path, outputSize) - .PYHPP_DEFINE_METHOD(Path, outputDerivativeSize); + return_internal_reference<>(), DocClassMethod(timeRange)) + .def("reverse", &Path::reverse, DocClassMethod(reverse)) + .def("paramRange", &Path::paramRange, return_internal_reference<>(), + DocClassMethod(paramRange)) + .def("length", &Path::length, DocClassMethod(length)) + .def("initial", &Path::initial, DocClassMethod(initial)) + .def("end", &Path::end, DocClassMethod(end)) + .def("constraints", &PathWrap::constraints) + .def("outputSize", &Path::outputSize, DocClassMethod(outputSize)) + .def("outputDerivativeSize", &Path::outputDerivativeSize, + DocClassMethod(outputDerivativeSize)); class_, hpp::shared_ptr, boost::noncopyable>( "PathWrap", no_init) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index e7036c04..3e846343 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -33,6 +33,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -218,49 +220,52 @@ struct RWrapper { }; void exposeRoadmap() { + // DocClass(Roadmap) class_("Roadmap", no_init) .def("__init__", make_constructor(&Roadmap::create)) .def("__str__", &to_str) - .PYHPP_DEFINE_METHOD(Roadmap, clear) - .PYHPP_DEFINE_METHOD1(RWrapper, addNode, - return_value_policy()) + .def("clear", &Roadmap::clear, DocClassMethod(clear)) + .def("addNode", &RWrapper::addNode, + return_value_policy()) .def("nearestNode", &RWrapper::nearestNode1) .def("nearestNode", &RWrapper::nearestNode2) .def("nearestNode", &RWrapper::nearestNode3) .def("nearestNode", &RWrapper::nearestNode4) .def("nearestNodes", &RWrapper::nearestNodes1) .def("nearestNodes", &RWrapper::nearestNodes2) - .PYHPP_DEFINE_METHOD(Roadmap, nodesWithinBall) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdges, - return_value_policy()) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdge, - return_value_policy()) + .def("nodesWithinBall", &Roadmap::nodesWithinBall, + DocClassMethod(nodesWithinBall)) + .def("addNodeAndEdges", &RWrapper::addNodeAndEdges, + return_value_policy()) + .def("addNodeAndEdge", &RWrapper::addNodeAndEdge, + return_value_policy()) .def("addEdge", &RWrapper::addEdge) .def("addEdge", &RWrapper::addEdge2) - .PYHPP_DEFINE_METHOD(Roadmap, addEdges) + .def("addEdges", &Roadmap::addEdges, DocClassMethod(addEdges)) .def("merge", - static_cast(&Roadmap::merge)) - .PYHPP_DEFINE_METHOD(Roadmap, insertPathVector) - .PYHPP_DEFINE_METHOD1(Roadmap, addGoalNode, - return_value_policy()) - .PYHPP_DEFINE_METHOD(Roadmap, resetGoalNodes) - .PYHPP_DEFINE_METHOD(Roadmap, pathExists) + static_cast(&Roadmap::merge), + DocClassMethod(merge)) + .def("insertPathVector", &Roadmap::insertPathVector, + DocClassMethod(insertPathVector)) + .def("addGoalNode", &Roadmap::addGoalNode, + return_value_policy(), + DocClassMethod(addGoalNode)) + .def("resetGoalNodes", &Roadmap::resetGoalNodes, + DocClassMethod(resetGoalNodes)) + .def("pathExists", &Roadmap::pathExists, DocClassMethod(pathExists)) .def("nodes", &RWrapper::nodes) .def("nodesConnectedComponent", &RWrapper::nodesConnectedComponent) .def("initNode", &RWrapper::initNode1) .def("initNode", &RWrapper::initNode2, return_value_policy()) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, goalNodes) + .def("goalNodes", &Roadmap::goalNodes, return_internal_reference<>(), + DocClassMethod(goalNodes)) .def("connectedComponents", &RWrapper::connectedComponents) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, distance) + .def("distance", &Roadmap::distance, return_internal_reference<>(), + DocClassMethod(distance)) .def("numberConnectedComponents", &RWrapper::numberConnectedComponents) .def("getConnectedComponent", &RWrapper::getConnectedComponent) - .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode) - // .def("cost", &RWrapper::cost1) - // .def("cost", &RWrapper::cost2, - // return_value_policy()) - - ; + .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode); } } // namespace core } // namespace pyhpp From b7fd95c9ca03dc6911a2bc4c3b0dea6414c0e62f Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 26 Jan 2026 09:25:33 +0100 Subject: [PATCH 080/109] Expand DocClassMethod() usage and add it to relevant functions --- doc/configure.py | 8 +- doc/doxygen_xml_parser.py | 117 ++++++++++++++---- src/pyhpp/constraints/by-substitution.cc | 4 +- src/pyhpp/constraints/explicit.cc | 3 + .../constraints/generic-transformation.cc | 2 + src/pyhpp/constraints/implicit.cc | 2 +- src/pyhpp/constraints/locked-joint.cc | 2 + src/pyhpp/constraints/relative-com.cc | 6 +- src/pyhpp/core/config-validation.cc | 2 +- src/pyhpp/core/configuration-shooter.cc | 5 +- src/pyhpp/core/connected-component.cc | 10 +- src/pyhpp/core/distance.cc | 7 +- src/pyhpp/core/parameter.cc | 3 + src/pyhpp/core/path-optimizer.cc | 14 ++- src/pyhpp/core/path-planner.cc | 40 ++++-- src/pyhpp/core/path-projector.cc | 5 +- src/pyhpp/core/path-validation.cc | 5 +- src/pyhpp/core/path/spline.cc | 2 + src/pyhpp/core/path/vector.cc | 18 +-- src/pyhpp/core/problem-target.cc | 3 + src/pyhpp/core/problem.cc | 26 ++-- src/pyhpp/core/reports.cc | 7 ++ src/pyhpp/core/steering-method.cc | 10 +- src/pyhpp/manipulation/device.cc | 17 ++- src/pyhpp/manipulation/graph.cc | 12 +- src/pyhpp/manipulation/path-optimizer.cc | 2 + src/pyhpp/manipulation/path-planner.cc | 30 +++-- src/pyhpp/manipulation/path-projector.cc | 2 + src/pyhpp/manipulation/problem.cc | 5 +- src/pyhpp/manipulation/steering-method.cc | 2 + src/pyhpp/pinocchio/device.cc | 28 +++-- src/pyhpp/pinocchio/liegroup.cc | 45 ++++--- 32 files changed, 321 insertions(+), 123 deletions(-) diff --git a/doc/configure.py b/doc/configure.py index 068d108d..b9e95cd9 100755 --- a/doc/configure.py +++ b/doc/configure.py @@ -116,7 +116,13 @@ def _xmlDirFromPkgConfig(pkg): nsToPackage = { "hpp::core": _xmlDirFromPkgConfig("hpp-core"), + "hpp::core::path": _xmlDirFromPkgConfig("hpp-core"), "hpp::constraints": _xmlDirFromPkgConfig("hpp-constraints"), + "hpp::constraints::solver": _xmlDirFromPkgConfig("hpp-constraints"), + "hpp::pinocchio": _xmlDirFromPkgConfig("hpp-pinocchio"), + "hpp::manipulation": _xmlDirFromPkgConfig("hpp-manipulation"), + "hpp::manipulation::graph": _xmlDirFromPkgConfig("hpp-manipulation"), + "hpp::manipulation::pathPlanner": _xmlDirFromPkgConfig("hpp-manipulation"), } @@ -142,7 +148,7 @@ def make_doc_string(brief, detailled): def make_args_string(args): - return "(" + ",".join(['arg("' + a + '")' for a in args]) + ")" + return "(" + ",".join(['boost::python::arg("' + a + '")' for a in args]) + ")" def substitute(istr, ostr): diff --git a/doc/doxygen_xml_parser.py b/doc/doxygen_xml_parser.py index 7d374531..aa59cad5 100644 --- a/doc/doxygen_xml_parser.py +++ b/doc/doxygen_xml_parser.py @@ -16,14 +16,18 @@ def _getCompound(self, kind, name): def classDoc(self, name): el = self._getCompound("class", name) - return ClassDoc(path.join(self.directory, el.get("refid") + ".xml")) + return ClassDoc( + path.join(self.directory, el.get("refid") + ".xml"), self.directory + ) class ClassDoc: - def __init__(self, filename): + def __init__(self, filename, directory=None): self.tree = etree.parse(filename) self.compound = self.tree.find("./compounddef") self.classname = self.compound.find("compoundname").text.strip() + self.directory = directory if directory else path.dirname(filename) + self._group_cache = {} @staticmethod def _getDoc(el): @@ -33,30 +37,93 @@ def _getDoc(el): detailed = d.text.strip() if d.text else "" return brief, detailed + def _getMemberFromGroup(self, memberDefKind, name): + """Look for memberdef in a group file via member refid.""" + members = self.compound.xpath( + "sectiondef/member[@kind='" + memberDefKind + "' and name='" + name + "']" + ) + if not members: + return None + member = members[0] + refid = member.get("refid") + if not refid: + return None + group_id = refid.rsplit("_1", 1)[0] + if group_id not in self._group_cache: + group_file = path.join(self.directory, group_id + ".xml") + if not path.isfile(group_file): + return None + self._group_cache[group_id] = etree.parse(group_file) + group_tree = self._group_cache[group_id] + memberdefs = group_tree.xpath( + "//memberdef[@id='" + refid + "' and @kind='" + memberDefKind + "']" + ) + if memberdefs: + return memberdefs[0] + return None + + def _getMemberFromInherited(self, memberDefKind, name): + """Look for memberdef in base class XML via listofallmembers member refid.""" + members = self.compound.xpath( + "listofallmembers/member[name='" + name + "']" + ) + if not members: + return None + member = members[0] + refid = member.get("refid") + if not refid: + return None + # Extract base class file from refid + # e.g., "classhpp_1_1pinocchio_1_1AbstractDevice_1a312..." -> "classhpp_1_1pinocchio_1_1AbstractDevice" + parts = refid.rsplit("_1", 1) + if len(parts) < 2: + return None + base_class_id = parts[0] + base_class_file = path.join(self.directory, base_class_id + ".xml") + if not path.isfile(base_class_file): + return None + if base_class_id not in self._group_cache: + self._group_cache[base_class_id] = etree.parse(base_class_file) + base_tree = self._group_cache[base_class_id] + memberdefs = base_tree.xpath( + "//memberdef[@id='" + refid + "' and @kind='" + memberDefKind + "']" + ) + if memberdefs: + return memberdefs[0] + return None + def _getMember(self, sectionKind, memberDefKind, name): - # return self.compound.xpath ("sectiondef[@kind='" + sectionKind - try: - return self.compound.xpath( - "sectiondef[re:test(@kind,'" - + sectionKind - + "')]/memberdef[@kind='" - + memberDefKind - + "' and name='" - + name - + "']", - namespaces={"re": "http://exslt.org/regular-expressions"}, - )[0] - except IndexError as e: - msg = ( - "Error: Could not find member (" - + sectionKind - + ") " - + name - + " of class " - + self.classname - ) - print(msg + "\n" + str(e), file=sys.stderr) - raise IndexError(msg) + # First try to find memberdef directly in class XML + results = self.compound.xpath( + "sectiondef[re:test(@kind,'" + + sectionKind + + "')]/memberdef[@kind='" + + memberDefKind + + "' and name='" + + name + + "']", + namespaces={"re": "http://exslt.org/regular-expressions"}, + ) + if results: + return results[0] + # Fall back to looking in group files (for @ingroup documented classes) + memberdef = self._getMemberFromGroup(memberDefKind, name) + if memberdef is not None: + return memberdef + # Fall back to looking in base class files (for inherited methods) + memberdef = self._getMemberFromInherited(memberDefKind, name) + if memberdef is not None: + return memberdef + msg = ( + "Error: Could not find member (" + + sectionKind + + ") " + + name + + " of class " + + self.classname + ) + print(msg, file=sys.stderr) + raise IndexError(msg) def getClassDoc(self): return self._getDoc(self.compound) diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index 55a7d882..fa9fa439 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -31,7 +31,7 @@ #include #include -// DocNamespace(hpp::constraints) +// DocNamespace(hpp::constraints::solver) using namespace boost::python; @@ -54,7 +54,7 @@ void exposeBySubstitution() { .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) .value("SUCCESS", HierarchicalIterative::SUCCESS); - // DocClass(solver::BySubstitution) + // DocClass(BySubstitution) class_ >( "BySubstitution", init()) .def("explicitConstraintSetHasChanged", diff --git a/src/pyhpp/constraints/explicit.cc b/src/pyhpp/constraints/explicit.cc index 4b076f8d..fe6c9cfb 100644 --- a/src/pyhpp/constraints/explicit.cc +++ b/src/pyhpp/constraints/explicit.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -61,6 +63,7 @@ ExplicitPtr_t createExplicit(const LiegroupSpacePtr_t& configSpace, } void exposeExplicit() { + // DocClass(Explicit) class_("Explicit", no_init) .def("create", &createExplicit) .staticmethod("create"); diff --git a/src/pyhpp/constraints/generic-transformation.cc b/src/pyhpp/constraints/generic-transformation.cc index a4be2f8a..425c0dfd 100644 --- a/src/pyhpp/constraints/generic-transformation.cc +++ b/src/pyhpp/constraints/generic-transformation.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/constraints/implicit.cc b/src/pyhpp/constraints/implicit.cc index 72aba304..a5cd89c0 100644 --- a/src/pyhpp/constraints/implicit.cc +++ b/src/pyhpp/constraints/implicit.cc @@ -61,7 +61,7 @@ void exposeImplicit() { .def("comparisonType", static_cast( &Implicit::comparisonType), - return_internal_reference<>()) + return_internal_reference<>(), DocClassMethod(comparisonType)) .def("comparisonType", static_cast( &Implicit::comparisonType)) diff --git a/src/pyhpp/constraints/locked-joint.cc b/src/pyhpp/constraints/locked-joint.cc index 450ae864..7cd14c13 100644 --- a/src/pyhpp/constraints/locked-joint.cc +++ b/src/pyhpp/constraints/locked-joint.cc @@ -25,6 +25,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/constraints/relative-com.cc b/src/pyhpp/constraints/relative-com.cc index 44b003a8..5672a0c8 100644 --- a/src/pyhpp/constraints/relative-com.cc +++ b/src/pyhpp/constraints/relative-com.cc @@ -32,6 +32,9 @@ #include #include #include + +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -64,9 +67,10 @@ static RelativeComPtr_t create3(const std::string& name, } void exposeRelativeCom() { + // DocClass(RelativeCom) class_, boost::noncopyable>("RelativeCom", no_init) - .def("create", &create1) + .def("create", &create1, DocClassMethod(create)) .def("create", &create2) .def("create", &create3) .staticmethod("create"); diff --git a/src/pyhpp/core/config-validation.cc b/src/pyhpp/core/config-validation.cc index 9c0bf84f..6f2c1a8c 100644 --- a/src/pyhpp/core/config-validation.cc +++ b/src/pyhpp/core/config-validation.cc @@ -69,7 +69,7 @@ void exposeConfigValidation() { .PYHPP_DEFINE_METHOD2(ConfigValidations, add, DocClassMethod(add)) .PYHPP_DEFINE_METHOD2(ConfigValidations, numberConfigValidations, DocClassMethod(numberConfigValidations)) - .PYHPP_DEFINE_METHOD2(ConfigValidations, clear, DocClassMethod(clear)); + .def("clear", &ConfigValidations::clear); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/configuration-shooter.cc b/src/pyhpp/core/configuration-shooter.cc index 36d0c39d..a6807200 100644 --- a/src/pyhpp/core/configuration-shooter.cc +++ b/src/pyhpp/core/configuration-shooter.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -43,9 +45,10 @@ struct CSWrapper { }; void exposeConfigurationShooter() { + // DocClass(ConfigurationShooter) class_( "ConfigurationShooter", no_init) - .def("shoot", &CSWrapper::shoot); + .def("shoot", &CSWrapper::shoot, DocClassMethod(shoot)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/connected-component.cc b/src/pyhpp/core/connected-component.cc index 8bc8ee40..399b5e13 100644 --- a/src/pyhpp/core/connected-component.cc +++ b/src/pyhpp/core/connected-component.cc @@ -35,6 +35,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -73,11 +75,13 @@ struct CCWrapper { } }; void exposeConnectedComponent() { + // DocClass(ConnectedComponent) class_( "ConnectedComponent", no_init) - .def("nodes", &CCWrapper::nodes) - .def("reachableFrom", &CCWrapper::reachableFrom) - .def("reachableTo", &CCWrapper::reachableTo) + .def("nodes", &CCWrapper::nodes, DocClassMethod(nodes)) + .def("reachableFrom", &CCWrapper::reachableFrom, + DocClassMethod(reachableFrom)) + .def("reachableTo", &CCWrapper::reachableTo, DocClassMethod(reachableTo)) .def("__eq__", &CCWrapper::equality); } } // namespace core diff --git a/src/pyhpp/core/distance.cc b/src/pyhpp/core/distance.cc index aa6ac92e..a96f3f40 100644 --- a/src/pyhpp/core/distance.cc +++ b/src/pyhpp/core/distance.cc @@ -33,6 +33,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -55,8 +57,11 @@ struct DistanceWrapper { }; void exposeDistance() { + // DocClass(Distance) class_("Distance", no_init) - .def("compute", &DistanceWrapper::compute); + .def("compute", &DistanceWrapper::compute, DocClassMethod(compute)); + + // DocClass(WeighedDistance) class_, WeighedDistancePtr_t, boost::noncopyable>("WeighedDistance", no_init) .def("__init__", make_constructor(&WeighedDistance::create)) diff --git a/src/pyhpp/core/parameter.cc b/src/pyhpp/core/parameter.cc index 530c524a..36dc3a46 100644 --- a/src/pyhpp/core/parameter.cc +++ b/src/pyhpp/core/parameter.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -80,6 +82,7 @@ Parameter create(object param) { Parameter createBool(bool param) { return Parameter(param); } void exposeParameter() { + // DocClass(Parameter) class_("Parameter", no_init) .def("create", &create) .staticmethod("create") diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index 1c153d49..1eac091a 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -40,6 +40,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -47,13 +49,15 @@ namespace core { using namespace hpp::core; void exposePathOptimizer() { + // DocClass(PathOptimizer) class_("PathOptimizer", no_init) - .def("problem", &PathOptimizer::problem) - .def("optimize", &PathOptimizer::optimize) - .def("interrupt", &PathOptimizer::interrupt) - .def("maxIterations", &PathOptimizer::maxIterations) - .def("timeOut", &PathOptimizer::timeOut); + .def("problem", &PathOptimizer::problem, DocClassMethod(problem)) + .def("optimize", &PathOptimizer::optimize, DocClassMethod(optimize)) + .def("interrupt", &PathOptimizer::interrupt, DocClassMethod(interrupt)) + .def("maxIterations", &PathOptimizer::maxIterations, + DocClassMethod(maxIterations)) + .def("timeOut", &PathOptimizer::timeOut, DocClassMethod(timeOut)); class_, diff --git a/src/pyhpp/core/path-planner.cc b/src/pyhpp/core/path-planner.cc index c09e37bb..124391d4 100644 --- a/src/pyhpp/core/path-planner.cc +++ b/src/pyhpp/core/path-planner.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + namespace pyhpp { namespace core { @@ -142,20 +144,32 @@ void PathPlanner::stopWhenProblemIsSolved(bool enable) { PathVectorPtr_t PathPlanner::computePath() const { return obj->computePath(); } void exposePathPlanner() { + // DocClass(PathPlanner) class_("PathPlanner", no_init) - .PYHPP_DEFINE_METHOD_CONST_REF(PathPlanner, roadmap) - .PYHPP_DEFINE_METHOD(PathPlanner, problem) - .PYHPP_DEFINE_METHOD(PathPlanner, startSolve) - .PYHPP_DEFINE_METHOD(PathPlanner, solve) - .PYHPP_DEFINE_METHOD(PathPlanner, tryConnectInitAndGoals) - .PYHPP_DEFINE_METHOD(PathPlanner, oneStep) - .PYHPP_DEFINE_METHOD(PathPlanner, finishSolve) - .PYHPP_DEFINE_METHOD(PathPlanner, interrupt) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PathPlanner, maxIterations, - unsigned long int) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PathPlanner, timeOut, double) - .PYHPP_DEFINE_METHOD(PathPlanner, stopWhenProblemIsSolved) - .PYHPP_DEFINE_METHOD(PathPlanner, computePath); + .def("roadmap", &PathPlanner::roadmap, + return_value_policy(), DocClassMethod(roadmap)) + .def("problem", &PathPlanner::problem, DocClassMethod(problem)) + .def("startSolve", &PathPlanner::startSolve, DocClassMethod(startSolve)) + .def("solve", &PathPlanner::solve, DocClassMethod(solve)) + .def("tryConnectInitAndGoals", &PathPlanner::tryConnectInitAndGoals, + DocClassMethod(tryConnectInitAndGoals)) + .def("oneStep", &PathPlanner::oneStep, DocClassMethod(oneStep)) + .def("finishSolve", &PathPlanner::finishSolve, DocClassMethod(finishSolve)) + .def("interrupt", &PathPlanner::interrupt, DocClassMethod(interrupt)) + .def("maxIterations", + static_cast( + &PathPlanner::maxIterations)) + .def("maxIterations", + static_cast( + &PathPlanner::maxIterations)) + .def("timeOut", + static_cast(&PathPlanner::timeOut)) + .def("timeOut", + static_cast( + &PathPlanner::timeOut)) + .def("stopWhenProblemIsSolved", &PathPlanner::stopWhenProblemIsSolved, + DocClassMethod(stopWhenProblemIsSolved)) + .def("computePath", &PathPlanner::computePath, DocClassMethod(computePath)); pyhpp::core::pathPlanner::exposePathPlanners(); } diff --git a/src/pyhpp/core/path-projector.cc b/src/pyhpp/core/path-projector.cc index cac3bf01..c6678db2 100644 --- a/src/pyhpp/core/path-projector.cc +++ b/src/pyhpp/core/path-projector.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -56,9 +58,10 @@ struct PPWrapper { }; void exposePathProjector() { + // DocClass(PathProjector) class_("PathProjector", no_init) - .def("apply", &PPWrapper::apply) + .def("apply", &PPWrapper::apply, DocClassMethod(apply)) .def("apply", &PPWrapper::py_apply); class_, diff --git a/src/pyhpp/core/path-validation.cc b/src/pyhpp/core/path-validation.cc index 22988828..b46db98e 100644 --- a/src/pyhpp/core/path-validation.cc +++ b/src/pyhpp/core/path-validation.cc @@ -40,6 +40,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -79,9 +81,10 @@ struct PVWrapper { }; void exposePathValidation() { + // DocClass(PathValidation) class_( "PathValidation", no_init) - .def("validate", &PVWrapper::validate) + .def("validate", &PVWrapper::validate, DocClassMethod(validate)) .def("validate", &PVWrapper::py_validate) .def("validateConfiguration", &PVWrapper::validateConfiguration); diff --git a/src/pyhpp/core/path/spline.cc b/src/pyhpp/core/path/spline.cc index 2f6e69b3..5b7b0759 100644 --- a/src/pyhpp/core/path/spline.cc +++ b/src/pyhpp/core/path/spline.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core::path) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 6388da39..fde83e71 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -42,17 +44,19 @@ namespace path { using namespace hpp::core; void exposeVector() { + // DocClass(PathVector) class_, boost::noncopyable>("Vector", no_init) .def("create", static_cast( - &PathVector::create)) + &PathVector::create), + DocClassMethod(create)) .staticmethod("create") - .PYHPP_DEFINE_METHOD(PathVector, numberPaths) - .PYHPP_DEFINE_METHOD(PathVector, pathAtRank) - .PYHPP_DEFINE_METHOD(PathVector, rankAtParam) - .PYHPP_DEFINE_METHOD(PathVector, appendPath) - .PYHPP_DEFINE_METHOD(PathVector, concatenate) - .PYHPP_DEFINE_METHOD(PathVector, flatten); + .def("numberPaths", &PathVector::numberPaths, DocClassMethod(numberPaths)) + .def("pathAtRank", &PathVector::pathAtRank, DocClassMethod(pathAtRank)) + .def("rankAtParam", &PathVector::rankAtParam, DocClassMethod(rankAtParam)) + .def("appendPath", &PathVector::appendPath, DocClassMethod(appendPath)) + .def("concatenate", &PathVector::concatenate, DocClassMethod(concatenate)) + .def("flatten", &PathVector::flatten, DocClassMethod(flatten)); class_("Vectors").def( vector_indexing_suite()); diff --git a/src/pyhpp/core/problem-target.cc b/src/pyhpp/core/problem-target.cc index 4b514d69..8256081a 100644 --- a/src/pyhpp/core/problem-target.cc +++ b/src/pyhpp/core/problem-target.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -39,6 +41,7 @@ namespace core { using namespace hpp::core; void exposeProblemTarget() { + // DocClass(ProblemTarget) class_("ProblemTarget", no_init) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index a8376220..5d9eca3a 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -30,6 +30,8 @@ #include "pyhpp/core/problem.hh" +// DocNamespace(hpp::core) + #include #include #include @@ -593,12 +595,16 @@ void register_problem_converters() { void exposeProblem() { class_("CppCoreProblem", no_init); + // DocClass(Problem) class_("Problem", init()) - .PYHPP_DEFINE_METHOD_CONST_REF_BY_VALUE(Problem, robot) - .PYHPP_DEFINE_METHOD(Problem, setParameter) + .def("robot", &Problem::robot, return_value_policy(), + DocClassMethod(robot)) + .def("setParameter", &Problem::setParameter, DocClassMethod(setParameter)) .def("setParameter", &Problem::setParameterFloat) .def("setParameter", &Problem::setParameterInt) - .PYHPP_DEFINE_METHOD_CONST_REF_BY_VALUE(Problem, getParameter) + .def("getParameter", &Problem::getParameter, + return_value_policy(), + DocClassMethod(getParameter)) .def("steeringMethod", static_cast( &Problem::steeringMethod)) @@ -613,8 +619,10 @@ void exposeProblem() { static_cast(&Problem::configValidation), (arg("configValidation"))) - .def("addConfigValidation", &Problem::addConfigValidation) - .def("clearConfigValidations", &Problem::clearConfigValidations) + .def("addConfigValidation", &Problem::addConfigValidation, + DocClassMethod(addConfigValidation)) + .def("clearConfigValidations", &Problem::clearConfigValidations, + DocClassMethod(clearConfigValidations)) .def("pathValidation", static_cast(&Problem::pathValidation)) @@ -640,9 +648,11 @@ void exposeProblem() { .def("configurationShooter", static_cast(&Problem::configurationShooter), (arg("configurationShooter"))) - .PYHPP_DEFINE_METHOD(Problem, initConfig) - .PYHPP_DEFINE_METHOD(Problem, addGoalConfig) - .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs) + .def("initConfig", &Problem::initConfig, DocClassMethod(initConfig)) + .def("addGoalConfig", &Problem::addGoalConfig, + DocClassMethod(addGoalConfig)) + .def("resetGoalConfigs", &Problem::resetGoalConfigs, + DocClassMethod(resetGoalConfigs)) .PYHPP_DEFINE_METHOD(Problem, addPartialCom) .PYHPP_DEFINE_METHOD(Problem, getPartialCom) .PYHPP_DEFINE_METHOD(Problem, createRelativeComConstraint) diff --git a/src/pyhpp/core/reports.cc b/src/pyhpp/core/reports.cc index 8ee49c50..b12e1a42 100644 --- a/src/pyhpp/core/reports.cc +++ b/src/pyhpp/core/reports.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -44,16 +46,19 @@ namespace core { using namespace hpp::core; void exposeReports() { + // DocClass(ValidationReport) class_( "ValidationReport", no_init) .def("__str__", &to_str); + // DocClass(CollisionValidationReport) class_ >("CollisionValidationReport", no_init) .def_readonly("object1", &CollisionValidationReport::object1) .def_readonly("object2", &CollisionValidationReport::object2) .def_readonly("result", &CollisionValidationReport::result); + // DocClass(JointBoundValidationReport) class_ >("JointBoundValidationReport", no_init) .def_readonly("joint_", &JointBoundValidationReport::joint_) @@ -62,12 +67,14 @@ void exposeReports() { .def_readonly("upperBound_", &JointBoundValidationReport::upperBound_) .def_readonly("value_", &JointBoundValidationReport::value_); + // DocClass(PathValidationReport) class_ >("PathValidationReport", no_init) .def_readwrite("parameter", &PathValidationReport::parameter) .def_readwrite("configurationReport", &PathValidationReport::configurationReport); + // DocClass(CollisionPathValidationReport) class_ >("CollisionPathValidationReport", no_init) diff --git a/src/pyhpp/core/steering-method.cc b/src/pyhpp/core/steering-method.cc index 8f46935b..e2e3532d 100644 --- a/src/pyhpp/core/steering-method.cc +++ b/src/pyhpp/core/steering-method.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + namespace pyhpp { namespace core { @@ -140,13 +142,15 @@ const ConstraintSetPtr_t& SteeringMethod::constraints() const { void exposeSteeringMethod() { register_ptr_to_python>(); + // DocClass(SteeringMethod) class_("SteeringMethod", no_init) .def("__call__", &SteeringMethod::operator()) - .def("steer", &SteeringMethod::steer) - .def("problem", &SteeringMethod::problem) + .def("steer", &SteeringMethod::steer, DocClassMethod(steer)) + .def("problem", &SteeringMethod::problem, DocClassMethod(problem)) .def("constraints", static_cast( - &SteeringMethod::constraints)) + &SteeringMethod::constraints), + DocClassMethod(constraints)) .def("constraints", static_cast( &SteeringMethod::constraints), diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index f414bf93..a68b273f 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::manipulation) + namespace pyhpp { namespace manipulation { using namespace boost::python; @@ -146,8 +148,9 @@ void setHandleMaskComp(const HandlePtr_t& handle, } void exposeHandle() { + // DocClass(Handle) class_("Handle", no_init) - .def("create", &Handle::create) + .def("create", &Handle::create, DocClassMethod(create)) .staticmethod("create") .add_property("name", &getHandleName, &setHandleName) .add_property("localPosition", &getHandleLocalPosition, @@ -158,15 +161,19 @@ void exposeHandle() { "clearance", static_cast(&Handle::clearance), static_cast(&Handle::clearance)) - .def("createGrasp", &Handle::createGrasp) - .def("createPreGrasp", &Handle::createPreGrasp) - .def("createGraspComplement", &Handle::createGraspComplement) - .def("createGraspAndComplement", &Handle::createGraspAndComplement); + .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) + .def("createPreGrasp", &Handle::createPreGrasp, + DocClassMethod(createPreGrasp)) + .def("createGraspComplement", &Handle::createGraspComplement, + DocClassMethod(createGraspComplement)) + .def("createGraspAndComplement", &Handle::createGraspAndComplement, + DocClassMethod(createGraspAndComplement)); class_ >("HandleMap") .def(boost::python::map_indexing_suite, true>()); } void exposeDevice() { + // DocClass(Device) class_, boost::shared_ptr, boost::noncopyable>("Device", init()) .def("setRobotRootPosition", &Device::setRobotRootPosition) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 0337bcb5..427575d3 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -46,6 +46,8 @@ #include "hpp/manipulation/constraint-set.hh" #include "pyhpp/core/problem.hh" +// DocNamespace(hpp::manipulation::graph) + namespace { const char* DOC_CREATESTATE = @@ -1236,13 +1238,17 @@ PyObject* getGraphCapsule(const PyWGraph& self) { using namespace boost::python; void exposeGraph() { + // DocClass(State) class_("State", no_init) - .def("name", &PyWState::name); + .def("name", &PyWState::name, DocClassMethod(name)); + // DocClass(Edge) class_("Transition", no_init) - .def("name", &PyWEdge::name) - .def("pathValidation", &PyWEdge::pathValidation); + .def("name", &PyWEdge::name, DocClassMethod(name)) + .def("pathValidation", &PyWEdge::pathValidation, + DocClassMethod(pathValidation)); + // DocClass(Graph) class_( "Graph", init()) diff --git a/src/pyhpp/manipulation/path-optimizer.cc b/src/pyhpp/manipulation/path-optimizer.cc index 5a761361..79df1863 100644 --- a/src/pyhpp/manipulation/path-optimizer.cc +++ b/src/pyhpp/manipulation/path-optimizer.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::manipulation) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index 382f9eff..fd5720e8 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -38,6 +38,8 @@ #include #include +// DocNamespace(hpp::manipulation::pathPlanner) + namespace pyhpp { namespace manipulation { @@ -227,6 +229,7 @@ bool EndEffectorTrajectory::checkFeasibilityOnly() const { // } void exposePathPlanners() { + // DocClass(TransitionPlanner) boost::python::class_>( "TransitionPlanner", boost::python::init()) @@ -236,21 +239,25 @@ void exposePathPlanners() { .def("innerPlanner", static_cast( &TransitionPlanner::innerPlanner)) - .def("innerProblem", &TransitionPlanner::innerProblem) - .def("planPath", &TransitionPlanner::planPath) + .def("innerProblem", &TransitionPlanner::innerProblem, + DocClassMethod(innerProblem)) + .def("planPath", &TransitionPlanner::planPath, DocClassMethod(planPath)) .def("directPath", &TransitionPlanner::directPath) .def("validateConfiguration", &TransitionPlanner::validateConfiguration) - .def("optimizePath", &TransitionPlanner::optimizePath) - .def("timeParameterization", &TransitionPlanner::timeParameterization) - .def("setEdge", &TransitionPlanner::setEdge) + .def("optimizePath", &TransitionPlanner::optimizePath, + DocClassMethod(optimizePath)) + .def("timeParameterization", &TransitionPlanner::timeParameterization, + DocClassMethod(timeParameterization)) + .def("setEdge", &TransitionPlanner::setEdge, DocClassMethod(setEdge)) .def("setReedsAndSheppSteeringMethod", - &TransitionPlanner::setReedsAndSheppSteeringMethod) - .def("pathProjector", &TransitionPlanner::pathProjector) - .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) + &TransitionPlanner::setReedsAndSheppSteeringMethod, + DocClassMethod(setReedsAndSheppSteeringMethod)) + .def("pathProjector", &TransitionPlanner::pathProjector, + DocClassMethod(pathProjector)) + .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers, + DocClassMethod(clearPathOptimizers)) .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer, - "Add a path optimizer\n\n Note: the input path optimizer should " - "have been constructed" - " with\n the inner problem of this class."); + DocClassMethod(addPathOptimizer)); boost::python::class_>( @@ -261,6 +268,7 @@ void exposePathPlanners() { boost::python::bases>( "StatesPathFinder", boost::python::init()); + // DocClass(EndEffectorTrajectory) boost::python::class_>( "EndEffectorTrajectory", diff --git a/src/pyhpp/manipulation/path-projector.cc b/src/pyhpp/manipulation/path-projector.cc index f420d24e..61b92bb7 100644 --- a/src/pyhpp/manipulation/path-projector.cc +++ b/src/pyhpp/manipulation/path-projector.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index 30b609a2..30582044 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::manipulation) + using namespace boost::python; namespace pyhpp { @@ -121,11 +123,12 @@ void Problem::graphSteeringMethod( // } void exposeProblem() { + // DocClass(Problem) class_>("Problem", init()) .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(Problem, constraintGraph, PyWGraphPtr_t) - .PYHPP_DEFINE_METHOD(Problem, checkProblem) + .def("checkProblem", &Problem::checkProblem, DocClassMethod(checkProblem)) .def( "steeringMethod", static_cast( diff --git a/src/pyhpp/manipulation/steering-method.cc b/src/pyhpp/manipulation/steering-method.cc index bea7cab8..ecd88083 100644 --- a/src/pyhpp/manipulation/steering-method.cc +++ b/src/pyhpp/manipulation/steering-method.cc @@ -33,6 +33,8 @@ #include #include +// DocNamespace(hpp::manipulation) + namespace pyhpp { namespace manipulation { diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 3e37f69e..b439402c 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -42,6 +42,8 @@ #include #include +// DocNamespace(hpp::pinocchio) + // #include // #include @@ -211,8 +213,9 @@ static boost::python::list getJointsPosition( } void exposeGripper() { + // DocClass(Gripper) class_("Gripper", no_init) - .def("create", &Gripper::create) + .def("create", &Gripper::create, DocClassMethod(create)) .staticmethod("create") .add_property("localPosition", &getObjectPositionInJoint) .add_property( @@ -295,30 +298,35 @@ void exposeDevice() { .value("COMPUTE_ALL", hpp::pinocchio::COMPUTE_ALL); void (Device::*cfk)(int) = &Device::computeForwardKinematics; + // DocClass(Device) class_, boost::noncopyable>("Device", no_init) .def("__init__", make_constructor(&createDevice)) - .def("name", &Device::name, return_value_policy()) + .def("name", &Device::name, return_value_policy(), + DocClassMethod(name)) .def("configSpace", &Device::configSpace, - return_value_policy()) + return_value_policy(), DocClassMethod(configSpace)) .def("model", &Device::model, return_internal_reference<>()) .def("data", &Device::data, return_internal_reference<>()) .def("geomData", &Device::geomData, return_internal_reference<>()) .def("geomModel", &Device::geomModel, return_internal_reference<>()) - .def("visualModel", &Device::visualModel, return_internal_reference<>()) - .PYHPP_DEFINE_METHOD(Device, configSize) - .PYHPP_DEFINE_METHOD(Device, numberDof) + .def("configSize", &Device::configSize, DocClassMethod(configSize)) + .def("numberDof", &Device::numberDof, DocClassMethod(numberDof)) .def("currentConfiguration", static_cast( &Device::currentConfiguration), - return_value_policy()) + return_value_policy(), + DocClassMethod(currentConfiguration)) .def("currentConfiguration", Device_currentConfiguration) - .def("computeForwardKinematics", cfk) + .def("computeForwardKinematics", cfk, + DocClassMethod(computeForwardKinematics)) .def("computeFramesForwardKinematics", - &Device::computeFramesForwardKinematics) - .def("updateGeometryPlacements", &Device::updateGeometryPlacements) + &Device::computeFramesForwardKinematics, + DocClassMethod(computeFramesForwardKinematics)) + .def("updateGeometryPlacements", &Device::updateGeometryPlacements, + DocClassMethod(updateGeometryPlacements)) .add_property("rankInConfiguration", &rankInConfiguration) .def("setJointBounds", &setJointBounds) .def("getCenterOfMass", &getCenterOfMass) diff --git a/src/pyhpp/pinocchio/liegroup.cc b/src/pyhpp/pinocchio/liegroup.cc index e51984e1..1c973878 100644 --- a/src/pyhpp/pinocchio/liegroup.cc +++ b/src/pyhpp/pinocchio/liegroup.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::pinocchio) + using namespace boost::python; namespace pyhpp { @@ -154,39 +156,43 @@ struct LgERWrapper { }; void exposeLiegroup() { + // DocClass(LiegroupSpace) class_("LiegroupSpace", no_init) .def("__str__", &to_str_from_operator) - .def("name", &LiegroupSpace::name, return_value_policy()) - .PYHPP_DEFINE_METHOD(LiegroupSpace, Rn) + .def("name", &LiegroupSpace::name, return_value_policy(), + DocClassMethod(name)) + .def("Rn", &LiegroupSpace::Rn, DocClassMethod(Rn)) .staticmethod("Rn") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R1) + .def("R1", &LiegroupSpace::R1, DocClassMethod(R1)) .staticmethod("R1") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R2) + .def("R2", &LiegroupSpace::R2, DocClassMethod(R2)) .staticmethod("R2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R3) + .def("R3", &LiegroupSpace::R3, DocClassMethod(R3)) .staticmethod("R3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, SE2) + .def("SE2", &LiegroupSpace::SE2, DocClassMethod(SE2)) .staticmethod("SE2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, SE3) + .def("SE3", &LiegroupSpace::SE3, DocClassMethod(SE3)) .staticmethod("SE3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R2xSO2) + .def("R2xSO2", &LiegroupSpace::R2xSO2, DocClassMethod(R2xSO2)) .staticmethod("R2xSO2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R3xSO3) + .def("R3xSO3", &LiegroupSpace::R3xSO3, DocClassMethod(R3xSO3)) .staticmethod("R3xSO3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, empty) + .def("empty", &LiegroupSpace::empty, DocClassMethod(empty)) .staticmethod("empty") - .PYHPP_DEFINE_METHOD(LiegroupSpace, mergeVectorSpaces) - .PYHPP_DEFINE_METHOD(LgSWrapper, dIntegrate_dq) - .PYHPP_DEFINE_METHOD(LgSWrapper, dIntegrate_dv) - .PYHPP_DEFINE_METHOD(LgSWrapper, dDifference_dq0) - .PYHPP_DEFINE_METHOD(LgSWrapper, dDifference_dq1) + .def("mergeVectorSpaces", &LiegroupSpace::mergeVectorSpaces, + DocClassMethod(mergeVectorSpaces)) + .def("dIntegrate_dq", &LgSWrapper::dIntegrate_dq) + .def("dIntegrate_dv", &LgSWrapper::dIntegrate_dv) + .def("dDifference_dq0", &LgSWrapper::dDifference_dq0) + .def("dDifference_dq1", &LgSWrapper::dDifference_dq1) .def(self == self) .def(self != self) // Operation on shared pointers... .def("__imul__", &LgSWrapper::itimes) .def("__mul__", &LgSWrapper::times); + // DocClass(LiegroupElement) class_("LiegroupElement", init()) .def(init()) @@ -199,12 +205,13 @@ void exposeLiegroup() { .def("vector", static_cast( &LiegroupElement::vector), - return_value_policy()) + return_value_policy(), DocClassMethod(vector)) .def("space", &LiegroupElement::space, - return_value_policy()) + return_value_policy(), DocClassMethod(space)) .def(self - self) .def(self + vector_t()); + // DocClass(LiegroupElementRef) class_("LiegroupElementRef", init()) // Pythonic API @@ -216,9 +223,9 @@ void exposeLiegroup() { .def("vector", static_cast( &LiegroupElementRef::vector), - return_value_policy()) + return_value_policy(), DocClassMethod(vector)) .def("space", &LiegroupElementRef::space, - return_value_policy()) + return_value_policy(), DocClassMethod(space)) .def(self - self) .def(self + vector_t()); } From b9be7de89d55a0950228f6e606723de4c3e66402 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Jan 2026 08:46:52 +0000 Subject: [PATCH 081/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/path/vector.cc | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 6bc77f07..d64d5ea2 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -31,12 +31,10 @@ #include #include #include -#include #include #include #include #include -#include #include using namespace boost::python; @@ -85,10 +83,8 @@ void exposeVector() { .PYHPP_DEFINE_METHOD(PathVector, flatten) // Serialization methods (binary format) - .def("save", &savePathVector, - "Save PathVector to file (binary format)") - .def("load", &loadPathVector, - "Load PathVector from file (binary format)") + .def("save", &savePathVector, "Save PathVector to file (binary format)") + .def("load", &loadPathVector, "Load PathVector from file (binary format)") .staticmethod("load"); class_("Vectors").def( From afba5d2d5260116694b6e57da3c7b9f7bf2377aa Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Jan 2026 08:49:36 +0000 Subject: [PATCH 082/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- doc/doxygen_xml_parser.py | 4 +--- src/pyhpp/constraints/by-substitution.cc | 3 +-- src/pyhpp/core/constraint.cc | 20 ++++++++------------ src/pyhpp/core/path-planner.cc | 11 ++++++----- src/pyhpp/core/path/vector.cc | 5 +++-- src/pyhpp/core/problem.cc | 4 ++-- 6 files changed, 21 insertions(+), 26 deletions(-) diff --git a/doc/doxygen_xml_parser.py b/doc/doxygen_xml_parser.py index aa59cad5..c4aa0400 100644 --- a/doc/doxygen_xml_parser.py +++ b/doc/doxygen_xml_parser.py @@ -64,9 +64,7 @@ def _getMemberFromGroup(self, memberDefKind, name): def _getMemberFromInherited(self, memberDefKind, name): """Look for memberdef in base class XML via listofallmembers member refid.""" - members = self.compound.xpath( - "listofallmembers/member[name='" + name + "']" - ) + members = self.compound.xpath("listofallmembers/member[name='" + name + "']") if not members: return None member = members[0] diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index fa9fa439..6b2e3c92 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -64,8 +64,7 @@ void exposeBySubstitution() { .def("explicitConstraintSet", static_cast( &BySubstitution::explicitConstraintSet), - return_internal_reference<>(), - DocClassMethod(explicitConstraintSet)) + return_internal_reference<>(), DocClassMethod(explicitConstraintSet)) .add_property("errorThreshold", static_cast( &BySubstitution::errorThreshold), diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 86fc8e13..4e76df84 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -115,18 +115,14 @@ void exposeConstraint() { &ConfigProjector::solver), return_internal_reference<>(), DocClassMethod(solver)) .def("add", &ConfigProjector::add, DocClassMethod(add)) - .def("lastIsOptional", - static_cast( - &ConfigProjector::lastIsOptional)) - .def("lastIsOptional", - static_cast( - &ConfigProjector::lastIsOptional)) - .def("maxIterations", - static_cast( - &ConfigProjector::maxIterations)) - .def("maxIterations", - static_cast( - &ConfigProjector::maxIterations)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) diff --git a/src/pyhpp/core/path-planner.cc b/src/pyhpp/core/path-planner.cc index 124391d4..bf1e33c2 100644 --- a/src/pyhpp/core/path-planner.cc +++ b/src/pyhpp/core/path-planner.cc @@ -154,7 +154,8 @@ void exposePathPlanner() { .def("tryConnectInitAndGoals", &PathPlanner::tryConnectInitAndGoals, DocClassMethod(tryConnectInitAndGoals)) .def("oneStep", &PathPlanner::oneStep, DocClassMethod(oneStep)) - .def("finishSolve", &PathPlanner::finishSolve, DocClassMethod(finishSolve)) + .def("finishSolve", &PathPlanner::finishSolve, + DocClassMethod(finishSolve)) .def("interrupt", &PathPlanner::interrupt, DocClassMethod(interrupt)) .def("maxIterations", static_cast( @@ -164,12 +165,12 @@ void exposePathPlanner() { &PathPlanner::maxIterations)) .def("timeOut", static_cast(&PathPlanner::timeOut)) - .def("timeOut", - static_cast( - &PathPlanner::timeOut)) + .def("timeOut", static_cast( + &PathPlanner::timeOut)) .def("stopWhenProblemIsSolved", &PathPlanner::stopWhenProblemIsSolved, DocClassMethod(stopWhenProblemIsSolved)) - .def("computePath", &PathPlanner::computePath, DocClassMethod(computePath)); + .def("computePath", &PathPlanner::computePath, + DocClassMethod(computePath)); pyhpp::core::pathPlanner::exposePathPlanners(); } diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index dfe89104..d08b368e 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -75,8 +75,9 @@ void exposeVector() { // DocClass(PathVector) class_, boost::noncopyable>("Vector", no_init) - .def("create", static_cast( - &PathVector::create), + .def("create", + static_cast( + &PathVector::create), DocClassMethod(create)) .staticmethod("create") .def("numberPaths", &PathVector::numberPaths, DocClassMethod(numberPaths)) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 5d9eca3a..e6dcb0f8 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -597,8 +597,8 @@ void exposeProblem() { // DocClass(Problem) class_("Problem", init()) - .def("robot", &Problem::robot, return_value_policy(), - DocClassMethod(robot)) + .def("robot", &Problem::robot, + return_value_policy(), DocClassMethod(robot)) .def("setParameter", &Problem::setParameter, DocClassMethod(setParameter)) .def("setParameter", &Problem::setParameterFloat) .def("setParameter", &Problem::setParameterInt) From dc9595b0f84ce1ec1c0f522c09d7905aa417503c Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 19 Jan 2026 08:32:06 +0100 Subject: [PATCH 083/109] Add static stability constraint creation tests --- tests/unit/test_static_stability.py | 88 +++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 tests/unit/test_static_stability.py diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py new file mode 100644 index 00000000..42366fc7 --- /dev/null +++ b/tests/unit/test_static_stability.py @@ -0,0 +1,88 @@ +#!/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, Problem, urdf +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) + + +def load_romeo(): + romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" + romeo_srdf = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" + + robot = Device("romeo") + romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf, 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], + ) + + return robot + + +class TestStaticStabilityConstraintsFactory(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.robot = load_romeo() + cls.problem = Problem(cls.robot) + cls.factory = StaticStabilityConstraintsFactory(cls.problem, cls.robot) + cls.q0 = cls.robot.currentConfiguration() + cls.left_ankle = "romeo/LAnkleRoll" + cls.right_ankle = "romeo/RAnkleRoll" + + def test_create_static_stability_constraint_returns_dict(self): + constraints = self.factory.createStaticStabilityConstraint( + "test_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("test_relative-com", constraints) + self.assertIn("test_pose-left-foot", constraints) + self.assertIn("test_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_sliding_stability_constraint_returns_dict(self): + constraints = self.factory.createSlidingStabilityConstraint( + "slide_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("slide_relative-com", constraints) + self.assertIn("slide_relative-pose", constraints) + self.assertIn("slide_pose-left-foot", constraints) + self.assertIn("slide_pose-left-foot-complement", constraints) + self.assertEqual(len(constraints), 4) + + def test_create_aligned_com_stability_constraint_returns_dict(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_", "", self.left_ankle, self.right_ankle, self.q0, sliding=False + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("aligned_com-between-feet", constraints) + self.assertIn("aligned_pose-left-foot", constraints) + self.assertIn("aligned_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_aligned_com_stability_constraint_sliding(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_slide_", "", self.left_ankle, self.right_ankle, self.q0, sliding=True + ) + + self.assertIsInstance(constraints, dict) + self.assertEqual(len(constraints), 3) + + +if __name__ == "__main__": + unittest.main() From 2ef687b6759bb7d35c26344c3671faf34d355ba8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Jan 2026 08:43:50 +0000 Subject: [PATCH 084/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- tests/unit/test_static_stability.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py index 42366fc7..babde7d1 100644 --- a/tests/unit/test_static_stability.py +++ b/tests/unit/test_static_stability.py @@ -16,7 +16,9 @@ def load_romeo(): romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" - romeo_srdf = "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" + romeo_srdf = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" + ) robot = Device("romeo") romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) @@ -31,7 +33,6 @@ def load_romeo(): class TestStaticStabilityConstraintsFactory(unittest.TestCase): - @classmethod def setUpClass(cls): cls.robot = load_romeo() @@ -77,7 +78,12 @@ def test_create_aligned_com_stability_constraint_returns_dict(self): def test_create_aligned_com_stability_constraint_sliding(self): constraints = self.factory.createAlignedCOMStabilityConstraint( - "aligned_slide_", "", self.left_ankle, self.right_ankle, self.q0, sliding=True + "aligned_slide_", + "", + self.left_ankle, + self.right_ankle, + self.q0, + sliding=True, ) self.assertIsInstance(constraints, dict) From 8988da7c73cb8e25cc9f4509577abb81418ee780 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 19 Jan 2026 09:33:49 +0100 Subject: [PATCH 085/109] Remove duplicate converter runtime warning --- src/pyhpp/__init__.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/__init__.py b/src/pyhpp/__init__.py index a8506fb6..a7ffa60d 100644 --- a/src/pyhpp/__init__.py +++ b/src/pyhpp/__init__.py @@ -1 +1,9 @@ -import eigenpy # noqa: F401 +import warnings + +warnings.filterwarnings( + "ignore", + message="to-Python converter .* already registered", + category=RuntimeWarning, +) + +import eigenpy # noqa: E402, F401 From a8fc4c035e92597083dde3c52a447616e32c74de Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 21 Jan 2026 16:20:54 +0100 Subject: [PATCH 086/109] Remove problem-solver source file --- include/pyhpp/core/fwd.hh | 1 - src/CMakeLists.txt | 1 - src/pyhpp/core/bindings.cc | 1 - src/pyhpp/core/problem-solver.cc | 230 ------------------------------- 4 files changed, 233 deletions(-) delete mode 100644 src/pyhpp/core/problem-solver.cc diff --git a/include/pyhpp/core/fwd.hh b/include/pyhpp/core/fwd.hh index 4204a2d2..64cc175c 100644 --- a/include/pyhpp/core/fwd.hh +++ b/include/pyhpp/core/fwd.hh @@ -58,7 +58,6 @@ void exposePathPlanner(); void exposeParameter(); void exposeProblem(); -void exposeProblemSolver(); // forward declaration of some classes class PathPlanner; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index eae87840..4a07250f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -121,7 +121,6 @@ add_python_library( pyhpp/core/problem-target.cc pyhpp/core/parameter.cc pyhpp/core/problem.cc - pyhpp/core/problem-solver.cc pyhpp/core/bindings.cc LINK_LIBRARIES hpp-core::hpp-core) diff --git a/src/pyhpp/core/bindings.cc b/src/pyhpp/core/bindings.cc index 8079a45b..3646a57f 100644 --- a/src/pyhpp/core/bindings.cc +++ b/src/pyhpp/core/bindings.cc @@ -44,7 +44,6 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::core::exposeParameter(); pyhpp::core::exposeProblem(); - pyhpp::core::exposeProblemSolver(); pyhpp::core::exposeConfigValidation(); pyhpp::core::exposeConfigurationShooter(); diff --git a/src/pyhpp/core/problem-solver.cc b/src/pyhpp/core/problem-solver.cc deleted file mode 100644 index c025c5a4..00000000 --- a/src/pyhpp/core/problem-solver.cc +++ /dev/null @@ -1,230 +0,0 @@ -// -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// DocNamespace(hpp::core) - -using namespace boost::python; - -#define PYHPP_PROBLEMSOLVER_SELECT_TYPE(type) \ - def(#type "Type", \ - static_cast( \ - &ProblemSolver::type##Type), \ - return_value_policy()) \ - .def(#type "Type", \ - static_cast( \ - &ProblemSolver::type##Type)) - -#define PYHPP_PROBLEMSOLVER_CONTAINER(name) \ - def_readwrite(#name, &ProblemSolver::name) - -namespace pyhpp { -namespace core { -using namespace hpp::core; - -// static list goalConfigs(ProblemSolver& ps) { -// const Configurations_t cfgs = ps.goalConfigs(); -// list ret; -// for (std::size_t i = 0; i < cfgs.size(); ++i) ret.append(*cfgs[i]); -// return ret; -// } -static PathVectorPtr_t path(ProblemSolver& ps, const std::size_t& i) { - if (i > ps.paths().size()) throw std::invalid_argument("Out of range"); - return ps.paths()[i]; -} - -template > -struct Builder { - Builder(PyObject* _callable) : callable(_callable) { incref(callable); } - - ~Builder() { decref(callable); } - - static TypePtr_t call2(PyObject* c, const hpp::core::ProblemConstPtr_t& p) { - object obj = call(c, p); - TypePtr_t ptr = extract(obj); - return ptr; - } - - template - static void add_to_container(hpp::core::Container& c, - const std::string& key, PyObject* callable) { - // TODO check if incref is needed - // incref (callable); - c.add(key, T(boost::bind(&Builder::call2, callable, - boost::placeholders::_1))); - } - - PyObject* callable; -}; - -struct NotABuilder { - template - static void add_to_container(hpp::core::Container& c, - const std::string& key, const T& value) { - c.add(key, value); - } -}; - -/// \tparam Builder_t void means not a Builder. -template -struct exposeContainer { - static void run(const char* name) { - typedef hpp::core::Container C_t; - class_(name, no_init) - .PYHPP_DEFINE_METHOD(C_t, erase) - .PYHPP_DEFINE_METHOD(C_t, clear) - .def("add", &Builder_t::template add_to_container) - .PYHPP_DEFINE_METHOD(C_t, has) - // .PYHPP_DEFINE_METHOD (C_t, get) - .def( - "__getitem__", - static_cast( - &C_t::get), - return_internal_reference<>()) - // .PYHPP_DEFINE_METHOD (C_t, getKeys) - .def("keys", &C_t::template getKeys >); - } -}; - -void exposeProblemSolver() { - // DocClass (ProblemSolver) - using namespace hpp::core; - class_("ProblemSolver", no_init) - .def("create", &ProblemSolver::create, - return_value_policy(), DocClassMethod(create)) - .staticmethod("create") - .def("problem", - static_cast( - &ProblemSolver::problem), - return_value_policy()) - // .def("initConfig", static_cast(&ProblemSolver::initConfig), - // return_internal_reference<>()) - .def("path", path) - .def("initConfig", - static_cast( - &ProblemSolver::initConfig), - return_internal_reference<>()) - .def("initConfig", - static_cast( - &ProblemSolver::initConfig)) - // .PYHPP_DEFINE_METHOD2(ProblemSolver, goalConfigs, - // DocClassMethod(goalConfigs)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, addGoalConfig, - DocClassMethod(addGoalConfig)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetGoalConfigs, - DocClassMethod(resetGoalConfigs)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetProblem, - DocClassMethod(resetProblem)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetRoadmap, - DocClassMethod(resetRoadmap)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, createPathOptimizers, - DocClassMethod(createPathOptimizers)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, prepareSolveStepByStep, - DocClassMethod(prepareSolveStepByStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, executeOneStep, - DocClassMethod(executeOneStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, finishSolveStepByStep, - DocClassMethod(finishSolveStepByStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, solve, DocClassMethod(solve)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, optimizePath, - DocClassMethod(optimizePath)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addPath, DocClassMethod(addPath)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, erasePath, DocClassMethod(erasePath)) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, paths) - .PYHPP_DEFINE_METHOD2(ProblemSolver, createRobot, - DocClassMethod(createRobot)) - .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(ProblemSolver, robot, - const DevicePtr_t&) - .def("addObstacle", - static_cast( - &ProblemSolver::addObstacle), - DocClassMethod(addObstacle)) - - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(robot) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(pathPlanner) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, pathPlanner) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(distance) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(steeringMethod) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(configurationShooter) - .def("pathValidationType", static_cast( - &ProblemSolver::pathValidationType)) - .def("pathProjectorType", static_cast( - &ProblemSolver::pathProjectorType)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addConfigValidation, - DocClassMethod(addConfigValidation)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, configValidationTypes, - DocClassMethod(configValidationTypes)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, clearConfigValidations, - DocClassMethod(clearConfigValidations)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addPathOptimizer, - DocClassMethod(addPathOptimizer)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, clearPathOptimizers, - DocClassMethod(clearPathOptimizers)) - //.PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, - // pathOptimizer) - - // .PYHPP_PROBLEMSOLVER_CONTAINER(robots) - .PYHPP_PROBLEMSOLVER_CONTAINER(pathPlanners) - .PYHPP_PROBLEMSOLVER_CONTAINER(pathOptimizers) - - .PYHPP_DEFINE_GETTER_SETTER(ProblemSolver, maxIterProjection, size_type) - .PYHPP_DEFINE_GETTER_SETTER(ProblemSolver, maxIterPathPlanning, size_type) - // .PYHPP_DEFINE_GETTER_SETTER (ProblemSolver, errorThreshold , - // value_type) - .def("errorThreshold", static_cast( - &ProblemSolver::errorThreshold)) - .def("errorThreshold", - static_cast( - &ProblemSolver::errorThreshold)); - - // exposeContainer(); - exposeContainer::run("PathPlannerContainer"); - exposeContainer >::run( - "PathOptimizerContainer"); -} -} // namespace core -} // namespace pyhpp From 5f606d1598b8cb24cfbddc844445a5d6369f0495 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 21 Jan 2026 18:36:32 +0100 Subject: [PATCH 087/109] Remove corba server references and unused files --- CMakeLists.txt | 1 - include/pyhpp/corbaserver/fwd.hh | 45 ------------ package.xml | 3 - src/CMakeLists.txt | 4 -- src/pyhpp/corbaserver/bindings.cc | 40 ----------- src/pyhpp/corbaserver/server.cc | 68 ------------------- .../corbaserver/wholebodyStep/bindings.cc | 41 ----------- src/pyhpp/corbaserver/wholebodyStep/server.cc | 64 ----------------- .../manipulation/constraint_graph_factory.py | 2 +- 9 files changed, 1 insertion(+), 267 deletions(-) delete mode 100644 include/pyhpp/corbaserver/fwd.hh delete mode 100644 src/pyhpp/corbaserver/bindings.cc delete mode 100644 src/pyhpp/corbaserver/server.cc delete mode 100644 src/pyhpp/corbaserver/wholebodyStep/bindings.cc delete mode 100644 src/pyhpp/corbaserver/wholebodyStep/server.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b2abe1..aeabfdbe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,6 @@ add_project_dependency(hpp-util REQUIRED) add_project_dependency(hpp-pinocchio REQUIRED) add_project_dependency(hpp-constraints REQUIRED) add_project_dependency(hpp-core REQUIRED) -add_project_dependency(hpp-corbaserver REQUIRED) add_project_dependency(hpp-manipulation REQUIRED) add_project_dependency(hpp-manipulation-urdf REQUIRED) diff --git a/include/pyhpp/corbaserver/fwd.hh b/include/pyhpp/corbaserver/fwd.hh deleted file mode 100644 index 28357e67..00000000 --- a/include/pyhpp/corbaserver/fwd.hh +++ /dev/null @@ -1,45 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#ifndef PYHPP_CORBASERVER_FWD_HH -#define PYHPP_CORBASERVER_FWD_HH - -#include - -namespace pyhpp { -namespace corbaserver { -void exposeServer(); -// void exposeProblemSolverMap(); -namespace wholebodyStep { -void exposeServer(); -} -} // namespace corbaserver -} // namespace pyhpp - -#endif // PYHPP_CORBASERVER_FWD_HH diff --git a/package.xml b/package.xml index bea53e45..cb904b19 100644 --- a/package.xml +++ b/package.xml @@ -28,7 +28,4 @@ hpp-core hpp-core hpp-core - hpp-corbaserver - hpp-corbaserver - hpp-corbaserver diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4a07250f..b45a2fb2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -144,10 +144,6 @@ add_python_library( pyhpp/core/path_optimization/spline-gradient-based-abstract.cc pyhpp/core/path_optimization/bindings.cc LINK_LIBRARIES hpp-core::hpp-core) -add_python_library( - pyhpp/corbaserver FILES pyhpp/corbaserver/bindings.cc - pyhpp/corbaserver/server.cc LINK_LIBRARIES hpp-corbaserver::hpp-corbaserver) - add_python_library( pyhpp/manipulation FILES diff --git a/src/pyhpp/corbaserver/bindings.cc b/src/pyhpp/corbaserver/bindings.cc deleted file mode 100644 index a6cff2e7..00000000 --- a/src/pyhpp/corbaserver/bindings.cc +++ /dev/null @@ -1,40 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -BOOST_PYTHON_MODULE(bindings) { - INIT_PYHPP_MODULE; - - boost::python::import("pyhpp.core"); - - pyhpp::corbaserver::exposeServer(); -} diff --git a/src/pyhpp/corbaserver/server.cc b/src/pyhpp/corbaserver/server.cc deleted file mode 100644 index c732982a..00000000 --- a/src/pyhpp/corbaserver/server.cc +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include - -using namespace boost::python; - -namespace pyhpp { -namespace corbaserver { -using hpp::core::ProblemSolverPtr_t; -using namespace hpp::corbaServer; - -struct SWrapper { - static Server* init1(ProblemSolverPtr_t ps, bool multithread = false) { - return new Server(ps, 0, NULL, multithread); - } - static Server* init2(ProblemSolverPtr_t ps, const char* name, - bool multithread = false) { - const char** argv = new const char*[2]; - const char* arg0 = "--name"; - argv[0] = arg0; - argv[1] = name; - Server* server = new Server(ps, 2, argv, multithread); - delete[] argv; - return server; - } -}; - -void exposeServer() { - class_("Server", init()) - .def("__init__", make_constructor(&SWrapper::init1)) - .def("__init__", make_constructor(&SWrapper::init2)) - .def("initialize", &Server::initialize) - .PYHPP_DEFINE_METHOD(Server, startCorbaServer) - .PYHPP_DEFINE_METHOD(Server, processRequest) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Server, mainContextId); -} -} // namespace corbaserver -} // namespace pyhpp diff --git a/src/pyhpp/corbaserver/wholebodyStep/bindings.cc b/src/pyhpp/corbaserver/wholebodyStep/bindings.cc deleted file mode 100644 index daa8d9e1..00000000 --- a/src/pyhpp/corbaserver/wholebodyStep/bindings.cc +++ /dev/null @@ -1,41 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -BOOST_PYTHON_MODULE(bindings) { - INIT_PYHPP_MODULE; - - boost::python::import("pyhpp.corbaserver"); - - pyhpp::corbaserver::wholebodyStep::exposeServer(); -} diff --git a/src/pyhpp/corbaserver/wholebodyStep/server.cc b/src/pyhpp/corbaserver/wholebodyStep/server.cc deleted file mode 100644 index 46c6b159..00000000 --- a/src/pyhpp/corbaserver/wholebodyStep/server.cc +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include -#include - -using namespace boost::python; - -namespace pyhpp { -namespace corbaserver { -namespace wholebodyStep { -typedef hpp::corbaServer::Server CorbaServer; -typedef hpp::wholebodyStep::Server WholebodyServer; - -struct SWrapper { - static WholebodyServer* init(CorbaServer* server, bool multithread = false) { - WholebodyServer* s = new WholebodyServer(0, NULL, multithread); - s->setProblemSolverMap(server->problemSolverMap()); - return s; - } - static void startCorbaServer(WholebodyServer* wbs, - const CorbaServer* server) { - wbs->startCorbaServer(server->mainContextId(), "corbaserver", - "wholebodyStep", "problem"); - } -}; - -void exposeServer() { - class_("Server", no_init) - .def("__init__", make_constructor(&SWrapper::init)) - .PYHPP_DEFINE_METHOD(SWrapper, startCorbaServer); -} -} // namespace wholebodyStep -} // namespace corbaserver -} // namespace pyhpp diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index c14d643f..1e537dbe 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -57,7 +57,7 @@ def __init__(self, grasps=[], pregrasps=[], numConstraints=[], lockedJoints=[]): warn( "argument lockedJoints in constructor of class " - + "hpp.corbaserver.manipulation.constraints.Constraints " + + "pyhpp.manipulation.constraint_graph_factory.Constraints " + "is deprecated. Locked joints are handled as numerical " + "constraints." ) From 60e17029169e8755e54a043cf61b2d89af3fa626 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Jan 2026 17:12:58 +0000 Subject: [PATCH 088/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.13 → v0.14.14](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.13...v0.14.14) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index cc1646b0..1d8156fa 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.13 + rev: v0.14.14 hooks: - id: ruff args: From 5dc741af20e54efcff8211fd084b24bb38c5fdd8 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 27 Jan 2026 11:10:53 +0100 Subject: [PATCH 089/109] Add missing visualModel getter to pinocchio device --- src/pyhpp/pinocchio/device.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index b439402c..6191c8fd 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -312,6 +312,7 @@ void exposeDevice() { .def("geomModel", &Device::geomModel, return_internal_reference<>()) .def("configSize", &Device::configSize, DocClassMethod(configSize)) .def("numberDof", &Device::numberDof, DocClassMethod(numberDof)) + .def("visualModel", &Device::visualModel, return_internal_reference<>()) .def("currentConfiguration", static_cast( From 435ecc2411dfbf591d412b8afddd897b551f65aa Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 28 Jan 2026 08:13:41 +0100 Subject: [PATCH 090/109] Deprecate Device::asPinDevice() --- src/pyhpp/manipulation/device.cc | 16 +++++++++------- src/pyhpp/manipulation/device.hh | 1 - 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index a68b273f..801afd89 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -56,12 +56,6 @@ std::map Device::grippers() { ->grippers.map; } -PinDevicePtr_t Device::asPinDevice() { - hpp::pinocchio::DevicePtr_t pinDevice = - std::dynamic_pointer_cast(obj); - return pinDevice; -} - void Device::setJointBounds(const char* jointName, boost::python::list py_jointBounds) { Frame frame = obj->getFrameByName(jointName); @@ -172,6 +166,14 @@ void exposeHandle() { .def(boost::python::map_indexing_suite, true>()); } +object asPinDevice(object self) { + PyErr_WarnEx(PyExc_DeprecationWarning, + "asPinDevice() is deprecated: manipulation.Device already " + "inherits from pinocchio.Device, use the object directly", + 1); + return self; +} + void exposeDevice() { // DocClass(Device) class_, boost::shared_ptr, @@ -179,7 +181,7 @@ void exposeDevice() { .def("setRobotRootPosition", &Device::setRobotRootPosition) .def("handles", &Device::handles) .def("grippers", &Device::grippers) - .def("asPinDevice", &Device::asPinDevice) + .def("asPinDevice", &asPinDevice) .def("getJointNames", &Device::getJointNames) .def("getJointConfig", &Device::getJointConfig) .def("setJointBounds", &Device::setJointBounds); diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 3b60042d..ed22d4db 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -80,7 +80,6 @@ struct Device : public pyhpp::pinocchio::Device { const Transform3s& positionWRTParentJoint); std::map handles(); std::map grippers(); - PinDevicePtr_t asPinDevice(); boost::python::list getJointConfig(const char* jointName); boost::python::list getJointNames(); void setJointBounds(const char* jointName, boost::python::list jointBounds); From af8225ac45508d914d3f680a387b113051f52d35 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 9 Feb 2026 17:54:22 +0000 Subject: [PATCH 091/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.14.14 → v0.15.0](https://github.com/astral-sh/ruff-pre-commit/compare/v0.14.14...v0.15.0) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1d8156fa..7f648ba1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.14.14 + rev: v0.15.0 hooks: - id: ruff args: From 4263c3127ead51ec97c880ec9908fe531591f939 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Tue, 10 Feb 2026 09:28:15 +0100 Subject: [PATCH 092/109] Add missing graph shared ptr type python exposition --- src/pyhpp/manipulation/graph.cc | 2 +- src/pyhpp/manipulation/graph.hh | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index 427575d3..bb682de2 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -1249,7 +1249,7 @@ void exposeGraph() { DocClassMethod(pathValidation)); // DocClass(Graph) - class_( + class_( "Graph", init()) diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index a01cadae..6840a038 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -227,6 +227,7 @@ struct PyWGraph { // Initialization void initialize(); }; +typedef std::shared_ptr PyWGraphPtr_t; } // namespace manipulation } // namespace pyhpp From e240229bb55f83a8176f0291eeb188ae7f0d23f6 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 11 Feb 2026 09:11:31 +0100 Subject: [PATCH 093/109] Fix constraint graph problem getter --- src/pyhpp/manipulation/problem.cc | 5 ++--- src/pyhpp/manipulation/problem.hh | 1 + 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index 30582044..df03e2a4 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -55,12 +55,11 @@ Problem::Problem(const hpp::manipulation::ProblemPtr_t& object) void Problem::constraintGraph(const PyWGraphPtr_t& graph) { asManipulationProblem()->constraintGraph(graph->obj); + graph_ = graph; } PyWGraphPtr_t Problem::constraintGraph() const { - pyhpp::manipulation::PyWGraph* graph = - new PyWGraph(asManipulationProblem()->constraintGraph()); - return std::shared_ptr(graph); + return graph_; } void Problem::checkProblem() const { asManipulationProblem()->checkProblem(); } diff --git a/src/pyhpp/manipulation/problem.hh b/src/pyhpp/manipulation/problem.hh index ae8df708..e04c165c 100644 --- a/src/pyhpp/manipulation/problem.hh +++ b/src/pyhpp/manipulation/problem.hh @@ -49,6 +49,7 @@ namespace manipulation { // Wrapper class for hpp::manipulation::Problem struct Problem : public pyhpp::core::Problem { hpp::core::Container jointAndShapes; + PyWGraphPtr_t graph_; Problem(const PyWDevicePtr_t& robot); Problem(const hpp::manipulation::ProblemPtr_t& object); From 0522d9cf462ee70f0df74413c06bd27e287e18dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 11 Feb 2026 08:14:05 +0000 Subject: [PATCH 094/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/manipulation/problem.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index df03e2a4..498daf70 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -58,9 +58,7 @@ void Problem::constraintGraph(const PyWGraphPtr_t& graph) { graph_ = graph; } -PyWGraphPtr_t Problem::constraintGraph() const { - return graph_; -} +PyWGraphPtr_t Problem::constraintGraph() const { return graph_; } void Problem::checkProblem() const { asManipulationProblem()->checkProblem(); } From 91176512006cf58c044279ad9ac00e27ebced5fd Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" Date: Fri, 13 Feb 2026 16:42:10 +0000 Subject: [PATCH 095/109] flake.lock: Update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Flake lock file updates: • Updated input 'gepetto': 'github:gepetto/nix/d1226cd' (2026-01-11) → 'github:gepetto/nix/9ff78e3' (2026-02-11) • Updated input 'gepetto/flake-parts': 'github:hercules-ci/flake-parts/250481a' (2026-01-05) → 'github:hercules-ci/flake-parts/5792860' (2026-02-02) • Updated input 'gepetto/flake-parts/nixpkgs-lib': 'github:nix-community/nixpkgs.lib/2075416' (2025-12-14) → 'github:nix-community/nixpkgs.lib/7271616' (2026-02-01) • Updated input 'gepetto/nix-ros-overlay': 'github:lopsided98/nix-ros-overlay/cbd23fc' (2026-01-10) → 'github:lopsided98/nix-ros-overlay/d1b9f17' (2026-02-09) • Updated input 'gepetto/src-agimus-controller': 'github:agimus-project/agimus_controller/5ef41cd' (2025-12-17) → 'github:agimus-project/agimus_controller/00ccabb' (2026-02-06) • Updated input 'gepetto/src-agimus-msgs': 'github:agimus-project/agimus_msgs/9f3bcea' (2026-01-09) → 'github:agimus-project/agimus_msgs/241d82d' (2026-02-09) • Updated input 'gepetto/src-franka-description': 'github:agimus-project/franka_description/2bdc8da' (2025-12-17) → 'github:agimus-project/franka_description/535aa51' (2026-02-06) • Updated input 'gepetto/system-manager': 'github:numtide/system-manager/2646bbc' (2026-01-10) → 'github:numtide/system-manager/442db31' (2026-02-09) • Added input 'gepetto/system-manager/userborn': 'github:jfroche/userborn/55c2cd7' (2026-02-06) • Added input 'gepetto/system-manager/userborn/flake-compat': 'github:edolstra/flake-compat/5edf11c' (2025-12-29) • Added input 'gepetto/system-manager/userborn/flake-parts': 'github:hercules-ci/flake-parts/80daad0' (2026-01-11) • Added input 'gepetto/system-manager/userborn/flake-parts/nixpkgs-lib': follows 'gepetto/system-manager/userborn/nixpkgs' • Added input 'gepetto/system-manager/userborn/nixpkgs': follows 'gepetto/system-manager/nixpkgs' • Added input 'gepetto/system-manager/userborn/pre-commit-hooks-nix': 'github:cachix/pre-commit-hooks.nix/a1ef738' (2026-01-22) • Added input 'gepetto/system-manager/userborn/pre-commit-hooks-nix/flake-compat': follows 'gepetto/system-manager/userborn/flake-compat' • Added input 'gepetto/system-manager/userborn/pre-commit-hooks-nix/gitignore': 'github:hercules-ci/gitignore.nix/637db32' (2024-02-28) • Added input 'gepetto/system-manager/userborn/pre-commit-hooks-nix/gitignore/nixpkgs': follows 'gepetto/system-manager/userborn/pre-commit-hooks-nix/nixpkgs' • Added input 'gepetto/system-manager/userborn/pre-commit-hooks-nix/nixpkgs': follows 'gepetto/system-manager/userborn/nixpkgs' • Added input 'gepetto/system-manager/userborn/systems': 'github:nix-systems/default/da67096' (2023-04-09) • Updated input 'gepetto/treefmt-nix': 'github:numtide/treefmt-nix/0c445aa' (2026-01-10) → 'github:numtide/treefmt-nix/337a4fe' (2026-02-04) --- flake.lock | 192 +++++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 164 insertions(+), 28 deletions(-) diff --git a/flake.lock b/flake.lock index f0240a3d..548cbd8d 100644 --- a/flake.lock +++ b/flake.lock @@ -1,15 +1,54 @@ { "nodes": { + "flake-compat": { + "flake": false, + "locked": { + "lastModified": 1767039857, + "narHash": "sha256-vNpUSpF5Nuw8xvDLj2KCwwksIbjua2LZCqhV1LNRDns=", + "owner": "edolstra", + "repo": "flake-compat", + "rev": "5edf11c44bc78a0d334f6334cdaf7d60d732daab", + "type": "github" + }, + "original": { + "owner": "edolstra", + "repo": "flake-compat", + "type": "github" + } + }, "flake-parts": { "inputs": { "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1767609335, - "narHash": "sha256-feveD98mQpptwrAEggBQKJTYbvwwglSbOv53uCfH9PY=", + "lastModified": 1769996383, + "narHash": "sha256-AnYjnFWgS49RlqX7LrC4uA+sCCDBj0Ry/WOJ5XWAsa0=", + "owner": "hercules-ci", + "repo": "flake-parts", + "rev": "57928607ea566b5db3ad13af0e57e921e6b12381", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "flake-parts", + "type": "github" + } + }, + "flake-parts_2": { + "inputs": { + "nixpkgs-lib": [ + "gepetto", + "system-manager", + "userborn", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1768135262, + "narHash": "sha256-PVvu7OqHBGWN16zSi6tEmPwwHQ4rLPU9Plvs8/1TUBY=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "250481aafeb741edfe23d29195671c19b36b6dca", + "rev": "80daad04eddbbf5a4d883996a73f3f542fa437ac", "type": "github" }, "original": { @@ -61,11 +100,11 @@ "treefmt-nix": "treefmt-nix" }, "locked": { - "lastModified": 1768097331, - "narHash": "sha256-RRY9GSpSiWfzo7P/v/R9xLJPdsvUzQdpuNq5gwAdFsM=", + "lastModified": 1770768792, + "narHash": "sha256-+XXMGK5AwI5n1QhiQB/yVC/AXCUha2MgFFXXgRMkk6s=", "owner": "gepetto", "repo": "nix", - "rev": "d1226cdb8ae6e489c28eb2d39dacf7fc905fce95", + "rev": "9ff78e3760efbd9f30232398674e71594fa79c00", "type": "github" }, "original": { @@ -74,17 +113,41 @@ "type": "github" } }, + "gitignore": { + "inputs": { + "nixpkgs": [ + "gepetto", + "system-manager", + "userborn", + "pre-commit-hooks-nix", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1709087332, + "narHash": "sha256-HG2cCnktfHsKV0s4XW83gU3F57gaTljL9KNSuG6bnQs=", + "owner": "hercules-ci", + "repo": "gitignore.nix", + "rev": "637db329424fd7e46cf4185293b9cc8c88c95394", + "type": "github" + }, + "original": { + "owner": "hercules-ci", + "repo": "gitignore.nix", + "type": "github" + } + }, "nix-ros-overlay": { "inputs": { "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1768042480, - "narHash": "sha256-MY81n7N8mn6We+PxGBq9eJJweg/j0vgEDhKLp9gs4IM=", + "lastModified": 1770622967, + "narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "cbd23fc7766de218051c79d4df6848303a915ee0", + "rev": "d1b9f17eba909116356436d46b5192d299c6b49a", "type": "github" }, "original": { @@ -133,11 +196,11 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1765674936, - "narHash": "sha256-k00uTP4JNfmejrCLJOwdObYC9jHRrr/5M/a/8L2EIdo=", + "lastModified": 1769909678, + "narHash": "sha256-cBEymOf4/o3FD5AZnzC3J9hLbiZ+QDT/KDuyHXVJOpM=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "2075416fcb47225d9b68ac469a5c4801a9c4dd85", + "rev": "72716169fe93074c333e8d0173151350670b824c", "type": "github" }, "original": { @@ -146,6 +209,36 @@ "type": "github" } }, + "pre-commit-hooks-nix": { + "inputs": { + "flake-compat": [ + "gepetto", + "system-manager", + "userborn", + "flake-compat" + ], + "gitignore": "gitignore", + "nixpkgs": [ + "gepetto", + "system-manager", + "userborn", + "nixpkgs" + ] + }, + "locked": { + "lastModified": 1769069492, + "narHash": "sha256-Efs3VUPelRduf3PpfPP2ovEB4CXT7vHf8W+xc49RL/U=", + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "rev": "a1ef738813b15cf8ec759bdff5761b027e3e1d23", + "type": "github" + }, + "original": { + "owner": "cachix", + "repo": "pre-commit-hooks.nix", + "type": "github" + } + }, "root": { "inputs": { "flake-parts": [ @@ -174,11 +267,11 @@ "src-agimus-controller": { "flake": false, "locked": { - "lastModified": 1765937017, - "narHash": "sha256-aAURRGwKom7pssu48I1io8giykUMNftNGTv72YIH5n8=", + "lastModified": 1770376027, + "narHash": "sha256-ZSOgLgsBS73d0gVbna2WzWkC7fhORPu3/KaktOr8CzA=", "owner": "agimus-project", "repo": "agimus_controller", - "rev": "5ef41cd0a64feeaafdb7bdebe0f0308fa07bf734", + "rev": "00ccabbad6ab4a55bad18c094e8fc608fd8e0aed", "type": "github" }, "original": { @@ -190,11 +283,11 @@ "src-agimus-msgs": { "flake": false, "locked": { - "lastModified": 1767943618, - "narHash": "sha256-zOaDYbuG46SzNCjBibzIgsIkZkPpg7wjxTQ1ETVVe+g=", + "lastModified": 1770623573, + "narHash": "sha256-gOTxAqOKx7B+tpaDrhEOf+0afWUBQj3RVXeIDZb0+nE=", "owner": "agimus-project", "repo": "agimus_msgs", - "rev": "9f3bcea94b5824b06530bc006b9afd2590a37db0", + "rev": "241d82d5bcffac00de83397c29347ab5bce45088", "type": "github" }, "original": { @@ -206,11 +299,11 @@ "src-franka-description": { "flake": false, "locked": { - "lastModified": 1765937047, - "narHash": "sha256-mTIQJnQLZN3UMyPq7Btu6SYWv8JuXMSjdkzXbRdGEh4=", + "lastModified": 1770385506, + "narHash": "sha256-A+UFglPEHA2deOI06EyG+NF1+ejMRRAOEUj4yldDo/o=", "owner": "agimus-project", "repo": "franka_description", - "rev": "2bdc8da187040d2e85da9d5d302698ff30de3d11", + "rev": "535aa51107bf990abd92404d2517b09e4b6417a3", "type": "github" }, "original": { @@ -258,14 +351,15 @@ "nixpkgs": [ "gepetto", "nixpkgs" - ] + ], + "userborn": "userborn" }, "locked": { - "lastModified": 1768082042, - "narHash": "sha256-6HjFn+tJi2C4xByHD0CZwPSnRHgKZNVqVCYRDdVHvSA=", + "lastModified": 1770630329, + "narHash": "sha256-Q0/JNws9SxY9wE/mCQ6WTA85MIgrJ+M0HDcJVUlk9Ds=", "owner": "numtide", "repo": "system-manager", - "rev": "2646bbcb730fbd2a38d3d20948373ec5cd6271ef", + "rev": "442db31401bdbdadc59356232153448e05bce1db", "type": "github" }, "original": { @@ -289,6 +383,21 @@ "type": "github" } }, + "systems_2": { + "locked": { + "lastModified": 1681028828, + "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", + "owner": "nix-systems", + "repo": "default", + "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", + "type": "github" + }, + "original": { + "owner": "nix-systems", + "repo": "default", + "type": "github" + } + }, "treefmt-nix": { "inputs": { "nixpkgs": [ @@ -297,11 +406,11 @@ ] }, "locked": { - "lastModified": 1768031762, - "narHash": "sha256-b2gJDJfi+TbA7Hu2sKip+1mWqya0GJaWrrXQjpbOVTU=", + "lastModified": 1770228511, + "narHash": "sha256-wQ6NJSuFqAEmIg2VMnLdCnUc0b7vslUohqqGGD+Fyxk=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "0c445aa21b01fd1d4bb58927f7b268568af87b20", + "rev": "337a4fe074be1042a35086f15481d763b8ddc0e7", "type": "github" }, "original": { @@ -309,6 +418,33 @@ "repo": "treefmt-nix", "type": "github" } + }, + "userborn": { + "inputs": { + "flake-compat": "flake-compat", + "flake-parts": "flake-parts_2", + "nixpkgs": [ + "gepetto", + "system-manager", + "nixpkgs" + ], + "pre-commit-hooks-nix": "pre-commit-hooks-nix", + "systems": "systems_2" + }, + "locked": { + "lastModified": 1770377964, + "narHash": "sha256-q2pnlX2IW0kg80GLFnwWd/GigIpkuZnyKPLhrgJql3E=", + "owner": "jfroche", + "repo": "userborn", + "rev": "55c2cd7952c207a62736a5bbd9499ea73da18d24", + "type": "github" + }, + "original": { + "owner": "jfroche", + "ref": "system-manager", + "repo": "userborn", + "type": "github" + } } }, "root": "root", From de55627277290b2924f290c58e0d389db7118454 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 16 Feb 2026 17:11:31 +0000 Subject: [PATCH 096/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.15.0 → v0.15.1](https://github.com/astral-sh/ruff-pre-commit/compare/v0.15.0...v0.15.1) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7f648ba1..2d1c4d18 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.0 + rev: v0.15.1 hooks: - id: ruff args: From 2477dde5f8e5e9eaa590660a84be60ba041579b4 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 18 Feb 2026 13:32:27 +0100 Subject: [PATCH 097/109] Remove dead PYTHON_UNIT_TESTS code from tests CMakeLists --- tests/CMakeLists.txt | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 678a5f29..eb6e472b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,21 +12,6 @@ # details. You should have received a copy of the GNU Lesser General Public # License along with hpp-python If not, see . -# ============================================================================= -# 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) -endforeach() - -set_tests_properties( - ${PYTHON_UNIT_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) # ============================================================================= From 00cbd3e15e8bf986aabf8682b49753ad2d4bbf35 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Mon, 23 Feb 2026 14:27:48 +0100 Subject: [PATCH 098/109] Expose contact surfaces from device --- src/pyhpp/manipulation/device.cc | 49 +++++++++++++++++++++++++++++++- src/pyhpp/manipulation/device.hh | 2 ++ 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 801afd89..360fc214 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -84,6 +84,49 @@ void Device::setJointBounds(const char* jointName, joint->upperBound(i, vMax); // Set upper bound for DOF i } } + +boost::python::list Device::contactSurfaceNames() { + auto manipDevice = + std::dynamic_pointer_cast(obj); + boost::python::list result; + for (const auto& entry : manipDevice->jointAndShapes.map) { + result.append(entry.first); + } + return result; +} + +boost::python::dict Device::contactSurfaces() { + auto manipDevice = + std::dynamic_pointer_cast(obj); + boost::python::dict result; + + for (const auto& entry : manipDevice->jointAndShapes.map) { + const std::string& name = entry.first; + const auto& jointAndShapes = entry.second; + boost::python::list surfacesList; + + for (const auto& jas : jointAndShapes) { + boost::python::dict surfaceDict; + // jas.first is JointPtr_t, jas.second is Shape_t (vector) + std::string jointName = + jas.first ? jas.first->name() : "universe"; + surfaceDict["joint"] = jointName; + + boost::python::list points; + for (const auto& pt : jas.second) { + boost::python::list point; + point.append(pt[0]); + point.append(pt[1]); + point.append(pt[2]); + points.append(point); + } + surfaceDict["points"] = points; + surfacesList.append(surfaceDict); + } + result[name] = surfacesList; + } + return result; +} boost::python::list Device::getJointConfig(const char* jointName) { try { Frame frame = obj->getFrameByName(jointName); @@ -184,7 +227,11 @@ void exposeDevice() { .def("asPinDevice", &asPinDevice) .def("getJointNames", &Device::getJointNames) .def("getJointConfig", &Device::getJointConfig) - .def("setJointBounds", &Device::setJointBounds); + .def("setJointBounds", &Device::setJointBounds) + .def("contactSurfaceNames", &Device::contactSurfaceNames, + "Return list of contact surface names registered on device") + .def("contactSurfaces", &Device::contactSurfaces, + "Return dict mapping surface names to list of {joint, points}"); } } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index ed22d4db..23cabda2 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -83,6 +83,8 @@ struct Device : public pyhpp::pinocchio::Device { boost::python::list getJointConfig(const char* jointName); boost::python::list getJointNames(); void setJointBounds(const char* jointName, boost::python::list jointBounds); + boost::python::list contactSurfaceNames(); + boost::python::dict contactSurfaces(); }; // struct Device } // namespace manipulation } // namespace pyhpp From 545d29e41046586a1ba69ec44a893f131752da45 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 23 Feb 2026 14:03:00 +0000 Subject: [PATCH 099/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/manipulation/device.cc | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 360fc214..9c802013 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -86,8 +86,7 @@ void Device::setJointBounds(const char* jointName, } boost::python::list Device::contactSurfaceNames() { - auto manipDevice = - std::dynamic_pointer_cast(obj); + auto manipDevice = std::dynamic_pointer_cast(obj); boost::python::list result; for (const auto& entry : manipDevice->jointAndShapes.map) { result.append(entry.first); @@ -96,8 +95,7 @@ boost::python::list Device::contactSurfaceNames() { } boost::python::dict Device::contactSurfaces() { - auto manipDevice = - std::dynamic_pointer_cast(obj); + auto manipDevice = std::dynamic_pointer_cast(obj); boost::python::dict result; for (const auto& entry : manipDevice->jointAndShapes.map) { @@ -108,8 +106,7 @@ boost::python::dict Device::contactSurfaces() { for (const auto& jas : jointAndShapes) { boost::python::dict surfaceDict; // jas.first is JointPtr_t, jas.second is Shape_t (vector) - std::string jointName = - jas.first ? jas.first->name() : "universe"; + std::string jointName = jas.first ? jas.first->name() : "universe"; surfaceDict["joint"] = jointName; boost::python::list points; From 12a8d2b919bcb284fbdf37e71503860527f8b163 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 23 Feb 2026 17:17:45 +0000 Subject: [PATCH 100/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.15.1 → v0.15.2](https://github.com/astral-sh/ruff-pre-commit/compare/v0.15.1...v0.15.2) --- .pre-commit-config.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2d1c4d18..0a0c0dc1 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.1 + rev: v0.15.2 hooks: - id: ruff args: From 7947e6681dbdc8c1b1ebac3a553db0f05da95da7 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 2 Mar 2026 17:19:08 +0000 Subject: [PATCH 101/109] [pre-commit.ci] pre-commit autoupdate MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit updates: - [github.com/astral-sh/ruff-pre-commit: v0.15.2 → v0.15.4](https://github.com/astral-sh/ruff-pre-commit/compare/v0.15.2...v0.15.4) - [github.com/pre-commit/mirrors-clang-format: v21.1.8 → v22.1.0](https://github.com/pre-commit/mirrors-clang-format/compare/v21.1.8...v22.1.0) --- .pre-commit-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 0a0c0dc1..97937e8e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,7 +2,7 @@ ci: autoupdate_branch: devel repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.2 + rev: v0.15.4 hooks: - id: ruff args: @@ -18,7 +18,7 @@ repos: hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.8 + rev: v22.1.0 hooks: - id: clang-format args: From 2fbe19a004807e23f12b0242511987d1e7b8ed45 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 4 Mar 2026 15:08:07 +0100 Subject: [PATCH 102/109] [pyhpp.pinocchio] Bind method Device::removeJoints --- src/pyhpp/pinocchio/device.cc | 3 ++- src/pyhpp/pinocchio/device.hh | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 6191c8fd..64a04ea9 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -332,7 +332,8 @@ void exposeDevice() { .def("setJointBounds", &setJointBounds) .def("getCenterOfMass", &getCenterOfMass) .def("getJointPosition", &getJointPosition) - .def("getJointsPosition", &getJointsPosition); + .def("getJointsPosition", &getJointsPosition) + .def("removeJoints", &Device::removeJoints); register_device_converters(); } } // namespace pinocchio diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 1b59d284..3627b797 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/src/pyhpp/pinocchio/device.hh @@ -54,7 +54,8 @@ struct Device { obj->computeFramesForwardKinematics(); } void updateGeometryPlacements() { obj->updateGeometryPlacements(); } - + void removeJoints(const std::vector& jointNames, + Configuration_t q) { obj->removeJoints(jointNames, q); } hpp::manipulation::DevicePtr_t asManipulationDevice() const { auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj); if (!manipDevice) { From 70800519699e5f2cda539d4a8bac66bdef71b6df Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Wed, 4 Mar 2026 15:07:34 +0100 Subject: [PATCH 103/109] [manipulation.Device] Add methods to add handles and grippers. Bind approachingDirection setter and getter. --- src/pyhpp/manipulation/device.cc | 99 ++++++++++++++++++++++++++++++-- src/pyhpp/manipulation/device.hh | 7 +++ src/pyhpp/pinocchio/device.cc | 3 +- 3 files changed, 103 insertions(+), 6 deletions(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9c802013..9291d511 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -124,13 +124,80 @@ boost::python::dict Device::contactSurfaces() { } return result; } + +void Device::addHandle(const std::string& linkName, const std::string& handleName, + const Transform3s& pose, value_type clearance, const std::vector& mask){ + hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); + if (!robot) { + throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); + } + if (mask.size () != 6) { + throw std::logic_error("mask should be of size 6"); + } + JointPtr_t joint = robot->getJointByBodyName(linkName); + + const ::pinocchio::Frame& linkFrame = + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + assert(linkFrame.type == ::pinocchio::BODY); + + hpp::pinocchio::JointIndex index(0); + std::string jointName("universe"); + if (joint) { + index = joint->index(); + jointName = joint->name(); + } + HandlePtr_t handle = + Handle::create(handleName, linkFrame.placement * pose, robot, joint); + handle->clearance(clearance); + handle->mask(mask); + robot->handles.add(handleName, handle); + assert(robot->model().existFrame(jointName)); + FrameIndex previousFrame(robot->model().getFrameId(jointName)); + robot->model().addFrame(::pinocchio::Frame(handleName, index, previousFrame, + linkFrame.placement * pose, + ::pinocchio::OP_FRAME)); + // Recreate pinocchio data after modifying model + robot->createData(); +} + +void Device::addGripper(const std::string& linkName, const std::string& gripperName, + const Transform3s& pose, value_type clearance) { + hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); + if (!robot) { + throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); + } + JointPtr_t joint = robot->getJointByBodyName(linkName); + + const ::pinocchio::Frame& linkFrame = + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + assert(linkFrame.type == ::pinocchio::BODY); + + hpp::pinocchio::JointIndex index(0); + std::string jointName("universe"); + if (joint) { + index = joint->index(); + jointName = joint->name(); + } + assert(robot->model().existFrame(jointName)); + FrameIndex previousFrame(robot->model().getFrameId(jointName)); + robot->model().addFrame(::pinocchio::Frame(gripperName, index, previousFrame, + linkFrame.placement * pose, ::pinocchio::OP_FRAME)); + // Recreate pinocchio data after modifying model + robot->createData(); + GripperPtr_t gripper = Gripper::create(gripperName, robot); + gripper->clearance(clearance); + robot->grippers.add(gripperName, gripper); +} + boost::python::list Device::getJointConfig(const char* jointName) { try { Frame frame = obj->getFrameByName(jointName); if (frame.isFixed()) return boost::python::list(); JointPtr_t joint = frame.joint(); if (!joint) return boost::python::list(); - hpp::core::vector_t config = obj->currentConfiguration(); + vector_t config = obj->currentConfiguration(); size_type ric = joint->rankInConfiguration(); size_type dim = joint->configSize(); auto segment = config.segment(ric, dim); @@ -181,11 +248,17 @@ void setHandleMaskComp(const HandlePtr_t& handle, handle->maskComp(mask); } +vector3_t getApproachingDirection(const HandlePtr_t& handle) { + return handle->approachingDirection(); +} + +void setApproachingDirection(const HandlePtr_t& handle, const vector3_t& dir) { + handle->approachingDirection(dir); +} + void exposeHandle() { // DocClass(Handle) class_("Handle", no_init) - .def("create", &Handle::create, DocClassMethod(create)) - .staticmethod("create") .add_property("name", &getHandleName, &setHandleName) .add_property("localPosition", &getHandleLocalPosition, &setHandleLocalPosition) @@ -195,6 +268,7 @@ void exposeHandle() { "clearance", static_cast(&Handle::clearance), static_cast(&Handle::clearance)) + .add_property("approachingDirection", &getApproachingDirection, &setApproachingDirection) .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) .def("createPreGrasp", &Handle::createPreGrasp, DocClassMethod(createPreGrasp)) @@ -228,7 +302,24 @@ void exposeDevice() { .def("contactSurfaceNames", &Device::contactSurfaceNames, "Return list of contact surface names registered on device") .def("contactSurfaces", &Device::contactSurfaces, - "Return dict mapping surface names to list of {joint, points}"); + "Return dict mapping surface names to list of {joint, points}") + .def("addHandle", &Device::addHandle, + "Add a handle to the kinematic chain\n\n" + " input\n" + " linkName: name of the link the handle is attached to,\n" + " handleName: name of the handle,\n" + " pose: pose of the handle in the link frame (SE3),\n" + " clearance: clearance of the handle, the sum of handle and gripper clearances\n" + " defines the distance between pregrasp and grasp,\n" + " mask: list of 6 Boolean use to define symmetries in the grasp constraint.") + .def("addGripper", &Device::addGripper, + "Add a gripper to the kinematic chain\n\n" + " input\n" + " linkName: name of the link the handle is attached to,\n" + " gripperName: name of the gripper,\n" + " pose: pose of the gripper in the link frame (SE3),\n" + " clearance: clearance of the gripper, the sum of handle and gripper clearances\n" + " defines the distance between pregrasp and grasp."); } } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 23cabda2..596140f1 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -54,6 +54,8 @@ typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; typedef hpp::pinocchio::GeomModel GeomModel; typedef hpp::pinocchio::Configuration_t Configuration_t; typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; +typedef hpp::pinocchio::vector_t vector_t; +typedef hpp::pinocchio::vector3_t vector3_t; typedef hpp::pinocchio::size_type size_type; typedef hpp::pinocchio::value_type value_type; typedef hpp::pinocchio::Transform3s Transform3s; @@ -65,6 +67,7 @@ typedef hpp::manipulation::Handle Handle; typedef hpp::manipulation::DevicePtr_t DevicePtr_t; typedef hpp::pinocchio::DevicePtr_t PinDevicePtr_t; typedef hpp::pinocchio::Frame Frame; +typedef hpp::pinocchio::FrameIndex FrameIndex; typedef hpp::pinocchio::Joint Joint; typedef hpp::pinocchio::JointPtr_t JointPtr_t; @@ -85,6 +88,10 @@ struct Device : public pyhpp::pinocchio::Device { void setJointBounds(const char* jointName, boost::python::list jointBounds); boost::python::list contactSurfaceNames(); boost::python::dict contactSurfaces(); + void addHandle(const std::string& linkName, const std::string& handleName, + const Transform3s& pose, value_type clearance, const std::vector& mask); + void addGripper(const std::string& linkName, const std::string& gripperName, + const Transform3s& pose, value_type clearance); }; // struct Device } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 64a04ea9..4d1f13d0 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -59,6 +59,7 @@ namespace pyhpp { namespace pinocchio { namespace bp = boost::python; +typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; typedef hpp::pinocchio::GripperPtr_t GripperPtr_t; typedef hpp::pinocchio::Gripper Gripper; typedef hpp::pinocchio::FrameIndex FrameIndex; @@ -215,8 +216,6 @@ static boost::python::list getJointsPosition( void exposeGripper() { // DocClass(Gripper) class_("Gripper", no_init) - .def("create", &Gripper::create, DocClassMethod(create)) - .staticmethod("create") .add_property("localPosition", &getObjectPositionInJoint) .add_property( "clearance", From 81cc7ced98984aa83b44c05fd88c3dda375f91a7 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 6 Mar 2026 15:23:07 +0100 Subject: [PATCH 104/109] [Gripper, Handle] Add a method to get the parent joint index in pinocchio model This is particularly useful to define RelativeTransformation constraints. Also add missing accessor to gripper name. --- src/pyhpp/manipulation/device.cc | 8 ++++++++ src/pyhpp/manipulation/device.hh | 1 + src/pyhpp/pinocchio/device.cc | 14 +++++++++++++- src/pyhpp/pinocchio/device.hh | 1 + 4 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9291d511..2c9b4b0b 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -256,6 +256,12 @@ void setApproachingDirection(const HandlePtr_t& handle, const vector3_t& dir) { handle->approachingDirection(dir); } +static JointIndex getParentJointId(const HandlePtr_t& handle) { + assert(handle->robot()); + Model model(handle->robot()->model()); + return model.frames[model.getFrameId(handle->name())].parentJoint; +} + void exposeHandle() { // DocClass(Handle) class_("Handle", no_init) @@ -270,6 +276,8 @@ void exposeHandle() { static_cast(&Handle::clearance)) .add_property("approachingDirection", &getApproachingDirection, &setApproachingDirection) .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) + .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to" + " in pinocchio Model") .def("createPreGrasp", &Handle::createPreGrasp, DocClassMethod(createPreGrasp)) .def("createGraspComplement", &Handle::createGraspComplement, diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 596140f1..76edb15f 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -69,6 +69,7 @@ typedef hpp::pinocchio::DevicePtr_t PinDevicePtr_t; typedef hpp::pinocchio::Frame Frame; typedef hpp::pinocchio::FrameIndex FrameIndex; typedef hpp::pinocchio::Joint Joint; +typedef hpp::pinocchio::JointIndex JointIndex; typedef hpp::pinocchio::JointPtr_t JointPtr_t; // Wrapper class to hpp::manipulation::Device diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 4d1f13d0..e32c8724 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -213,6 +213,15 @@ static boost::python::list getJointsPosition( } } +static std::string getGripperName(const GripperPtr_t& gripper) { return gripper->name(); } + +static JointIndex getParentJointId(const GripperPtr_t& gripper) { + assert(gripper->joint()); + assert(gripper->joint()->robot()); + Model model(gripper->joint()->robot()->model()); + return model.frames[model.getFrameId(gripper->name())].parentJoint; +} + void exposeGripper() { // DocClass(Gripper) class_("Gripper", no_init) @@ -221,7 +230,10 @@ void exposeGripper() { "clearance", static_cast(&Gripper::clearance), static_cast( - &Gripper::clearance)); + &Gripper::clearance)) + .def("name", &getGripperName) + .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to" + " in pinocchio Model"); class_ >("GripperMap") .def( boost::python::map_indexing_suite, diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index 3627b797..cefb3775 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/src/pyhpp/pinocchio/device.hh @@ -21,6 +21,7 @@ typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; typedef hpp::pinocchio::GeomModel GeomModel; typedef hpp::pinocchio::Configuration_t Configuration_t; typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; +typedef hpp::pinocchio::JointIndex JointIndex; typedef hpp::pinocchio::size_type size_type; typedef hpp::pinocchio::Transform3s Transform3s; typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; From 8bff6335b0c57e5cb4a49e19818bfafcce3192b0 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Thu, 19 Feb 2026 09:39:47 +0100 Subject: [PATCH 105/109] drop submodule --- .gitmodules | 3 --- cmake | 1 - 2 files changed, 4 deletions(-) delete mode 100644 .gitmodules delete mode 160000 cmake diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 2b7a4fe1..00000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "cmake"] - path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules.git diff --git a/cmake b/cmake deleted file mode 160000 index 8456ad46..00000000 --- a/cmake +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8456ad466d18b415b3c6ddd4c21f712c83160315 From 24d1864c678807fb11d8a99bd42cf933fc2f4b85 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 6 Mar 2026 21:31:17 +0100 Subject: [PATCH 106/109] update packaging --- .github/workflows/nix.yml | 1 + .github/workflows/update-flake-lock.yml | 28 ++- .mergify.yml | 1 + flake.lock | 291 +++++++++++------------- flake.nix | 42 ++-- 5 files changed, 164 insertions(+), 199 deletions(-) diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml index 18849861..e0a43639 100644 --- a/.github/workflows/nix.yml +++ b/.github/workflows/nix.yml @@ -24,6 +24,7 @@ jobs: name: gepetto authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' - run: nix flake check -L + - run: nix build -L check: if: always() name: check-macos-linux-nix diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml index 07c09291..bca13295 100644 --- a/.github/workflows/update-flake-lock.yml +++ b/.github/workflows/update-flake-lock.yml @@ -6,14 +6,28 @@ on: - cron: '0 16 13 * *' jobs: - lockfile: - runs-on: ubuntu-latest + update-flake-inputs: + runs-on: ubuntu-slim + permissions: + contents: write + pull-requests: write steps: + - name: Generate GitHub App Token + id: app-token + uses: actions/create-github-app-token@v2 + with: + app-id: ${{ secrets.GEPETTO_NIX_APP_ID }} + private-key: ${{ secrets.GEPETTO_NIX_APP_PRIVATE_KEY }} - name: Checkout repository uses: actions/checkout@v6 - - name: Install Nix - uses: DeterminateSystems/nix-installer-action@main - - name: Update flake.lock - uses: DeterminateSystems/update-flake-lock@main with: - token: ${{ secrets.GH_TOKEN_FOR_UPDATES }} + token: ${{ steps.app-token.outputs.token }} + - name: Setup Nix + uses: cachix/install-nix-action@v31 + - name: Update flake inputs + uses: mic92/update-flake-inputs@v1 + with: + github-token: ${{ steps.app-token.outputs.token }} + pr-labels: 'no-changelog' + git-author-name: 'hrp2-14' + git-author-email: '40568249+hrp2-14@users.noreply.github.com' diff --git a/.mergify.yml b/.mergify.yml index 1e2dd5a7..8ece7ea1 100644 --- a/.mergify.yml +++ b/.mergify.yml @@ -5,6 +5,7 @@ pull_request_rules: - check-success = "pre-commit.ci - pr" - or: - author = dependabot[bot] + - author = gepetto-flake-updater[bot] - author = github-actions[bot] - author = hrp2-14 - author = pre-commit-ci[bot] diff --git a/flake.lock b/flake.lock index 548cbd8d..a1c93df9 100644 --- a/flake.lock +++ b/flake.lock @@ -1,21 +1,5 @@ { "nodes": { - "flake-compat": { - "flake": false, - "locked": { - "lastModified": 1767039857, - "narHash": "sha256-vNpUSpF5Nuw8xvDLj2KCwwksIbjua2LZCqhV1LNRDns=", - "owner": "edolstra", - "repo": "flake-compat", - "rev": "5edf11c44bc78a0d334f6334cdaf7d60d732daab", - "type": "github" - }, - "original": { - "owner": "edolstra", - "repo": "flake-compat", - "type": "github" - } - }, "flake-parts": { "inputs": { "nixpkgs-lib": "nixpkgs-lib" @@ -34,29 +18,6 @@ "type": "github" } }, - "flake-parts_2": { - "inputs": { - "nixpkgs-lib": [ - "gepetto", - "system-manager", - "userborn", - "nixpkgs" - ] - }, - "locked": { - "lastModified": 1768135262, - "narHash": "sha256-PVvu7OqHBGWN16zSi6tEmPwwHQ4rLPU9Plvs8/1TUBY=", - "owner": "hercules-ci", - "repo": "flake-parts", - "rev": "80daad04eddbbf5a4d883996a73f3f542fa437ac", - "type": "github" - }, - "original": { - "owner": "hercules-ci", - "repo": "flake-parts", - "type": "github" - } - }, "flake-utils": { "inputs": { "systems": "systems" @@ -75,65 +36,102 @@ "type": "github" } }, - "gepetto": { + "gazebros2nix": { "inputs": { "flake-parts": "flake-parts", + "gepetto-lib": "gepetto-lib", "nix-ros-overlay": "nix-ros-overlay", - "nix-system-graphics": "nix-system-graphics", "nixpkgs": [ "gepetto", + "gazebros2nix", "nix-ros-overlay", "nixpkgs" ], - "src-agimus-controller": "src-agimus-controller", - "src-agimus-msgs": "src-agimus-msgs", - "src-franka-description": "src-franka-description", - "src-odri-control-interface": "src-odri-control-interface", - "src-odri-masterboard-sdk": "src-odri-masterboard-sdk", - "system-manager": "system-manager", + "pyproject-build-systems": "pyproject-build-systems", + "pyproject-nix": "pyproject-nix", "systems": [ "gepetto", + "gazebros2nix", "nix-ros-overlay", "flake-utils", "systems" ], - "treefmt-nix": "treefmt-nix" + "treefmt-nix": "treefmt-nix", + "uv2nix": "uv2nix" }, "locked": { - "lastModified": 1770768792, - "narHash": "sha256-+XXMGK5AwI5n1QhiQB/yVC/AXCUha2MgFFXXgRMkk6s=", + "lastModified": 1772816781, + "narHash": "sha256-Ac0KEl+8ygy+BnDgczNHgTumw8HpCasp/zJU5Yx3kQs=", "owner": "gepetto", - "repo": "nix", - "rev": "9ff78e3760efbd9f30232398674e71594fa79c00", + "repo": "gazebros2nix", + "rev": "ea8aff2fca6d45fa85fe5e90ef3c71fe0fcc0d12", "type": "github" }, "original": { "owner": "gepetto", - "repo": "nix", + "repo": "gazebros2nix", "type": "github" } }, - "gitignore": { + "gepetto": { "inputs": { + "flake-parts": [ + "gepetto", + "gazebros2nix", + "flake-parts" + ], + "gazebros2nix": "gazebros2nix", + "nix-ros-overlay": [ + "gepetto", + "gazebros2nix", + "nix-ros-overlay" + ], + "nix-system-graphics": "nix-system-graphics", "nixpkgs": [ "gepetto", - "system-manager", - "userborn", - "pre-commit-hooks-nix", + "gazebros2nix", "nixpkgs" + ], + "src-odri-control-interface": "src-odri-control-interface", + "src-odri-masterboard-sdk": "src-odri-masterboard-sdk", + "system-manager": "system-manager", + "systems": [ + "gepetto", + "gazebros2nix", + "systems" + ], + "treefmt-nix": [ + "gepetto", + "gazebros2nix", + "treefmt-nix" ] }, "locked": { - "lastModified": 1709087332, - "narHash": "sha256-HG2cCnktfHsKV0s4XW83gU3F57gaTljL9KNSuG6bnQs=", - "owner": "hercules-ci", - "repo": "gitignore.nix", - "rev": "637db329424fd7e46cf4185293b9cc8c88c95394", + "lastModified": 1772825496, + "narHash": "sha256-ZCgGWufV1suEVlft03k9TGOD190kGRCA3rrO8qsjeQ0=", + "owner": "gepetto", + "repo": "nix", + "rev": "5c1a5edffd02c51e267c42f8dfd36a13c7817950", "type": "github" }, "original": { - "owner": "hercules-ci", - "repo": "gitignore.nix", + "owner": "gepetto", + "repo": "nix", + "type": "github" + } + }, + "gepetto-lib": { + "locked": { + "lastModified": 1770945346, + "narHash": "sha256-L88f+oJbpIkMm9Ln1GP9SFyGztMvnOowbdshQHBeGGs=", + "owner": "Gepetto", + "repo": "nix-lib", + "rev": "82ef58cdf50514f6b1fde96b9d5b38fd8d3e83f5", + "type": "github" + }, + "original": { + "owner": "Gepetto", + "repo": "nix-lib", "type": "github" } }, @@ -143,11 +141,11 @@ "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1770622967, - "narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=", + "lastModified": 1771885942, + "narHash": "sha256-TlBFvE4YHNlbhKVkayP/FWBNAAv+yG9APA8vMR+5NBw=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "d1b9f17eba909116356436d46b5192d299c6b49a", + "rev": "f891b118c8f4ddb2b6f38d6ce1bfe2f8079552ba", "type": "github" }, "original": { @@ -209,33 +207,57 @@ "type": "github" } }, - "pre-commit-hooks-nix": { + "pyproject-build-systems": { "inputs": { - "flake-compat": [ + "nixpkgs": [ "gepetto", - "system-manager", - "userborn", - "flake-compat" + "gazebros2nix", + "nixpkgs" + ], + "pyproject-nix": [ + "gepetto", + "gazebros2nix", + "pyproject-nix" ], - "gitignore": "gitignore", + "uv2nix": [ + "gepetto", + "gazebros2nix", + "uv2nix" + ] + }, + "locked": { + "lastModified": 1771423342, + "narHash": "sha256-7uXPiWB0YQ4HNaAqRvVndYL34FEp1ZTwVQHgZmyMtC8=", + "owner": "pyproject-nix", + "repo": "build-system-pkgs", + "rev": "04e9c186e01f0830dad3739088070e4c551191a4", + "type": "github" + }, + "original": { + "owner": "pyproject-nix", + "repo": "build-system-pkgs", + "type": "github" + } + }, + "pyproject-nix": { + "inputs": { "nixpkgs": [ "gepetto", - "system-manager", - "userborn", + "gazebros2nix", "nixpkgs" ] }, "locked": { - "lastModified": 1769069492, - "narHash": "sha256-Efs3VUPelRduf3PpfPP2ovEB4CXT7vHf8W+xc49RL/U=", - "owner": "cachix", - "repo": "pre-commit-hooks.nix", - "rev": "a1ef738813b15cf8ec759bdff5761b027e3e1d23", + "lastModified": 1771518446, + "narHash": "sha256-nFJSfD89vWTu92KyuJWDoTQJuoDuddkJV3TlOl1cOic=", + "owner": "pyproject-nix", + "repo": "pyproject.nix", + "rev": "eb204c6b3335698dec6c7fc1da0ebc3c6df05937", "type": "github" }, "original": { - "owner": "cachix", - "repo": "pre-commit-hooks.nix", + "owner": "pyproject-nix", + "repo": "pyproject.nix", "type": "github" } }, @@ -245,6 +267,10 @@ "gepetto", "flake-parts" ], + "gazebros2nix": [ + "gepetto", + "gazebros2nix" + ], "gepetto": "gepetto", "nix-ros-overlay": [ "gepetto", @@ -264,54 +290,6 @@ ] } }, - "src-agimus-controller": { - "flake": false, - "locked": { - "lastModified": 1770376027, - "narHash": "sha256-ZSOgLgsBS73d0gVbna2WzWkC7fhORPu3/KaktOr8CzA=", - "owner": "agimus-project", - "repo": "agimus_controller", - "rev": "00ccabbad6ab4a55bad18c094e8fc608fd8e0aed", - "type": "github" - }, - "original": { - "owner": "agimus-project", - "repo": "agimus_controller", - "type": "github" - } - }, - "src-agimus-msgs": { - "flake": false, - "locked": { - "lastModified": 1770623573, - "narHash": "sha256-gOTxAqOKx7B+tpaDrhEOf+0afWUBQj3RVXeIDZb0+nE=", - "owner": "agimus-project", - "repo": "agimus_msgs", - "rev": "241d82d5bcffac00de83397c29347ab5bce45088", - "type": "github" - }, - "original": { - "owner": "agimus-project", - "repo": "agimus_msgs", - "type": "github" - } - }, - "src-franka-description": { - "flake": false, - "locked": { - "lastModified": 1770385506, - "narHash": "sha256-A+UFglPEHA2deOI06EyG+NF1+ejMRRAOEUj4yldDo/o=", - "owner": "agimus-project", - "repo": "franka_description", - "rev": "535aa51107bf990abd92404d2517b09e4b6417a3", - "type": "github" - }, - "original": { - "owner": "agimus-project", - "repo": "franka_description", - "type": "github" - } - }, "src-odri-control-interface": { "flake": false, "locked": { @@ -351,15 +329,14 @@ "nixpkgs": [ "gepetto", "nixpkgs" - ], - "userborn": "userborn" + ] }, "locked": { - "lastModified": 1770630329, - "narHash": "sha256-Q0/JNws9SxY9wE/mCQ6WTA85MIgrJ+M0HDcJVUlk9Ds=", + "lastModified": 1770135975, + "narHash": "sha256-J3qmZ4rTfmgyjrsQRrQWT7ZIYVtYqtLomMNDUibuw2k=", "owner": "numtide", "repo": "system-manager", - "rev": "442db31401bdbdadc59356232153448e05bce1db", + "rev": "413f296fb1fd210c44e38744e270b3afc4c733d7", "type": "github" }, "original": { @@ -383,25 +360,11 @@ "type": "github" } }, - "systems_2": { - "locked": { - "lastModified": 1681028828, - "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", - "owner": "nix-systems", - "repo": "default", - "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", - "type": "github" - }, - "original": { - "owner": "nix-systems", - "repo": "default", - "type": "github" - } - }, "treefmt-nix": { "inputs": { "nixpkgs": [ "gepetto", + "gazebros2nix", "nixpkgs" ] }, @@ -419,30 +382,30 @@ "type": "github" } }, - "userborn": { + "uv2nix": { "inputs": { - "flake-compat": "flake-compat", - "flake-parts": "flake-parts_2", "nixpkgs": [ "gepetto", - "system-manager", + "gazebros2nix", "nixpkgs" ], - "pre-commit-hooks-nix": "pre-commit-hooks-nix", - "systems": "systems_2" + "pyproject-nix": [ + "gepetto", + "gazebros2nix", + "pyproject-nix" + ] }, "locked": { - "lastModified": 1770377964, - "narHash": "sha256-q2pnlX2IW0kg80GLFnwWd/GigIpkuZnyKPLhrgJql3E=", - "owner": "jfroche", - "repo": "userborn", - "rev": "55c2cd7952c207a62736a5bbd9499ea73da18d24", + "lastModified": 1772187362, + "narHash": "sha256-gCojeIlQ/rfWMe3adif3akyHsT95wiMkLURpxTeqmPc=", + "owner": "pyproject-nix", + "repo": "uv2nix", + "rev": "abe65de114300de41614002fe9dce2152ac2ac23", "type": "github" }, "original": { - "owner": "jfroche", - "ref": "system-manager", - "repo": "userborn", + "owner": "pyproject-nix", + "repo": "uv2nix", "type": "github" } } diff --git a/flake.nix b/flake.nix index 68f3cac6..30e065e2 100644 --- a/flake.nix +++ b/flake.nix @@ -3,6 +3,7 @@ inputs = { gepetto.url = "github:gepetto/nix"; + gazebros2nix.follows = "gepetto/gazebros2nix"; flake-parts.follows = "gepetto/flake-parts"; nixpkgs.follows = "gepetto/nixpkgs"; nix-ros-overlay.follows = "gepetto/nix-ros-overlay"; @@ -12,26 +13,14 @@ outputs = inputs: - inputs.flake-parts.lib.mkFlake { inherit inputs; } { - systems = import inputs.systems; - imports = [ inputs.gepetto.flakeModule ]; - perSystem = - { - lib, - pkgs, - self', - ... - }: - { - apps = lib.filterAttrs (_n: v: v.program.meta.available && !v.program.meta.broken) { - default = { - type = "app"; - program = pkgs.python3.withPackages (_: [ self'.packages.default ]); - }; - }; - packages = { - default = self'.packages.hpp-python; - hpp-python = pkgs.python3Packages.hpp-python.overrideAttrs (super: { + inputs.flake-parts.lib.mkFlake { inherit inputs; } ( + { lib, ... }: + { + systems = import inputs.systems; + imports = [ + inputs.gepetto.flakeModule + { + gazebros2nix.overrides.hpp-python = _final: { src = lib.fileset.toSource { root = ./.; fileset = lib.fileset.unions [ @@ -43,12 +32,9 @@ ./tests ]; }; - patches = [ ]; - propagatedBuildInputs = (super.propagatedBuildInputs or [ ]) ++ [ - pkgs.python3Packages.hpp-tutorial - ]; - }); - }; - }; - }; + }; + } + ]; + } + ); } From 11b101e9b8ba33d9a7d23c1455a0034ebc46d254 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 6 Mar 2026 20:31:46 +0000 Subject: [PATCH 107/109] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/manipulation/device.cc | 54 +++++++++++++++++++------------- src/pyhpp/manipulation/device.hh | 3 +- src/pyhpp/pinocchio/device.cc | 7 +++-- src/pyhpp/pinocchio/device.hh | 4 ++- 4 files changed, 43 insertions(+), 25 deletions(-) diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 2c9b4b0b..b07ce75c 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -125,20 +125,23 @@ boost::python::dict Device::contactSurfaces() { return result; } -void Device::addHandle(const std::string& linkName, const std::string& handleName, - const Transform3s& pose, value_type clearance, const std::vector& mask){ - hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); +void Device::addHandle(const std::string& linkName, + const std::string& handleName, const Transform3s& pose, + value_type clearance, const std::vector& mask) { + hpp::manipulation::DevicePtr_t robot = + HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); if (!robot) { - throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp." - "manipulation)"); + throw std::logic_error( + "Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); } - if (mask.size () != 6) { + if (mask.size() != 6) { throw std::logic_error("mask should be of size 6"); } JointPtr_t joint = robot->getJointByBodyName(linkName); const ::pinocchio::Frame& linkFrame = - robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; assert(linkFrame.type == ::pinocchio::BODY); hpp::pinocchio::JointIndex index(0); @@ -148,7 +151,7 @@ void Device::addHandle(const std::string& linkName, const std::string& handleNam jointName = joint->name(); } HandlePtr_t handle = - Handle::create(handleName, linkFrame.placement * pose, robot, joint); + Handle::create(handleName, linkFrame.placement * pose, robot, joint); handle->clearance(clearance); handle->mask(mask); robot->handles.add(handleName, handle); @@ -161,17 +164,20 @@ void Device::addHandle(const std::string& linkName, const std::string& handleNam robot->createData(); } -void Device::addGripper(const std::string& linkName, const std::string& gripperName, - const Transform3s& pose, value_type clearance) { - hpp::manipulation::DevicePtr_t robot = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); +void Device::addGripper(const std::string& linkName, + const std::string& gripperName, const Transform3s& pose, + value_type clearance) { + hpp::manipulation::DevicePtr_t robot = + HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); if (!robot) { - throw std::logic_error("Device.addHandle expects a manipulation device (imported from pyhpp." - "manipulation)"); + throw std::logic_error( + "Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); } JointPtr_t joint = robot->getJointByBodyName(linkName); const ::pinocchio::Frame& linkFrame = - robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; assert(linkFrame.type == ::pinocchio::BODY); hpp::pinocchio::JointIndex index(0); @@ -183,7 +189,8 @@ void Device::addGripper(const std::string& linkName, const std::string& gripperN assert(robot->model().existFrame(jointName)); FrameIndex previousFrame(robot->model().getFrameId(jointName)); robot->model().addFrame(::pinocchio::Frame(gripperName, index, previousFrame, - linkFrame.placement * pose, ::pinocchio::OP_FRAME)); + linkFrame.placement * pose, + ::pinocchio::OP_FRAME)); // Recreate pinocchio data after modifying model robot->createData(); GripperPtr_t gripper = Gripper::create(gripperName, robot); @@ -274,10 +281,12 @@ void exposeHandle() { "clearance", static_cast(&Handle::clearance), static_cast(&Handle::clearance)) - .add_property("approachingDirection", &getApproachingDirection, &setApproachingDirection) + .add_property("approachingDirection", &getApproachingDirection, + &setApproachingDirection) .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) - .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to" - " in pinocchio Model") + .def("getParentJointId", &getParentJointId, + "Get index of the joint the handle is attached to" + " in pinocchio Model") .def("createPreGrasp", &Handle::createPreGrasp, DocClassMethod(createPreGrasp)) .def("createGraspComplement", &Handle::createGraspComplement, @@ -317,16 +326,19 @@ void exposeDevice() { " linkName: name of the link the handle is attached to,\n" " handleName: name of the handle,\n" " pose: pose of the handle in the link frame (SE3),\n" - " clearance: clearance of the handle, the sum of handle and gripper clearances\n" + " clearance: clearance of the handle, the sum of handle and " + "gripper clearances\n" " defines the distance between pregrasp and grasp,\n" - " mask: list of 6 Boolean use to define symmetries in the grasp constraint.") + " mask: list of 6 Boolean use to define symmetries in the grasp " + "constraint.") .def("addGripper", &Device::addGripper, "Add a gripper to the kinematic chain\n\n" " input\n" " linkName: name of the link the handle is attached to,\n" " gripperName: name of the gripper,\n" " pose: pose of the gripper in the link frame (SE3),\n" - " clearance: clearance of the gripper, the sum of handle and gripper clearances\n" + " clearance: clearance of the gripper, the sum of handle and " + "gripper clearances\n" " defines the distance between pregrasp and grasp."); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index 76edb15f..f1417e2f 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -90,7 +90,8 @@ struct Device : public pyhpp::pinocchio::Device { boost::python::list contactSurfaceNames(); boost::python::dict contactSurfaces(); void addHandle(const std::string& linkName, const std::string& handleName, - const Transform3s& pose, value_type clearance, const std::vector& mask); + const Transform3s& pose, value_type clearance, + const std::vector& mask); void addGripper(const std::string& linkName, const std::string& gripperName, const Transform3s& pose, value_type clearance); }; // struct Device diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index e32c8724..24797888 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -213,7 +213,9 @@ static boost::python::list getJointsPosition( } } -static std::string getGripperName(const GripperPtr_t& gripper) { return gripper->name(); } +static std::string getGripperName(const GripperPtr_t& gripper) { + return gripper->name(); +} static JointIndex getParentJointId(const GripperPtr_t& gripper) { assert(gripper->joint()); @@ -232,7 +234,8 @@ void exposeGripper() { static_cast( &Gripper::clearance)) .def("name", &getGripperName) - .def("getParentJointId", &getParentJointId, "Get index of the joint the handle is attached to" + .def("getParentJointId", &getParentJointId, + "Get index of the joint the handle is attached to" " in pinocchio Model"); class_ >("GripperMap") .def( diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh index cefb3775..f695767c 100644 --- a/src/pyhpp/pinocchio/device.hh +++ b/src/pyhpp/pinocchio/device.hh @@ -56,7 +56,9 @@ struct Device { } void updateGeometryPlacements() { obj->updateGeometryPlacements(); } void removeJoints(const std::vector& jointNames, - Configuration_t q) { obj->removeJoints(jointNames, q); } + Configuration_t q) { + obj->removeJoints(jointNames, q); + } hpp::manipulation::DevicePtr_t asManipulationDevice() const { auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj); if (!manipDevice) { From 0f3b6609b27ef11a8d634425662ad28f156d3b32 Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 6 Mar 2026 21:34:30 +0100 Subject: [PATCH 108/109] oops --- flake.nix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flake.nix b/flake.nix index 30e065e2..d9edb38b 100644 --- a/flake.nix +++ b/flake.nix @@ -20,7 +20,7 @@ imports = [ inputs.gepetto.flakeModule { - gazebros2nix.overrides.hpp-python = _final: { + gazebros2nix.pyOverrides.hpp-python = _final: _python-final: { src = lib.fileset.toSource { root = ./.; fileset = lib.fileset.unions [ From 59282a358af44979636b3e213a3bd1371d10dcde Mon Sep 17 00:00:00 2001 From: Guilhem Saurel Date: Fri, 6 Mar 2026 22:02:05 +0100 Subject: [PATCH 109/109] tooling --- .github/dependabot.yml | 2 +- .pre-commit-config.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 203f3c88..4ad7bd50 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -3,4 +3,4 @@ updates: - package-ecosystem: "github-actions" directory: "/" schedule: - interval: "weekly" + interval: "monthly" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 97937e8e..7c9180b7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,8 +1,8 @@ ci: - autoupdate_branch: devel + autoupdate_schedule: quarterly repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.15.4 + rev: v0.15.5 hooks: - id: ruff args: