From 31b1cf9133d0d4d8d3bd21bcec44ca2849b32e68 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 13 Apr 2021 13:12:51 +0200 Subject: [PATCH 01/10] introduce abstraction storeState analogous to storeSolution --- core/include/moveit/task_constructor/stage_p.h | 5 +++++ core/src/container.cpp | 2 +- core/src/stage.cpp | 16 ++++++++-------- 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 7e51458c2..85b78abca 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -146,7 +146,12 @@ class StagePrivate void spawn(InterfaceState&& state, const SolutionBasePtr& solution); void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution); + // store stage-owned data structures bool storeSolution(const SolutionBasePtr& solution, const InterfaceState* from, const InterfaceState* to); + inline InterfaceState* storeState(InterfaceState&& state) { + return &*states_.insert(states_.end(), std::move(state)); + } + void newSolution(const SolutionBasePtr& solution); bool storeFailures() const { return introspection_ != nullptr; } void runCompute() { diff --git a/core/src/container.cpp b/core/src/container.cpp index c144f114d..d7f251298 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -212,7 +212,7 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I if (it != internalToExternalMap().end()) return const_cast(it->second); - InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal)); + InterfaceState* external = storeState(InterfaceState(*internal)); internalToExternalMap().insert(std::make_pair(internal, external)); created = true; return external; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1d0f1d36e..8c7c6ae93 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -160,14 +160,14 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, me()->forwardProperties(from, to); - auto to_it = states_.insert(states_.end(), std::move(to)); + InterfaceState* to_stored{ storeState(std::move(to)) }; // register stored interfaces with solution solution->setStartState(from); - solution->setEndState(*to_it); + solution->setEndState(*to_stored); if (!solution->isFailure()) - nextStarts()->add(*to_it); + nextStarts()->add(*to_stored); newSolution(solution); } @@ -182,13 +182,13 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, me()->forwardProperties(to, from); - auto from_it = states_.insert(states_.end(), std::move(from)); + InterfaceState* from_stored{ storeState(std::move(from)) }; - solution->setStartState(*from_it); + solution->setStartState(*from_stored); solution->setEndState(to); if (!solution->isFailure()) - prevEnds()->add(*from_it); + prevEnds()->add(*from_stored); newSolution(solution); } @@ -201,8 +201,8 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution if (!storeSolution(solution, nullptr, nullptr)) return; // solution dropped - auto from = states_.insert(states_.end(), InterfaceState(state)); // copy - auto to = states_.insert(states_.end(), std::move(state)); + InterfaceState* from{ storeState(InterfaceState{ state }) }; + InterfaceState* to{ storeState(std::move(state)) }; solution->setStartState(*from); solution->setEndState(*to); From 70c97a4916edd8e2c9a801487ff4b9bc3507d732 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 7 Apr 2021 11:09:29 +0200 Subject: [PATCH 02/10] support modifying wrappers in user stages Possible features that require this interface might be - trajectory reparameterization, - trajectory optimization as post-processing (like MoveIt's CHOMP adapter), - multi-trajectory blending (similar to Pilz' sequence planner) Also remove invalid interfaces. Parallel containers must always forward solutions of their child stages, so simple spawning is not allowed without a child solution. --- .../moveit/task_constructor/container.h | 11 ++- .../moveit/task_constructor/container_p.h | 4 +- core/src/container.cpp | 78 +++++++++++-------- core/src/stages/compute_ik.cpp | 34 ++++---- 4 files changed, 71 insertions(+), 56 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a71..49481ac02 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -127,12 +127,11 @@ class ParallelContainerBase : public ContainerBase /// lift child solution to external interface, adapting the costs and comment void liftSolution(const SolutionBase& solution, double cost, std::string comment); - /// spawn a new solution with given state as start and end - void spawn(InterfaceState&& state, SubTrajectory&& trajectory); - /// propagate a solution forwards - void sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& trajectory); - /// propagate a solution backwards - void sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& trajectory); + /// lift a modified solution based on the solution of a child stage + void liftModifiedSolution(SolutionBasePtr&& new_solution, const SolutionBase& child_solution); + /// lift a modified solution, changing the (single!) new associated start or end InterfaceState + void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, + const SolutionBase& child_solution); }; /** Plan for different alternatives in parallel. diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index d62b9fbc6..322a5de87 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -150,9 +150,9 @@ class ContainerBasePrivate : public StagePrivate /// copy external_state to a child's interface and remember the link in internal_external map template void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); - /// lift solution from internal to external level + /// lift solution from internal to external level, possibly replacing the generated InterfaceState with new_external void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, - const InterfaceState* internal_to); + const InterfaceState* internal_to, const InterfaceState* new_external = nullptr); /// protected writable overloads inline auto& internalToExternalMap() { return internal_external_.left; } diff --git a/core/src/container.cpp b/core/src/container.cpp index d7f251298..2e68aa848 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -203,37 +203,49 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa } void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, - const InterfaceState* internal_to) { - computeCost(*internal_from, *internal_to, *solution); - - // map internal to external states - auto find_or_create_external = [this](const InterfaceState* internal, bool& created) -> InterfaceState* { - auto it = internalToExternalMap().find(internal); - if (it != internalToExternalMap().end()) - return const_cast(it->second); - - InterfaceState* external = storeState(InterfaceState(*internal)); - internalToExternalMap().insert(std::make_pair(internal, external)); - created = true; + const InterfaceState* internal_to, const InterfaceState* new_external) { + const bool create_from{ requiredInterface().testFlag(WRITES_PREV_END) }; + const bool create_to{ requiredInterface().testFlag(WRITES_NEXT_START) }; + + // external states, nullptr if they don't exist yet + const InterfaceState* external_from{ create_from ? new_external : internalToExternalMap().at(internal_from) }; + const InterfaceState* external_to{ create_to ? new_external : internalToExternalMap().at(internal_to) }; + + // computeCost + computeCost(external_from ? *external_from : InterfaceState(*internal_from), + external_to ? *external_to : InterfaceState(*internal_to), *solution); + + // storeSolution + if (!storeSolution(solution, external_from, external_to)) { + return; + } + + auto create_state = [this, new_external](const InterfaceState& internal) { + InterfaceState* external{ storeState(new_external ? InterfaceState{ *new_external } : + InterfaceState{ internal }) }; + internalToExternalMap().insert(std::make_pair(&internal, external)); return external; }; - bool created_from = false; - bool created_to = false; - InterfaceState* external_from = find_or_create_external(internal_from, created_from); - InterfaceState* external_to = find_or_create_external(internal_to, created_to); - if (!storeSolution(solution, external_from, external_to)) - return; + if (create_from) + external_from = create_state(*internal_from); + if (create_to) + external_to = create_state(*internal_to); + + assert(external_from); + assert(external_to); - // connect solution to start/end state + // connect solution to states solution->setStartState(*external_from); solution->setEndState(*external_to); - // spawn created states in external interfaces - if (created_from) - prevEnds()->add(*external_from); - if (created_to) - nextStarts()->add(*external_to); + // spawn new external states + if (!solution->isFailure()) { + if (create_from) + prevEnds()->add(*const_cast(external_from)); + if (create_to) + nextStarts()->add(*const_cast(external_to)); + } newSolution(solution); } @@ -735,16 +747,20 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co solution.start(), solution.end()); } -void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) { - pimpl()->StagePrivate::spawn(std::move(state), std::make_shared(std::move(t))); -} +void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase &child_solution) { + // child_solution is correctly prepared by a child of this container + assert(child_solution.creator()); + assert(child_solution.creator()->parent() == this); -void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { - pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared(std::move(t))); + pimpl()->liftSolution(std::move(modified_solution), + child_solution.start(), child_solution.end()); } -void ParallelContainerBase::sendBackward(InterfaceState&& from, const InterfaceState& to, SubTrajectory&& t) { - pimpl()->StagePrivate::sendBackward(std::move(from), to, std::make_shared(std::move(t))); +void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr &&new_solution, InterfaceState &&new_propagated_state, const SolutionBase &child_solution) { + assert(child_solution.creator()); + assert(child_solution.creator()->parent() == this); + + pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state); } WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name) diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 4954bd5ed..e75694425 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -335,15 +335,15 @@ void ComputeIK::compute() { ->getParentJointModel() ->getDescendantLinkModels(); if (colliding) { - SubTrajectory solution; + auto solution = std::make_shared(); generateCollisionMarkers(sandbox_state, appender, links_to_visualize); - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); - solution.markAsFailure(); + std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers())); + solution->markAsFailure(); // TODO: visualize collisions - solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); + solution->setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", ")); auto colliding_scene{ scene->diff() }; colliding_scene->setCurrentState(sandbox_state); - spawn(InterfaceState(colliding_scene), std::move(solution)); + liftModifiedSolution(solution, InterfaceState(colliding_scene), s); return; } else generateVisualMarkers(sandbox_state, appender, links_to_visualize); @@ -396,18 +396,18 @@ void ComputeIK::compute() { for (size_t i = previous; i != ik_solutions.size(); ++i) { // create a new scene for each solution as they will have different robot states planning_scene::PlanningScenePtr solution_scene = scene->diff(); - SubTrajectory solution; - solution.setComment(s.comment()); + auto solution = std::make_shared(); + solution->setComment(s.comment()); // frames at target pose and ik frame - rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution->markers(), ik_pose_msg, 0.1, "ik frame"); if (succeeded && i + 1 == ik_solutions.size()) // compute cost as distance to compare_pose - solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); + solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); else // found an IK solution, but this was not valid - solution.markAsFailure(); + solution->markAsFailure(); // set scene's robot state robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst(); @@ -416,7 +416,7 @@ void ComputeIK::compute() { InterfaceState state(solution_scene); forwardProperties(*s.start(), state); - spawn(std::move(state), std::move(solution)); + liftModifiedSolution(solution, std::move(state), s); } // TODO: magic constant should be a property instead ("current_seed_only", or equivalent) @@ -428,15 +428,15 @@ void ComputeIK::compute() { if (ik_solutions.empty()) { // failed to find any solution planning_scene::PlanningScenePtr scene = s.start()->scene()->diff(); - SubTrajectory solution; + auto solution = std::make_shared(); - solution.markAsFailure(); - solution.setComment(s.comment() + " no IK found"); + solution->markAsFailure(); + solution->setComment(s.comment() + " no IK found"); // ik target link placement - std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers())); + std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution->markers())); - spawn(InterfaceState(scene), std::move(solution)); + liftModifiedSolution(solution, InterfaceState(scene), s); } } } // namespace stages From 08353e10efe7ade2b252b951afc8ff44b67c0a9f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 13 Apr 2021 13:13:23 +0200 Subject: [PATCH 03/10] remember internal/external mapping before pushing the new states Doesn't make a difference in single-threaded planning, but still better. Also move to storeState helper. --- core/src/container.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 2e68aa848..69a9f7873 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -196,10 +196,11 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa } // create a clone of external state within target interface (child's starts() or ends()) - auto internal = states_.insert(states_.end(), InterfaceState(*external)); - target->add(*internal); + InterfaceState* internal = storeState(InterfaceState{ *external }); + // and remember the mapping between them - internalToExternalMap().insert(std::make_pair(&*internal, &*external)); + internalToExternalMap().insert(std::make_pair(internal, &*external)); + target->add(*internal); } void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, From 636bfcabeb66bc9ee6a774289f60280a0d91879d Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 13 Apr 2021 13:38:21 +0200 Subject: [PATCH 04/10] revert to passing internal states to CostTerm while lifting and add a justification for doing so. --- core/src/container.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 69a9f7873..f8df20f0e 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -213,8 +213,9 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I const InterfaceState* external_to{ create_to ? new_external : internalToExternalMap().at(internal_to) }; // computeCost - computeCost(external_from ? *external_from : InterfaceState(*internal_from), - external_to ? *external_to : InterfaceState(*internal_to), *solution); + // we can pass intern_{from/to} here because in this case the lifted states that might be created later + // are equivalent up to the connected Solutions (which are not relevant for CostTerms) + computeCost(external_from ? *external_from : *internal_from, external_to ? *external_to : *internal_to, *solution); // storeSolution if (!storeSolution(solution, external_from, external_to)) { From 577573019c1d1811040bbda42a442a01212291e9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 13 Apr 2021 14:15:02 +0200 Subject: [PATCH 05/10] add modifier with both modified start/end states Required to lift a SolutionSequence, and modify its states (e.g., to add properties). --- .../moveit/task_constructor/container.h | 4 +++ .../moveit/task_constructor/container_p.h | 6 ++-- core/src/container.cpp | 31 ++++++++++++------- 3 files changed, 28 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 49481ac02..a4911ae51 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -132,6 +132,10 @@ class ParallelContainerBase : public ContainerBase /// lift a modified solution, changing the (single!) new associated start or end InterfaceState void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution); + /// lift a modified solution, providing new start and end states + /// The new states will only be used if this's should actually create the corresponding states + void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_start_state, + InterfaceState&& new_end_state, const SolutionBase& child_solution); }; /** Plan for different alternatives in parallel. diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 322a5de87..a55c45dbf 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -150,9 +150,11 @@ class ContainerBasePrivate : public StagePrivate /// copy external_state to a child's interface and remember the link in internal_external map template void copyState(Interface::iterator external, const InterfacePtr& target, bool updated); - /// lift solution from internal to external level, possibly replacing the generated InterfaceState with new_external + /// lift solution from internal to external level, possibly replacing the generated InterfaceStates with new_* + /// If specified, *new_from/*new_to will be moved from. void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, - const InterfaceState* internal_to, const InterfaceState* new_external = nullptr); + const InterfaceState* internal_to, InterfaceState* new_from = nullptr, + InterfaceState* new_to = nullptr); /// protected writable overloads inline auto& internalToExternalMap() { return internal_external_.left; } diff --git a/core/src/container.cpp b/core/src/container.cpp index f8df20f0e..427b2e993 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -204,13 +204,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa } void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, - const InterfaceState* internal_to, const InterfaceState* new_external) { + const InterfaceState* internal_to, InterfaceState* new_from, + InterfaceState* new_to) { const bool create_from{ requiredInterface().testFlag(WRITES_PREV_END) }; const bool create_to{ requiredInterface().testFlag(WRITES_NEXT_START) }; // external states, nullptr if they don't exist yet - const InterfaceState* external_from{ create_from ? new_external : internalToExternalMap().at(internal_from) }; - const InterfaceState* external_to{ create_to ? new_external : internalToExternalMap().at(internal_to) }; + const InterfaceState* external_from{ create_from ? new_from : internalToExternalMap().at(internal_from) }; + const InterfaceState* external_to{ create_to ? new_to : internalToExternalMap().at(internal_to) }; // computeCost // we can pass intern_{from/to} here because in this case the lifted states that might be created later @@ -222,17 +223,16 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I return; } - auto create_state = [this, new_external](const InterfaceState& internal) { - InterfaceState* external{ storeState(new_external ? InterfaceState{ *new_external } : - InterfaceState{ internal }) }; + auto create_state = [this](const InterfaceState& internal, InterfaceState* new_external) { + InterfaceState* external{ storeState(new_external ? std::move(*new_external) : InterfaceState{ internal }) }; internalToExternalMap().insert(std::make_pair(&internal, external)); return external; }; if (create_from) - external_from = create_state(*internal_from); + external_from = create_state(*internal_from, new_from); if (create_to) - external_to = create_state(*internal_to); + external_to = create_state(*internal_to, new_to); assert(external_from); assert(external_to); @@ -749,7 +749,7 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co solution.start(), solution.end()); } -void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase &child_solution) { +void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase& child_solution) { // child_solution is correctly prepared by a child of this container assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); @@ -758,13 +758,22 @@ void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solu child_solution.start(), child_solution.end()); } -void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr &&new_solution, InterfaceState &&new_propagated_state, const SolutionBase &child_solution) { +void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) { assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); + assert(pimpl()->requiredInterface() == PROPAGATE_FORWARDS || pimpl()->requiredInterface() == PROPAGATE_BACKWARDS); - pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state); + pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); } +void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) { + assert(child_solution.creator()); + assert(child_solution.creator()->parent() == this); + + pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_from, &new_to); +} + + WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} From d4b03edf6be7ab523bb1978b06cb94fa6c93e11f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 13 Apr 2021 15:24:09 +0200 Subject: [PATCH 06/10] Allow Generator wrappers to use lift with one new state It's just a nicer interface than calling `liftModifiedSolution(sol, InterfaceState{s}, InterfaceState{s}, child)` everywhere. --- core/src/container.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 427b2e993..954ab04b2 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -761,9 +761,16 @@ void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solu void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) { assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); - assert(pimpl()->requiredInterface() == PROPAGATE_FORWARDS || pimpl()->requiredInterface() == PROPAGATE_BACKWARDS); - pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); + if(pimpl()->requiredInterface() == GENERATE){ + // in this case we need a second InterfaceState to move from + InterfaceState new_to{ new_propagated_state }; + pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_to); + } + else { + // pass new_propagated_state as start *and* end. We know at most one will be used. + pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); + } } void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) { From 30b085b6a17635213a582ef5bb2d97f05411cf46 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 08:38:13 +0200 Subject: [PATCH 07/10] name test containers debugging to simplify debugging. --- core/test/test_cost_terms.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index a610ca043..4372ba558 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -310,7 +310,7 @@ TEST(CostTerm, CompositeSolutions) { { auto s1{ std::make_unique() }; auto s2{ std::make_unique() }; - auto c1{ std::make_unique() }; + auto c1{ std::make_unique("c1") }; auto constant1{ std::make_shared(1.0) }; s1->setCostTerm(constant1); s2->setCostTerm(constant1); @@ -334,7 +334,7 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { auto s1_ptr{ s1.get() }; auto s2{ std::make_unique() }; - auto c1{ std::make_unique() }; + auto c1{ std::make_unique("c1") }; c1->add(std::move(s1)); c1->add(std::move(s2)); From 525080d212a069df330d2809d7f37a229cb63a2b Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 12 May 2021 15:54:40 +0200 Subject: [PATCH 08/10] find external states also in writing-interfaces Essential for SerialContainer that spawns multiple solutions with the same end state. --- core/src/container.cpp | 56 +++++++++++++++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 12 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index 954ab04b2..317fb0c6c 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -206,38 +206,70 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to, InterfaceState* new_from, InterfaceState* new_to) { - const bool create_from{ requiredInterface().testFlag(WRITES_PREV_END) }; - const bool create_to{ requiredInterface().testFlag(WRITES_NEXT_START) }; + // NOLINTNEXTLINE(readability-identifier-naming) + auto findExternal = [this](const InterfaceState* internal, + const InterfaceState* replacement) -> const InterfaceState* { + auto it = internalToExternalMap().find(internal); + if (it != internalToExternalMap().end() && (!replacement || &*it->second == replacement)) { + return &*it->second; + } + + return nullptr; + }; // external states, nullptr if they don't exist yet - const InterfaceState* external_from{ create_from ? new_from : internalToExternalMap().at(internal_from) }; - const InterfaceState* external_to{ create_to ? new_to : internalToExternalMap().at(internal_to) }; + const InterfaceState* external_from{ findExternal(internal_from, new_from) }; + const InterfaceState* external_to{ findExternal(internal_to, new_to) }; // computeCost - // we can pass intern_{from/to} here because in this case the lifted states that might be created later + // If there are no external states known yet, we can pass internal_{from/to} here + // because in this case the lifted states that will be created later // are equivalent up to the connected Solutions (which are not relevant for CostTerms) - computeCost(external_from ? *external_from : *internal_from, external_to ? *external_to : *internal_to, *solution); + { + auto getPreliminaryState = [](const InterfaceState* external, const InterfaceState* new_state, + const InterfaceState* internal) -> const InterfaceState& { + if (external) + return *external; + if (new_state) + return *new_state; + else + return *internal; + }; + computeCost(getPreliminaryState(external_from, new_from, internal_from), + getPreliminaryState(external_to, new_to, internal_to), *solution); + } // storeSolution + // only pass stored external states here (others are not relevant for pruning) if (!storeSolution(solution, external_from, external_to)) { return; } - auto create_state = [this](const InterfaceState& internal, InterfaceState* new_external) { + // store unstored states in stage-internal storage + + // NOLINTNEXTLINE(readability-identifier-naming) + auto createState = [this](const InterfaceState& internal, InterfaceState* new_external) { InterfaceState* external{ storeState(new_external ? std::move(*new_external) : InterfaceState{ internal }) }; internalToExternalMap().insert(std::make_pair(&internal, external)); return external; }; - if (create_from) - external_from = create_state(*internal_from, new_from); - if (create_to) - external_to = create_state(*internal_to, new_to); + const bool create_from{ external_from == nullptr }; + const bool create_to{ external_to == nullptr }; + + if (create_from) { + assert(requiredInterface().testFlag(WRITES_PREV_END)); + external_from = createState(*internal_from, new_from); + } + if (create_to) { + assert(requiredInterface().testFlag(WRITES_NEXT_START)); + external_to = createState(*internal_to, new_to); + } assert(external_from); assert(external_to); - // connect solution to states + // connect solution to stored states solution->setStartState(*external_from); solution->setEndState(*external_to); From 8ac2968d8c3f4448d31c43aa2af11d5effc9a7d5 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 3 Jun 2021 11:30:53 +0200 Subject: [PATCH 09/10] accept const-refs in liftModifiedSolution clang-tidy rightfully complained that moving rvalues doesn't make sense when the underlying method accepts const refs. The rvalue parameter interface, as it is used with spawn/etc. indicates that the solution is submitted to the system. But with the shared_ptr interface we need for `liftModifiedSolution` it does not make any sense, especially because we can't even move the passed pointer to the internal datastructures but have to copy... --- core/include/moveit/task_constructor/container.h | 6 +++--- core/src/container.cpp | 15 +++++++-------- 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index a4911ae51..0d5d09f6f 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -128,13 +128,13 @@ class ParallelContainerBase : public ContainerBase void liftSolution(const SolutionBase& solution, double cost, std::string comment); /// lift a modified solution based on the solution of a child stage - void liftModifiedSolution(SolutionBasePtr&& new_solution, const SolutionBase& child_solution); + void liftModifiedSolution(const SolutionBasePtr& new_solution, const SolutionBase& child_solution); /// lift a modified solution, changing the (single!) new associated start or end InterfaceState - void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, + void liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution); /// lift a modified solution, providing new start and end states /// The new states will only be used if this's should actually create the corresponding states - void liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_start_state, + void liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_start_state, InterfaceState&& new_end_state, const SolutionBase& child_solution); }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 317fb0c6c..ee0a65433 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -781,35 +781,34 @@ void ParallelContainerBase::liftSolution(const SolutionBase& solution, double co solution.start(), solution.end()); } -void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& modified_solution, const SolutionBase& child_solution) { +void ParallelContainerBase::liftModifiedSolution(const SolutionBasePtr& modified_solution, const SolutionBase& child_solution) { // child_solution is correctly prepared by a child of this container assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); - pimpl()->liftSolution(std::move(modified_solution), - child_solution.start(), child_solution.end()); + pimpl()->liftSolution(modified_solution, child_solution.start(), child_solution.end()); } -void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) { +void ParallelContainerBase::liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_propagated_state, const SolutionBase& child_solution) { assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); if(pimpl()->requiredInterface() == GENERATE){ // in this case we need a second InterfaceState to move from InterfaceState new_to{ new_propagated_state }; - pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_to); + pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_propagated_state, &new_to); } else { // pass new_propagated_state as start *and* end. We know at most one will be used. - pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); + pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); } } -void ParallelContainerBase::liftModifiedSolution(SolutionBasePtr&& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) { +void ParallelContainerBase::liftModifiedSolution(const SolutionBasePtr& new_solution, InterfaceState&& new_from, InterfaceState&& new_to, const SolutionBase& child_solution) { assert(child_solution.creator()); assert(child_solution.creator()->parent() == this); - pimpl()->liftSolution(std::move(new_solution), child_solution.start(), child_solution.end(), &new_from, &new_to); + pimpl()->liftSolution(new_solution, child_solution.start(), child_solution.end(), &new_from, &new_to); } From 1ef46e2d4800342cf6e44ab5bb8ee7bf17f30f81 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 14 Jun 2021 13:35:25 +0200 Subject: [PATCH 10/10] WIP --- core/src/container.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/core/src/container.cpp b/core/src/container.cpp index ee0a65433..624007e0b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -207,10 +207,9 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I const InterfaceState* internal_to, InterfaceState* new_from, InterfaceState* new_to) { // NOLINTNEXTLINE(readability-identifier-naming) - auto findExternal = [this](const InterfaceState* internal, - const InterfaceState* replacement) -> const InterfaceState* { + auto findExternal = [this](const InterfaceState* internal) -> const InterfaceState* { auto it = internalToExternalMap().find(internal); - if (it != internalToExternalMap().end() && (!replacement || &*it->second == replacement)) { + if (it != internalToExternalMap().end()) { return &*it->second; } @@ -218,8 +217,18 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I }; // external states, nullptr if they don't exist yet - const InterfaceState* external_from{ findExternal(internal_from, new_from) }; - const InterfaceState* external_to{ findExternal(internal_to, new_to) }; + const InterfaceState* external_from{ findExternal(internal_from) }; + const InterfaceState* external_to{ findExternal(internal_to) }; + + // TODO(v4hn) rethink this. ComputeIK is exactly this case. Do I want to support an n:m mapping from internal to + // external? + if ((new_from && external_from && external_from != new_from) || (new_to && external_to && external_to != new_to)) { + ROS_ERROR_STREAM_NAMED("Container", "Container '" << name_ << "' tried to lift a modified solution from child '" + << solution->creator()->name() + << "', but a different one already exists. Children's " + "InterfaceStates can only ever match one "); + return; + } // computeCost // If there are no external states known yet, we can pass internal_{from/to} here