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()