Skip to content

[Problem] Fix constraint handling and projection defaults #182

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
psardin001 merged 3 commits into from
Sep 4, 2026
Merged

[Problem] Fix constraint handling and projection defaults #182

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e2c92d9
[Problem] Fix constraints handling
florent-lamiraux Aug 28, 2026
bac8ad7
[pre-commit.ci] auto fixes from pre-commit.com hooks
pre-commit-ci[bot] Aug 28, 2026
1661127
[Problem] Initialize projection defaults for wrapped problems
psardin001 Sep 4, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions include/pyhpp/core/problem.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,10 +147,9 @@ 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_;
value_type errorThreshold_{1e-4};
size_type maxIterProjection_{20};
};

} // namespace core
Expand Down
27 changes: 11 additions & 16 deletions src/pyhpp/core/problem.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,11 +117,9 @@ 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)) {
obj->constraints(
hpp::core::ConstraintSet::create(robot, "Default constraint set"));
}

const DevicePtr_t& Problem::robot() const { return obj->robot(); }
Expand Down Expand Up @@ -393,9 +391,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);
Expand Down Expand Up @@ -427,10 +425,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());
}
Expand All @@ -447,7 +442,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.");
}
Expand All @@ -466,12 +461,12 @@ void Problem::addNumericalConstraintsToConfigProjector1(
extract_vector<hpp::constraints::ImplicitPtr_t>(constraints);
auto prioritiesVec = extract_vector<std::size_t>(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]);
}
Expand All @@ -486,12 +481,12 @@ void Problem::addNumericalConstraintsToConfigProjector2(
auto constraintsVec =
extract_vector<hpp::constraints::ImplicitPtr_t>(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);
}
Expand Down
31 changes: 30 additions & 1 deletion tests/unit/test_problem.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -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):
Expand Down