diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index fab732a71..0d5d09f6f 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -127,12 +127,15 @@ 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(const SolutionBasePtr& new_solution, const SolutionBase& child_solution); + /// lift a modified solution, changing the (single!) new associated start or end InterfaceState + 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(const 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 d62b9fbc6..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 + /// 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* internal_to, InterfaceState* new_from = nullptr, + InterfaceState* new_to = nullptr); /// protected writable overloads inline auto& internalToExternalMap() { return internal_external_.left; } 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..624007e0b 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -196,44 +196,99 @@ 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, - 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* { + const InterfaceState* internal_to, InterfaceState* new_from, + InterfaceState* new_to) { + // NOLINTNEXTLINE(readability-identifier-naming) + auto findExternal = [this](const InterfaceState* internal) -> const InterfaceState* { auto it = internalToExternalMap().find(internal); - if (it != internalToExternalMap().end()) - return const_cast(it->second); + if (it != internalToExternalMap().end()) { + return &*it->second; + } - InterfaceState* external = &*states_.insert(states_.end(), InterfaceState(*internal)); - internalToExternalMap().insert(std::make_pair(internal, external)); - created = true; - return external; + return nullptr; }; - 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)) + // external states, nullptr if they don't exist yet + 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 + // 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) + { + 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; + } + + // 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; + }; + + 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); + } - // connect solution to start/end state + assert(external_from); + assert(external_to); + + // connect solution to stored 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,18 +790,37 @@ 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(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(modified_solution, child_solution.start(), child_solution.end()); } -void ParallelContainerBase::sendForward(const InterfaceState& from, InterfaceState&& to, SubTrajectory&& t) { - pimpl()->StagePrivate::sendForward(from, std::move(to), std::make_shared(std::move(t))); +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(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(new_solution, child_solution.start(), child_solution.end(), &new_propagated_state, &new_propagated_state); + } } -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(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(new_solution, child_solution.start(), child_solution.end(), &new_from, &new_to); } + WrapperBasePrivate::WrapperBasePrivate(WrapperBase* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {} 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); 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 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));