diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h new file mode 100644 index 00000000..d61dce15 --- /dev/null +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -0,0 +1,174 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Hamburg University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael 'v4hn' Goerner + Desc: Define implementations for general CostTerm's to use with Stage::setCostTerm() +*/ + +#pragma once + +#include +#include + +namespace moveit { +namespace task_constructor { + +/** basic interface to compute costs for solutions + * + * If your cost term will only work on SubTrajectory solution objects, + * inherit from TrajectoryCostTerm instead. + */ + +MOVEIT_CLASS_FORWARD(CostTerm); +class CostTerm +{ +public: + CostTerm() = default; + CostTerm(std::nullptr_t) : CostTerm{} {} + + virtual double operator()(const SubTrajectory& s, std::string& comment) const; + virtual double operator()(const SolutionSequence& s, std::string& comment) const; + virtual double operator()(const WrappedSolution& s, std::string& comment) const; +}; + +/** base class for cost terms that only work on SubTrajectory solutions + * + */ +class TrajectoryCostTerm : public CostTerm +{ +public: + double operator()(const WrappedSolution& s, std::string& comment) const override; +}; + +class LambdaCostTerm : public TrajectoryCostTerm +{ +public: + using SubTrajectorySignature = std::function; + using SubTrajectoryShortSignature = std::function; + + // accept lambdas according to either signature above + template ()))> + LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {} + + LambdaCostTerm(const SubTrajectorySignature&); + LambdaCostTerm(const SubTrajectoryShortSignature&); + + double operator()(const SubTrajectory& s, std::string& comment) const override; + +protected: + SubTrajectorySignature term_; + +private: + template + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{}); + template + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{}); +}; + +namespace cost { + +/// add a constant cost to each solution +class Constant : public CostTerm +{ +public: + Constant(double c) : cost{ c } {}; + + double operator()(const SubTrajectory&, std::string&) const override; + double operator()(const SolutionSequence& s, std::string& comment) const override; + double operator()(const WrappedSolution& s, std::string& comment) const override; + + double cost; +}; + +/// trajectory length (interpolated between waypoints) +class PathLength : public TrajectoryCostTerm +{ + // TODO(v4hn): allow to consider specific joints only +public: + double operator()(const SubTrajectory&, std::string&) const override; +}; + +/// execution duration of the whole trajectory +class TrajectoryDuration : public TrajectoryCostTerm +{ +public: + double operator()(const SubTrajectory&, std::string&) const override; +}; + +/** length of Cartesian trajection of a link */ +class LinkMotion : public TrajectoryCostTerm +{ +public: + LinkMotion(std::string link_name); + + std::string link_name; + + double operator()(const SubTrajectory&, std::string&) const override; +}; + +/** inverse distance to collision + * + * \arg with_world check distances to world objects or look at self-collisions + * \arg cumulative if true, compute clearance as aggregated distance of all bodies + * \arg group_property the name of the property which defines the group to look at + * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over + * trajectory + * */ +class Clearance : public TrajectoryCostTerm +{ +public: + enum class Mode + { + AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, + START_INTERFACE, + END_INTERFACE, + TRAJECTORY + }; + + Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", + Mode mode = Mode::AUTO); + bool with_world; + bool cumulative; + std::string group_property; + + Mode mode; + + std::function distance_to_cost; + + double operator()(const SubTrajectory& s, std::string& comment) const override; +}; + +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4b4fcd30..c6ac9b57 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,6 +138,8 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); +MOVEIT_CLASS_FORWARD(CostTerm); +class LambdaCostTerm; class ContainerBase; class StagePrivate; class Stage @@ -159,6 +161,7 @@ public: PARENT = 2, INTERFACE = 4, }; + virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed @@ -212,6 +215,15 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + /** set method to determine costs for solutions of this stage */ + + void setCostTerm(const CostTermConstPtr& term); + // overload to accept appropriate lambda expressions + template ::value>> + void setCostTerm(T term) { + setCostTerm(std::make_shared(term)); + } + const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; @@ -227,8 +239,10 @@ public: void setProperty(const std::string& name, const boost::any& value); /// overload: const char* values are stored as std::string inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); } + /// analyze source of error and report accordingly [[noreturn]] void reportPropertyError(const Property::error& e); + double getTotalComputeTime() const; protected: diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 150080f5..3982f7c6 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -40,6 +40,7 @@ #include #include +#include #include #include @@ -154,8 +155,13 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } + /** compute cost for solution through configured CostTerm */ + void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); + protected: - Stage* me_; // associated/owning Stage instance + // associated/owning Stage instance + Stage* me_; + std::string name_; PropertyMap properties_; @@ -163,6 +169,9 @@ protected: InterfacePtr starts_; InterfacePtr ends_; + // user-configurable cost estimator + CostTermConstPtr cost_term_; + // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/include/moveit/task_constructor/stages/passthrough.h b/core/include/moveit/task_constructor/stages/passthrough.h new file mode 100644 index 00000000..61f1c40d --- /dev/null +++ b/core/include/moveit/task_constructor/stages/passthrough.h @@ -0,0 +1,61 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* Authors: Michael 'v4hn' Goerner */ + +#pragma once + +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +/** Generic Wrapper that passes on a solution + * + * Useful to set a custom CostTransform via Stage::setCostTerm + * to change a solution's cost without loosing the original value + */ +class PassThrough : public WrapperBase +{ +public: + PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer()); + + void onNewSolution(const SolutionBase& s) override; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3bf0e08f..25aa8c51 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -192,10 +192,15 @@ private: using base_type::remove_if; }; +class CostTerm; class StagePrivate; +class ContainerBasePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase { + friend StagePrivate; // for set[Start|End]StateUnsafe + friend ContainerBasePrivate; + public: virtual ~SolutionBase() = default; @@ -206,18 +211,22 @@ public: template inline const InterfaceState::Solutions& trajectories() const; + /** set the solution's start_state_ + * + * Must not be used with different states because it registers the solution with the state as well. + */ inline void setStartState(const InterfaceState& state) { - // only allow setting once (by Stage) assert(start_ == nullptr || start_ == &state); - start_ = &state; - const_cast(state).addOutgoing(this); + setStartStateUnsafe(state); } + /** set the solution's end_state_ + * + * Must not be used with different states because it registers the solution with the state as well. + */ inline void setEndState(const InterfaceState& state) { - // only allow setting once (by Stage) assert(end_ == nullptr || end_ == &state); - end_ = &state; - const_cast(state).addIncoming(this); + setEndStateUnsafe(state); } inline const Stage* creator() const { return creator_; } @@ -239,6 +248,9 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; + /// required to dispatch to type-specific CostTerm methods via vtable + virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0; + /// order solutions by their cost bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; } @@ -246,6 +258,18 @@ protected: SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "") : creator_(creator), cost_(cost), comment_(std::move(comment)) {} + /** unsafe setter for start_state_ + * + * must only be used if the previously set state removes its link to this solution + */ + void setStartStateUnsafe(const InterfaceState& state); + + /** unsafe setter for end_state_ + * + * must only be used if the previously set state removes its link to this solution + */ + void setEndStateUnsafe(const InterfaceState& state); + private: // back-pointer to creating stage, allows to access sub-solutions Stage* creator_; @@ -276,6 +300,8 @@ public: void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override; + double computeCost(const CostTerm& cost, std::string& comment) const override; + private: // actual trajectory, might be empty robot_trajectory::RobotTrajectoryConstPtr trajectory_; @@ -301,6 +327,8 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + double computeCost(const CostTerm& cost, std::string& comment) const override; + const container_type& solutions() const { return subsolutions_; } inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); } @@ -331,6 +359,8 @@ public: void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; + double computeCost(const CostTerm& cost, std::string& comment) const override; + const SolutionBase* wrapped() const { return wrapped_; } private: diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index cf8b6ad1..263c5033 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/solvers/pipeline_planner.h container.cpp + cost_terms.cpp introspection.cpp marker_tools.cpp merge.cpp diff --git a/core/src/container.cpp b/core/src/container.cpp index 2a089456..18892591 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -120,6 +120,10 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from, const InterfaceState* internal_to) { + solution->setCreator(me()); + + computeCost(*internal_from, *internal_to, *solution); + if (!storeSolution(solution)) return; @@ -139,8 +143,8 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I InterfaceState* external_to = find_or_create_external(internal_to, created_to); // connect solution to start/end state - solution->setStartState(*external_from); - solution->setEndState(*external_to); + solution->setStartStateUnsafe(*external_from); + solution->setEndStateUnsafe(*external_to); // spawn created states in external interfaces if (created_from) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp new file mode 100644 index 00000000..a1384c0a --- /dev/null +++ b/core/src/cost_terms.cpp @@ -0,0 +1,250 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner */ + +#include +#include + +#include +#include + +#include + +#include + +#include + +namespace moveit { +namespace task_constructor { + +double CostTerm::operator()(const SubTrajectory& s, std::string&) const { + return s.cost(); +} + +double CostTerm::operator()(const SolutionSequence& s, std::string& comment) const { + double cost{ 0.0 }; + std::string subcomment; + for (auto& solution : s.solutions()) { + cost += solution->computeCost((*this), subcomment); + if (!subcomment.empty()) { + if (!comment.empty()) + comment.append(", "); + comment.append(subcomment); + subcomment.clear(); + } + } + + return cost; +} + +double CostTerm::operator()(const WrappedSolution& s, std::string& /*comment*/) const { + return s.cost(); +} + +double TrajectoryCostTerm::operator()(const WrappedSolution& s, std::string& comment) const { + return s.wrapped()->computeCost(*this, comment); +} + +LambdaCostTerm::LambdaCostTerm(const SubTrajectorySignature& term) + : term_{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); } } {} + +LambdaCostTerm::LambdaCostTerm(const SubTrajectoryShortSignature& term) + : term_{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); } } {} + +double LambdaCostTerm::operator()(const SubTrajectory& s, std::string& comment) const { + assert(bool{ term_ }); + return term_(s, comment); +} + +namespace cost { + +double Constant::operator()(const SubTrajectory&, std::string&) const { + return cost; +} + +double Constant::operator()(const SolutionSequence&, std::string&) const { + return cost; +} + +double Constant::operator()(const WrappedSolution&, std::string&) const { + return cost; +} + +double PathLength::operator()(const SubTrajectory& s, std::string&) const { + const auto& traj = s.trajectory(); + + if (traj == nullptr) + return 0.0; + + double path_length{ 0.0 }; + for (size_t i = 1; i < traj->getWayPointCount(); ++i) + path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i)); + return path_length; +} + +double TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; +} + +LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {} + +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { + const auto& traj{ s.trajectory() }; + + if (traj == nullptr || traj->getWayPointCount() == 0) + return 0.0; + + if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) { + boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory"); + desc % link_name; + comment = desc.str(); + return std::numeric_limits::infinity(); + } + + double distance{ 0.0 }; + Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { + const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position - position).norm(); + position = new_position; + } + return distance; +} + +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) + : with_world{ with_world } + , cumulative{ cumulative } + , group_property{ group_property } + , mode{ mode } + , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} + +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { + const std::string PREFIX{ "Clearance: " }; + + collision_detection::DistanceRequest request; + request.type = + cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; + + const auto& state{ (mode == Mode::END_INTERFACE) ? s.end() : s.start() }; + + // prefer interface state property over stage property to find group_name + // TODO: This pattern is general enough to justify its own interface (in the properties?). + auto& state_properties{ state->properties() }; + auto& stage_properties{ s.creator()->properties() }; + request.group_name = state_properties.hasProperty(group_property) ? + state_properties.get(group_property) : + stage_properties.get(group_property); + + // look at all forbidden collisions involving group_name + request.enableGroup(state->scene()->getRobotModel()); + request.acm = &state->scene()->getAllowedCollisionMatrix(); + + // compute relevant distance data for state & robot + auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) { + collision_detection::DistanceResult result; + if (with_world) +#if MOVEIT_MASTER + state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); +#else + state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(), + robot); +#endif + else +#if MOVEIT_MASTER + state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); +#else + state->scene()->getCollisionRobot()->distanceSelf(request, result, robot); +#endif + + if (result.minimum_distance.distance <= 0) { + return result.minimum_distance; + } + + if (cumulative) { + double distance{ 0.0 }; + for (const auto& distance_of_pair : result.distances) { + assert(distance_of_pair.second.size() == 1); + distance += distance_of_pair.second[0].distance; + } + result.minimum_distance.distance = distance; + } + + return result.minimum_distance; + } }; + + auto collision_comment{ [=](const auto& distance) { + boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" }; + desc % distance.link_names[0] % distance.link_names[1]; + return desc.str(); + } }; + + double distance{ 0.0 }; + + if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || + (mode == Mode::AUTO && s.trajectory() == nullptr)) { + auto distance_data{ check_distance(state, state->scene()->getCurrentState()) }; + if (distance_data.distance < 0) { + comment = collision_comment(distance_data); + return std::numeric_limits::infinity(); + } + distance = distance_data.distance; + if (!cumulative) { + boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" }; + desc % distance % distance_data.link_names[0] % distance_data.link_names[1]; + comment = desc.str(); + } else { + comment = PREFIX + "cumulative distance " + std::to_string(distance); + } + } else { // check trajectory + for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) { + auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i)); + if (distance_data.distance < 0) { + comment = collision_comment(distance_data); + return std::numeric_limits::infinity(); + } + distance += distance_data.distance; + } + distance /= s.trajectory()->getWayPointCount(); + + boost::format desc(PREFIX + "average%1% distance: %2%"); + desc % (cumulative ? " cumulative" : "") % distance; + comment = desc.str(); + } + + return distance_to_cost(distance); +} +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 38761006..58abd555 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -97,7 +97,12 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { } StagePrivate::StagePrivate(Stage* me, const std::string& name) - : me_(me), name_(name), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {} + : me_{ me } + , name_{ name } + , cost_term_{ std::make_unique() } + , total_compute_time_{} + , parent_{ nullptr } + , introspection_{ nullptr } {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -125,7 +130,6 @@ void StagePrivate::validateConnectivity() const { } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); @@ -142,14 +146,21 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(nextStarts()); + + solution->setCreator(me()); + + computeCost(from, to, *solution); + if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); - solution->setStartState(from); - solution->setEndState(*to_it); + // register stored interfaces with solution + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(*to_it); if (!solution->isFailure()) nextStarts()->add(*to_it); @@ -159,14 +170,20 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) { assert(prevEnds()); + + solution->setCreator(me()); + + computeCost(from, to, *solution); + if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); - solution->setStartState(*from_it); - solution->setEndState(to); + solution->setStartStateUnsafe(*from_it); + solution->setEndStateUnsafe(to); if (!solution->isFailure()) prevEnds()->add(*from_it); @@ -176,14 +193,19 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { assert(prevEnds() && nextStarts()); + + solution->setCreator(me()); + + computeCost(state, state, *solution); + if (!storeSolution(solution)) return; // solution dropped auto from = states_.insert(states_.end(), InterfaceState(state)); // copy auto to = states_.insert(states_.end(), std::move(state)); - solution->setStartState(*from); - solution->setEndState(*to); + solution->setStartStateUnsafe(*from); + solution->setEndStateUnsafe(*to); if (!solution->isFailure()) { prevEnds()->add(*from); @@ -194,11 +216,15 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution } void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { + solution->setCreator(me()); + + computeCost(from, to, *solution); + if (!storeSolution(solution)) return; // solution dropped - solution->setStartState(from); - solution->setEndState(to); + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(to); newSolution(solution); } @@ -212,6 +238,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } +void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { + // no reason to compute costs for a failed solution + if (solution.isFailure()) + return; + + // chicken-and-egg problem: we don't know whether/where we will store the solution yet, + // but need to store it properly to compute the cost with a clean interface: + // set state copies for solution before computing cost: + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution.setStartState(tmp_from); + solution.setEndState(tmp_to); + + std::string comment; + assert(cost_term_); + solution.setCost(solution.computeCost(*cost_term_, comment)); + + // If a comment was specified, add it to the solution + if (!comment.empty() && !solution.comment().empty()) { + solution.setComment(solution.comment() + " (" + comment + ")"); + } else if (!comment.empty()) { + solution.setComment(comment); + } +} + Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); @@ -254,8 +304,9 @@ void Stage::reset() { } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { - // init properties once from parent auto impl = pimpl(); + + // init properties once from parent impl->properties_.reset(); if (impl->parent()) { try { @@ -308,6 +359,13 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } +void Stage::setCostTerm(const CostTermConstPtr& term) { + if (!term) + pimpl()->cost_term_ = std::make_unique(); + else + pimpl()->cost_term_ = std::move(term); +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index c254762f..857a4977 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h ${PROJECT_INCLUDE}/stages/compute_ik.h + ${PROJECT_INCLUDE}/stages/passthrough.h ${PROJECT_INCLUDE}/stages/predicate_filter.h ${PROJECT_INCLUDE}/stages/connect.h @@ -28,6 +29,7 @@ add_library(${PROJECT_NAME}_stages generate_grasp_pose.cpp generate_place_pose.cpp compute_ik.cpp + passthrough.cpp predicate_filter.cpp connect.cpp diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0d07f52d..c9f4cd06 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -40,12 +40,16 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); + setCostTerm(std::make_unique()); + auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); p.declare("path_constraints", moveit_msgs::Constraints(), @@ -181,15 +185,6 @@ Connect::makeSequential(const std::vectorgetWayPointDurations()) - cost += distance; - } - SolutionSequence::container_type sub_solutions; for (const auto& sub : sub_trajectories) { planning_scene::PlanningSceneConstPtr end_ps = *++scene_it; @@ -212,24 +207,15 @@ Connect::makeSequential(const std::vector(std::move(sub_solutions), cost); + return std::make_shared(std::move(sub_solutions)); } SubTrajectoryPtr Connect::merge(const std::vector& sub_trajectories, const std::vector& intermediate_scenes, const moveit::core::RobotState& state) { - // calculate cost - double cost = 0; - for (const auto& trajectory : sub_trajectories) { - if (!trajectory) - continue; - for (const double& distance : trajectory->getWayPointDurations()) - cost += distance; - } - // no need to merge if there is only a single sub trajectory if (sub_trajectories.size() == 1) - return std::make_shared(sub_trajectories[0], cost); + return std::make_shared(sub_trajectories[0]); auto jmg = merged_jmg_.get(); assert(jmg); @@ -242,7 +228,7 @@ SubTrajectoryPtr Connect::merge(const std::vector("path_constraints"))) return SubTrajectoryPtr(); - return std::make_shared(trajectory, cost); + return std::make_shared(trajectory); } } // namespace stages } // namespace task_constructor diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 2e518608..4546454e 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -40,6 +40,8 @@ #include #include +#include + #include #include #include @@ -53,6 +55,9 @@ namespace task_constructor { namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { + // TODO: possibly weight solutions based on the required displacement? + setCostTerm(std::make_unique(0.0)); + auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); p.declare("direction", "direction vector to use for corrections"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index aecd16ee..068f2759 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -36,7 +36,9 @@ #include #include +#include #include + #include #include @@ -47,6 +49,8 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { + setCostTerm(std::make_unique(0.0)); + auto& p = properties(); p.declare("poses", PosesList(), "target poses to spawn"); } diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index b6a2aa5a..fddc9e4d 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -35,13 +35,17 @@ /* Authors: Robert Haschke */ #include +#include + #include namespace moveit { namespace task_constructor { namespace stages { -FixedState::FixedState(const std::string& name) : Generator(name) {} +FixedState::FixedState(const std::string& name) : Generator(name) { + setCostTerm(std::make_unique(0.0)); +} void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { scene_ = scene; diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index ebcee60c..8315bc95 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,8 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { + setCostTerm(std::make_unique(0.0)); + auto& p = properties(); p.declare("pose", "target pose to pass on in spawned states"); } diff --git a/core/src/stages/modify_planning_scene.cpp b/core/src/stages/modify_planning_scene.cpp index f57650b8..c14605a6 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -38,13 +38,17 @@ #include #include +#include + #include namespace moveit { namespace task_constructor { namespace stages { -ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {} +ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { + setCostTerm(std::make_unique(0.0)); +} void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { auto it_inserted = attach_objects_.insert(std::make_pair(attach_link, std::make_pair(Names(), attach))); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index b5544a0e..e5266825 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -37,6 +37,8 @@ */ #include +#include + #include #include #include @@ -47,6 +49,8 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { + setCostTerm(std::make_unique()); + auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); @@ -291,13 +295,6 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning robot_trajectory->reverse(); solution.setTrajectory(robot_trajectory); - // set cost - double cost = 0; - for (const double& distance : robot_trajectory->getWayPointDurations()) { - cost += distance; - } - solution.setCost(cost); - if (!success) solution.markAsFailure(); return true; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index a3c09a36..f9f915c3 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -37,6 +37,8 @@ */ #include +#include + #include #include #include @@ -48,6 +50,8 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { + setCostTerm(std::make_unique()); + auto& p = properties(); p.property("timeout").setDefaultValue(1.0); p.declare("group", "name of planning group"); @@ -254,13 +258,6 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP robot_trajectory->reverse(); solution.setTrajectory(robot_trajectory); - // set cost - double cost = 0; - for (const double& distance : robot_trajectory->getWayPointDurations()) { - cost += distance; - } - solution.setCost(cost); - if (!success) solution.markAsFailure(); diff --git a/core/src/stages/passthrough.cpp b/core/src/stages/passthrough.cpp new file mode 100644 index 00000000..786221e1 --- /dev/null +++ b/core/src/stages/passthrough.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Hamburg University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holders nor the names of their + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* Author: Michael 'v4hn' Goerner */ + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace stages { + +PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} + +void PassThrough::onNewSolution(const SolutionBase& s) { + this->liftSolution(s); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ed53b4ec..be1690ab 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -141,6 +141,16 @@ void SolutionBase::setCreator(Stage* creator) { creator_ = creator; } +void SolutionBase::setStartStateUnsafe(const InterfaceState& state) { + start_ = &state; + const_cast(state).addOutgoing(this); +} + +void SolutionBase::setEndStateUnsafe(const InterfaceState& state) { + end_ = &state; + const_cast(state).addIncoming(this); +} + void SolutionBase::setCost(double cost) { cost_ = cost; } @@ -168,6 +178,10 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff); } +double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const { + return f(*this, comment); +} + void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } @@ -207,6 +221,10 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } } +double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { + return f(*this, comment); +} + void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); @@ -217,5 +235,10 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0); solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg)); } + +double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const { + return f(*this, comment); +} + } // namespace task_constructor } // namespace moveit diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index f36fde93..24ec06a4 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -24,6 +24,8 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp) target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main) + catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp) + target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp new file mode 100644 index 00000000..ee62f0c8 --- /dev/null +++ b/core/test/test_cost_terms.cpp @@ -0,0 +1,351 @@ +#include "models.h" + +#include +#include +#include +#include + +#include + +#include + +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; + +const double STAGE_COST{ 7.0 }; + +const double TRAJECTORY_DURATION{ 9.0 }; + +class GeneratorMockup : public Generator +{ + PlanningScenePtr ps; + InterfacePtr prev; + InterfacePtr next; + +public: + GeneratorMockup() : Generator("generator") { + prev.reset(new Interface); + next.reset(new Interface); + pimpl()->setPrevEnds(prev); + pimpl()->setNextStarts(next); + } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + ps.reset(new PlanningScene(robot_model)); + Generator::init(robot_model); + } + + bool canCompute() const override { return true; } + + void compute() override { + InterfaceState state(ps); + spawn(std::move(state), STAGE_COST); + } +}; + +class ConnectMockup : public Connecting +{ + using Connecting::Connecting; + + void compute(const InterfaceState& from, const InterfaceState& to) override { + auto solution{ std::make_shared() }; + solution->setCost(STAGE_COST); + connect(from, to, solution); + } +}; + +class ForwardMockup : public PropagatingForward +{ + PlanningScenePtr ps; + +public: + using PropagatingForward::PropagatingForward; + + void init(const moveit::core::RobotModelConstPtr& robot_model) override { + ps.reset(new PlanningScene(robot_model)); + PropagatingForward::init(robot_model); + } + + void computeForward(const InterfaceState& from) override { + SubTrajectory solution; + auto traj{ std::make_shared(ps->getRobotModel(), + ps->getRobotModel()->getJointModelGroups()[0]) }; + traj->addSuffixWayPoint(ps->getCurrentState(), 0.0); + traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION); + solution.setTrajectory(traj); + solution.setCost(STAGE_COST); + InterfaceState to(from); + + sendForward(from, std::move(to), std::move(solution)); + }; +}; + +class BackwardMockup : public PropagatingBackward +{ + using PropagatingBackward::PropagatingBackward; + + void computeBackward(const InterfaceState& to) override { + SubTrajectory solution; + solution.setCost(STAGE_COST); + InterfaceState from(to); + + sendBackward(std::move(from), to, std::move(solution)); + }; +}; + +template +class Standalone : public T +{ + moveit::core::RobotModelConstPtr robot; + InterfacePtr dummy; + planning_scene::PlanningSceneConstPtr ps; + InterfaceStatePtr state_start, state_end; + +public: + Standalone(const moveit::core::RobotModelConstPtr& robot) + : T(), robot(robot), dummy(std::make_shared()), ps(new planning_scene::PlanningScene(robot)) {} + + // reset and prepare for a compute step + void prepare() { + auto impl{ this->pimpl() }; + this->reset(); + + state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); + state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); + + // infer and setup interface from children + Stage* s{ this }; + ContainerBase* b; + + while ((b = dynamic_cast(s))) + s = &(*b->pimpl()->children().front()); + InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK }; + + s = this; + while ((b = dynamic_cast(s))) + s = &(*b->pimpl()->children().back()); + InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK }; + + InterfaceFlags flags{ start_flags | end_flags }; + + impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); + impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); + T::init(robot); + impl->resolveInterface(flags); + + // feed interfaces as required for one computation + if (flags & READS_START) { + impl->starts()->add(*state_start); + } + if (flags & READS_END) { + impl->ends()->add(*state_end); + } + } + + void runCompute() { this->pimpl()->runCompute(); } + + void computeWithStages(std::initializer_list stages) { + this->clear(); + + // initializer_list offers only const access (a known shortcoming in C++) + for (const auto& stage : stages) { + this->add(const_cast(stage)); + } + + prepare(); + runCompute(); + } + + void computeWithStageCost(std::initializer_list stages, const CostTermConstPtr& cost_term) { + this->setCostTerm(nullptr); + for (const auto& stage : stages) + const_cast(stage)->setCostTerm(cost_term); + computeWithStages(stages); + } + + void computeWithContainerCost(std::initializer_list stages, const CostTermPtr& cost_term) { + this->setCostTerm(cost_term); + computeWithStages(stages); + } +}; + +TEST(CostTerm, SetLambdaCostTerm) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + Standalone container(robot); + auto stage{ std::make_unique() }; + stage->setCostTerm([](auto&&) { return 1.0; }); + container.computeWithStages({ std::move(stage) }); + EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature"; + + stage = std::make_unique(); + stage->setCostTerm([](auto&&, auto&&) { return 1.0; }); + container.computeWithStages({ std::move(stage) }); + EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature"; + + stage = std::make_unique(); + stage->setCostTerm([](auto&&, auto&& comment) { + comment = "I want the user to see this"; + return 1.0; + }); + container.computeWithStages({ std::move(stage) }); + auto sol = std::dynamic_pointer_cast(container.solutions().front()); + EXPECT_EQ(sol->cost(), 1.0); + EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment"; +} + +TEST(CostTerm, CostOverwrite) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + Standalone container(robot); + + container.computeWithStages({ std::make_unique() }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; + + container.computeWithStageCost({ std::make_unique() }, nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; + + auto constant_cost{ std::make_shared(1.0) }; + container.computeWithStageCost({ std::make_unique() }, constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost"; +} + +TEST(CostTerm, StageTypes) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + + Standalone container(robot); + + auto constant{ std::make_shared(1.0) }; + + // already tested above + // cont.computeWithStageCost(std::make_unique(), constant); + // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); + + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect"; + + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator"; + + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator"; +} + +TEST(CostTerm, PassThroughUsesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + auto constant{ std::make_shared(84.0) }; + stage->setCostTerm(constant); + + container.computeWithStages({ std::move(stage) }); + + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant->cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "Child cost equals PassThrough cost"; +} + +TEST(CostTerm, PassThroughOverwritesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + auto constant_inner{ std::make_shared(84.0) }; + stage->setCostTerm(constant_inner); + + auto constant_outer{ std::make_shared(72.0) }; + container.setCostTerm(constant_outer); + + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_outer->cost) << "PassThrough can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_inner->cost) << "child's cost is not affected"; +} + +TEST(CostTerm, PassThroughCanModifyCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + Standalone container(robot); + + auto stage{ std::make_unique() }; + auto constant{ std::make_shared(8.0) }; + stage->setCostTerm(constant); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant->cost * constant->cost) << "PassThrough can compute cost based on child"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "child's cost is not affected"; +} + +TEST(CostTerm, CompositeSolutions) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + + container.computeWithStages({ std::move(s1), std::move(s2) }); + EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added"; + } + + { + auto s1{ std::make_unique() }; + auto constant{ std::make_shared(1.0) }; + s1->setCostTerm(constant); + auto s2{ std::make_unique() }; + + container.computeWithStages({ std::move(s1), std::move(s2) }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost) + << "mix of explicit and implicit cost terms works"; + } + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto c1{ std::make_unique() }; + auto constant1{ std::make_shared(1.0) }; + s1->setCostTerm(constant1); + s2->setCostTerm(constant1); + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + auto constant2{ std::make_shared(9.0) }; + s3->setCostTerm(constant2); + + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1->cost + constant2->cost) + << "cost aggregation works across multiple levels"; + } +} + +TEST(CostTerm, CompositeSolutionsContainerCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + auto s1{ std::make_unique() }; + auto s1_ptr{ s1.get() }; + auto s2{ std::make_unique() }; + + auto c1{ std::make_unique() }; + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + + container.setCostTerm(std::make_unique()); + container.computeWithStages({ std::move(c1), std::move(s3) }); + EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) + << "container cost term overwrites stage costs"; + EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; +}