From e2c92d9d6b9668171d1b6dafd936fadfce86f359 Mon Sep 17 00:00:00 2001 From: Florent Lamiraux Date: Fri, 28 Aug 2026 14:21:02 +0200 Subject: [PATCH 1/3] [Problem] Fix constraints handling The wrapper class pyhpp::core::Problem contained a member constraints_ that used to store constraints provided by users, but those constraints where never fowarded to class hpp::core::Problem. This fixes the benchmark of Pyrene sliding on the ground. --- include/pyhpp/core/problem.hh | 1 - src/pyhpp/core/problem.cc | 22 +++++++++------------- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index c582e23..cdda919 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -147,7 +147,6 @@ struct Problem { boost::python::tuple directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate); - hpp::core::ConstraintSetPtr_t constraints_; PyWPathValidationPtr_t pathValidation_; value_type errorThreshold_; size_type maxIterProjection_; diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 494175b..ee39bc3 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -120,8 +120,7 @@ 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->constraints(hpp::core::ConstraintSet::create(robot, "Default constraint set")); } const DevicePtr_t& Problem::robot() const { return obj->robot(); } @@ -393,9 +392,9 @@ boost::python::tuple Problem::applyConstraints(ConfigurationIn_t config) { double residualError = 0.0; try { Configuration_t output = config; - success = constraints_->apply(output); + success = obj->constraints()->apply(output); if (hpp::core::ConfigProjectorPtr_t configProjector = - constraints_->configProjector()) { + obj->constraints()->configProjector()) { residualError = configProjector->residualError(); } return boost::python::make_tuple(success, output, residualError); @@ -427,10 +426,7 @@ 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_); + obj->constraints(constraints); } catch (const std::exception& exc) { throw std::logic_error(exc.what()); } @@ -447,7 +443,7 @@ hpp::core::ConstraintSetPtr_t Problem::getConstraints() { void Problem::setRightHandSideFromConfig(ConfigurationIn_t configIn) { try { hpp::core::ConfigProjectorPtr_t configProjector( - constraints_->configProjector()); + obj->constraints()->configProjector()); if (!configProjector) { throw std::runtime_error("No constraint has been set."); } @@ -466,12 +462,12 @@ void Problem::addNumericalConstraintsToConfigProjector1( extract_vector(constraints); auto prioritiesVec = extract_vector(priorities); hpp::core::ConfigProjectorPtr_t configProjector = - constraints_->configProjector(); + obj->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); + obj->constraints()->addConstraint(configProjector); } configProjector->add(constraintsVec[i], prioritiesVec[i]); } @@ -486,12 +482,12 @@ void Problem::addNumericalConstraintsToConfigProjector2( auto constraintsVec = extract_vector(constraints); hpp::core::ConfigProjectorPtr_t configProjector = - constraints_->configProjector(); + obj->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); + obj->constraints()->addConstraint(configProjector); } configProjector->add(constraintsVec[i], 0); } From bac8ad7ee6cd8333e10d962532fbd387d7a29369 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Aug 2026 12:24:53 +0000 Subject: [PATCH 2/3] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/pyhpp/core/problem.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index ee39bc3..6451a1d 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -120,7 +120,8 @@ Problem::Problem(const DevicePtr_t& robot) : obj(hpp::core::Problem::create(robot)), errorThreshold_(1e-4), maxIterProjection_(20) { - obj->constraints(hpp::core::ConstraintSet::create(robot, "Default constraint set")); + obj->constraints( + hpp::core::ConstraintSet::create(robot, "Default constraint set")); } const DevicePtr_t& Problem::robot() const { return obj->robot(); } @@ -394,7 +395,7 @@ boost::python::tuple Problem::applyConstraints(ConfigurationIn_t config) { Configuration_t output = config; success = obj->constraints()->apply(output); if (hpp::core::ConfigProjectorPtr_t configProjector = - obj->constraints()->configProjector()) { + obj->constraints()->configProjector()) { residualError = configProjector->residualError(); } return boost::python::make_tuple(success, output, residualError); @@ -462,7 +463,7 @@ void Problem::addNumericalConstraintsToConfigProjector1( extract_vector(constraints); auto prioritiesVec = extract_vector(priorities); hpp::core::ConfigProjectorPtr_t configProjector = - obj->constraints()->configProjector(); + obj->constraints()->configProjector(); for (unsigned int i = 0; i < constraintsVec.size(); ++i) { if (!configProjector) { configProjector = hpp::core::ConfigProjector::create( @@ -482,7 +483,7 @@ void Problem::addNumericalConstraintsToConfigProjector2( auto constraintsVec = extract_vector(constraints); hpp::core::ConfigProjectorPtr_t configProjector = - obj->constraints()->configProjector(); + obj->constraints()->configProjector(); for (unsigned int i = 0; i < constraintsVec.size(); ++i) { if (!configProjector) { configProjector = hpp::core::ConfigProjector::create( From 1661127d820e957ab796527ad4497fbeb8ad6bbd Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Fri, 4 Sep 2026 08:26:38 +0200 Subject: [PATCH 3/3] [Problem] Initialize projection defaults for wrapped problems Move projection defaults to in-class initializers so wrappers built from an existing problem, including manipulation problems, receive the same values. Add regression coverage for the active constraint set and manipulation defaults. --- include/pyhpp/core/problem.hh | 4 ++-- src/pyhpp/core/problem.cc | 4 +--- tests/unit/test_problem.py | 31 ++++++++++++++++++++++++++++++- 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index cdda919..b8d64ab 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -148,8 +148,8 @@ struct Problem { boost::python::tuple directPath(ConfigurationIn_t start, ConfigurationIn_t end, bool validate); PyWPathValidationPtr_t pathValidation_; - value_type errorThreshold_; - size_type maxIterProjection_; + value_type errorThreshold_{1e-4}; + size_type maxIterProjection_{20}; }; } // namespace core diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 6451a1d..92b4b3c 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -117,9 +117,7 @@ namespace core { using namespace boost::python; Problem::Problem(const DevicePtr_t& robot) - : obj(hpp::core::Problem::create(robot)), - errorThreshold_(1e-4), - maxIterProjection_(20) { + : obj(hpp::core::Problem::create(robot)) { obj->constraints( hpp::core::ConstraintSet::create(robot, "Default constraint set")); } diff --git a/tests/unit/test_problem.py b/tests/unit/test_problem.py index 8c71377..00e0897 100644 --- a/tests/unit/test_problem.py +++ b/tests/unit/test_problem.py @@ -7,7 +7,8 @@ import unittest import numpy as np from pinocchio import SE3 -from unit.conftest import create_ur5_problem +from pyhpp.manipulation import Problem as ManipulationProblem +from unit.conftest import create_ur3_robot, create_ur5_problem class TestProblemAccessors(unittest.TestCase): @@ -109,6 +110,34 @@ def test_apply_constraints_returns_tuple(self): self.assertIsInstance(success, bool) self.assertEqual(len(output), len(q)) + def test_add_numerical_constraints_updates_active_constraint_set(self): + problem, robot = create_ur5_problem() + active_constraints = problem.getConstraints() + constraint = problem.createTransformationConstraint( + "test_constraint", + "", + "ur5/ee_fixed_joint", + SE3.Identity(), + [True, True, True, True, True, True], + ) + + problem.addNumericalConstraintsToConfigProjector("test-projector", [constraint]) + + projector = active_constraints.configProjector() + self.assertIsNotNone(projector) + self.assertEqual(len(projector.numericalConstraints()), 1) + self.assertIsNotNone(problem.getConstraints().configProjector()) + + +class TestManipulationProblemProjectionDefaults(unittest.TestCase): + def test_projection_defaults_match_core_problem(self): + robot = create_ur3_robot() + + problem = ManipulationProblem(robot) + + self.assertAlmostEqual(problem.errorThreshold, 1e-4) + self.assertEqual(problem.maxIterProjection, 20) + class TestProblemTransformationConstraints(unittest.TestCase): def test_create_transformation_constraint(self):