From 0167330ac8eb63241b45b25e3d1eb110ae044a7d Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 13 May 2026 09:25:15 +0200 Subject: [PATCH 1/3] [core] Bind trapezoidal time parameterization --- src/pyhpp/core/path-optimizer.cc | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index bf37f45..d037c9f 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -119,6 +120,23 @@ void exposePathOptimizer() { .def("__init__", make_constructor(&pathOptimization::RSTimeParameterization::create)); + class_, + bases, boost::noncopyable>( + "TrapezoidalTimeParameterization", no_init) + .def("__init__", + make_constructor( + &pathOptimization::TrapezoidalTimeParameterization::create)) + .def_readwrite( + "maxVelocity", + &pathOptimization::TrapezoidalTimeParameterization::maxVelocity) + .def_readwrite( + "maxAcceleration", + &pathOptimization::TrapezoidalTimeParameterization::maxAcceleration) + .def_readwrite( + "minimumDuration", + &pathOptimization::TrapezoidalTimeParameterization::minimumDuration); + exposeSplineGradientBased<1>("SplineGradientBased_bezier1"); exposeSplineGradientBased<3>("SplineGradientBased_bezier3"); exposeSplineGradientBased<5>("SplineGradientBased_bezier5"); From 9f0dee201e0457a27d60d51b4ae6c91904b519ca Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Wed, 13 May 2026 11:24:01 +0200 Subject: [PATCH 2/3] [core] Bind trapezoidal parameter properties --- src/pyhpp/core/path-optimizer.cc | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index d037c9f..ad1b65c 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -120,22 +120,26 @@ void exposePathOptimizer() { .def("__init__", make_constructor(&pathOptimization::RSTimeParameterization::create)); - class_, - bases, boost::noncopyable>( - "TrapezoidalTimeParameterization", no_init) + typedef pathOptimization::TrapezoidalTimeParameterization TTP_t; + class_, bases, + boost::noncopyable>("TrapezoidalTimeParameterization", no_init) .def("__init__", make_constructor( &pathOptimization::TrapezoidalTimeParameterization::create)) - .def_readwrite( + .add_property( "maxVelocity", - &pathOptimization::TrapezoidalTimeParameterization::maxVelocity) - .def_readwrite( + static_cast(&TTP_t::maxVelocity), + static_cast(&TTP_t::maxVelocity)) + .add_property( "maxAcceleration", - &pathOptimization::TrapezoidalTimeParameterization::maxAcceleration) - .def_readwrite( + static_cast(&TTP_t::maxAcceleration), + static_cast( + &TTP_t::maxAcceleration)) + .add_property( "minimumDuration", - &pathOptimization::TrapezoidalTimeParameterization::minimumDuration); + static_cast(&TTP_t::minimumDuration), + static_cast( + &TTP_t::minimumDuration)); exposeSplineGradientBased<1>("SplineGradientBased_bezier1"); exposeSplineGradientBased<3>("SplineGradientBased_bezier3"); From fd03fd25702f8be0306739c5b64b99530cd903e0 Mon Sep 17 00:00:00 2001 From: Paul Sardin Date: Thu, 9 Jul 2026 10:21:27 +0200 Subject: [PATCH 3/3] [manipulation] Fix problem steering setter Set core steering methods as the inner steering method of the manipulation graph, preserving the manipulation steering method required by constraint graphs. Update integration scripts to call the setter methods explicitly. --- src/pyhpp/manipulation/problem.cc | 24 +++++++++++++++++++++ src/pyhpp/manipulation/problem.hh | 2 ++ tests/integration/construction-set-m-rrt.py | 24 ++++++++++----------- tests/integration/pr2-in-iai-kitchen.py | 4 ++-- tests/integration/romeo-placard.py | 8 +++---- tests/integration/ur3-spheres-spf.py | 8 +++---- tests/integration/ur3-spheres.py | 8 +++---- 7 files changed, 52 insertions(+), 26 deletions(-) diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index 498daf7..237066b 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -64,6 +65,28 @@ void Problem::checkProblem() const { asManipulationProblem()->checkProblem(); } void Problem::steeringMethod( const pyhpp::core::PyWSteeringMethodPtr_t& steeringMethod) { + auto manipulationSteeringMethod = HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::SteeringMethod, steeringMethod->obj); + auto endEffectorTrajectorySteeringMethod = HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::steeringMethod::EndEffectorTrajectory, + steeringMethod->obj); + if (manipulationSteeringMethod || endEffectorTrajectorySteeringMethod) { + obj->steeringMethod(steeringMethod->obj); + return; + } + + manipulationSteeringMethod = HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::SteeringMethod, obj->steeringMethod()); + if (!manipulationSteeringMethod) { + manipulationSteeringMethod = + hpp::manipulation::steeringMethod::Graph::create(obj); + obj->steeringMethod(manipulationSteeringMethod); + } + manipulationSteeringMethod->innerSteeringMethod(steeringMethod->obj); +} + +void Problem::fullSteeringMethod( + const pyhpp::core::PyWSteeringMethodPtr_t& steeringMethod) { obj->steeringMethod(steeringMethod->obj); } @@ -137,6 +160,7 @@ void exposeProblem() { static_cast( &Problem::graphSteeringMethod)) + .def("fullSteeringMethod", &Problem::fullSteeringMethod) // .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(Problem, pathValidation, // PathValidationPtr_t) .PYHPP_DEFINE_METHOD(Problem, // manipulationSteeringMethod) .PYHPP_DEFINE_METHOD(Problem, diff --git a/src/pyhpp/manipulation/problem.hh b/src/pyhpp/manipulation/problem.hh index e04c165..8dfb590 100644 --- a/src/pyhpp/manipulation/problem.hh +++ b/src/pyhpp/manipulation/problem.hh @@ -61,6 +61,8 @@ struct Problem : public pyhpp::core::Problem { const pyhpp::core::PyWSteeringMethodPtr_t& steeringMethod); pyhpp::core::PyWSteeringMethodPtr_t steeringMethod() const; void graphSteeringMethod(const PyWGraphSteeringMethodPtr_t& steeringMethod); + void fullSteeringMethod( + const pyhpp::core::PyWSteeringMethodPtr_t& steeringMethod); // PathValidationPtr_t pathValidation() const; // void pathValidation (const PathValidationPtr_t &pathValidation); // SteeringMethodPtr_t manipulationSteeringMethod() const; diff --git a/tests/integration/construction-set-m-rrt.py b/tests/integration/construction-set-m-rrt.py index c2f55b6..c6840d0 100644 --- a/tests/integration/construction-set-m-rrt.py +++ b/tests/integration/construction-set-m-rrt.py @@ -406,10 +406,10 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: 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 + problem.steeringMethod(Straight(problem)) + problem.pathValidation(Progressive(robot, 0.02)) + problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.05) ) factory = ConstraintGraphFactory(cg, constraints) @@ -429,8 +429,8 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: cg.initialize() - problem.pathProjector = ProgressiveProjector( - problem.distance(), problem.steeringMethod, 0.05 + problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.05) ) nodes.append(nodes[-1]) @@ -445,10 +445,10 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: 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 + problem.steeringMethod(Straight(problem)) + problem.pathValidation(Progressive(robot, 0.02)) + problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.05) ) factory = ConstraintGraphFactory(cg, constraints) @@ -468,8 +468,8 @@ def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: cg.initialize() - problem.pathProjector = ProgressiveProjector( - problem.distance(), problem.steeringMethod, 0.05 + problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.05) ) assert nCylinder == 2 and nSphere == 2 diff --git a/tests/integration/pr2-in-iai-kitchen.py b/tests/integration/pr2-in-iai-kitchen.py index 9897b49..5eaff63 100644 --- a/tests/integration/pr2-in-iai-kitchen.py +++ b/tests/integration/pr2-in-iai-kitchen.py @@ -45,8 +45,8 @@ q_goal[rank] = -0.5 problem = Problem(robot) -problem.pathValidation = Dichotomy(robot, 0.0) -problem.steeringMethod = Straight(problem) +problem.pathValidation(Dichotomy(robot, 0.0)) +problem.steeringMethod(Straight(problem)) problem.initConfig(q_init) problem.addGoalConfig(q_goal) diff --git a/tests/integration/romeo-placard.py b/tests/integration/romeo-placard.py index cc481f7..1f9513d 100644 --- a/tests/integration/romeo-placard.py +++ b/tests/integration/romeo-placard.py @@ -253,10 +253,10 @@ 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.steeringMethod(Straight(problem)) +problem.pathValidation(Dichotomy(robot, 0)) +problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.05) ) problem.initConfig(q_init_proj) diff --git a/tests/integration/ur3-spheres-spf.py b/tests/integration/ur3-spheres-spf.py index 1df2220..9efb178 100644 --- a/tests/integration/ur3-spheres-spf.py +++ b/tests/integration/ur3-spheres-spf.py @@ -279,12 +279,12 @@ e, [constraints["place_sphere{}/complement".format(i)]] ) -problem.steeringMethod = Straight(problem) -problem.pathValidation = Dichotomy(robot, 0) +problem.steeringMethod(Straight(problem)) +problem.pathValidation(Dichotomy(robot, 0)) # need to set path projector due to implicit constraints added above -problem.pathProjector = ProgressiveProjector( - problem.distance(), problem.steeringMethod, 0.01 +problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.01) ) cg.initialize() diff --git a/tests/integration/ur3-spheres.py b/tests/integration/ur3-spheres.py index 9efa9ec..0b7c3bd 100644 --- a/tests/integration/ur3-spheres.py +++ b/tests/integration/ur3-spheres.py @@ -258,10 +258,10 @@ e, [constraints["place_sphere{}/complement".format(i)]] ) -problem.steeringMethod = Straight(problem) -problem.pathValidation = Dichotomy(robot, 0) -problem.pathProjector = ProgressiveProjector( - problem.distance(), problem.steeringMethod, 0.01 +problem.steeringMethod(Straight(problem)) +problem.pathValidation(Dichotomy(robot, 0)) +problem.pathProjector( + ProgressiveProjector(problem.distance(), problem.steeringMethod(), 0.01) ) cg.initialize()