diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index c582e23..b8d64ab 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -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 diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 494175b..92b4b3c 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -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(); } @@ -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); @@ -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()); } @@ -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."); } @@ -466,12 +461,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 +481,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); } 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):