From 9337743de2654895961033aa57b679bc1409a744 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:25:30 +0200 Subject: [PATCH 01/42] add CostTerm interface basic stages can now be configured with arbitrary cost terms. --- .../moveit/task_constructor/container.h | 3 +++ core/include/moveit/task_constructor/stage.h | 6 ++++++ .../include/moveit/task_constructor/stage_p.h | 13 +++++++++++- core/src/stage.cpp | 21 ++++++++++++++++--- 4 files changed, 39 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8aa557a9 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -80,6 +80,9 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); + +private: + using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 4b4fcd30..bcb9f1f3 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -159,6 +159,7 @@ public: PARENT = 2, INTERFACE = 4, }; + virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed @@ -212,6 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); + using CostTerm = std::function; + void setCostTerm(const CostTerm&); + const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; @@ -227,8 +231,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..3fbabfa0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -154,8 +154,16 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } + /** compute cost for solution through configured CostTerm + * + * @return true if solution remains feasible (is no failure) + */ + bool addCost(SolutionBase& solution); + protected: - Stage* me_; // associated/owning Stage instance + // associated/owning Stage instance + Stage* me_; + std::string name_; PropertyMap properties_; @@ -163,6 +171,9 @@ protected: InterfacePtr starts_; InterfacePtr ends_; + // user-configurable cost estimator + Stage::CostTerm cost_term_; + // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 38761006..45932615 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -151,8 +151,9 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setStartState(from); solution->setEndState(*to_it); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) { nextStarts()->add(*to_it); + } newSolution(solution); } @@ -168,7 +169,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setStartState(*from_it); solution->setEndState(to); - if (!solution->isFailure()) + if (!solution->isFailure() && addCost(*solution)) prevEnds()->add(*from_it); newSolution(solution); @@ -185,7 +186,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setStartState(*from); solution->setEndState(*to); - if (!solution->isFailure()) { + if (!solution->isFailure() && addCost(*solution)) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -200,6 +201,9 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, solution->setStartState(from); solution->setEndState(to); + if (!solution->isFailure()) + addCost(*solution); + newSolution(solution); } @@ -212,6 +216,13 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } +bool StagePrivate::addCost(SolutionBase& solution) { + if (cost_term_) + solution.setCost(cost_term_(solution)); + + return !solution.isFailure(); +} + Stage::Stage(StagePrivate* impl) : pimpl_(impl) { assert(impl); auto& p = properties(); @@ -308,6 +319,10 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } +void Stage::setCostTerm(const CostTerm& cost_term) { + pimpl()->cost_term_ = cost_term; +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; } From 9eae554d89770938bca91adb8f1012107388f7ac Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:54:31 +0200 Subject: [PATCH 02/42] provide Constant and PathLength cost terms --- .../moveit/task_constructor/cost_terms.h | 27 ++++++++ core/src/CMakeLists.txt | 1 + core/src/cost_terms.cpp | 61 +++++++++++++++++++ 3 files changed, 89 insertions(+) create mode 100644 core/include/moveit/task_constructor/cost_terms.h create mode 100644 core/src/cost_terms.cpp 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..586797ea --- /dev/null +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -0,0 +1,27 @@ +#pragma once + +#include + +namespace moveit { +namespace task_constructor { +namespace cost { + +/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() + +/// add a constant cost to each solution +class ConstantCost +{ +public: + ConstantCost(double cost) : cost_(cost) {} + + double operator()(const SolutionBase&) { return cost_; } + +private: + double cost_; +}; + +/// define cost as execution duration of the whole trajectory +double PathLengthCost(const SolutionBase& s); +} +} +} 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/cost_terms.cpp b/core/src/cost_terms.cpp new file mode 100644 index 00000000..cc0f3758 --- /dev/null +++ b/core/src/cost_terms.cpp @@ -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 Goerner */ + +#include +#include + +namespace moveit { +namespace task_constructor { +namespace cost { + +double PathLengthCost(const SolutionBase& s) { + auto* trajectory{ dynamic_cast(&s) }; + + if (trajectory) { + return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0; + } else { + // there is no other alternative implementation of SolutionBase so static_cast is safe + auto& seq{ *static_cast(&s) }; + + double cost{ 0.0 }; + for (auto& sub : seq.solutions()) + cost += PathLengthCost(*sub); + return cost; + } +} +} +} +} From ffc62c2d1123dd339e8e47d99ad133a30ed7e3dd Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 19 May 2020 16:59:12 +0200 Subject: [PATCH 03/42] add explicit default CostTerms to most basic stages Simplifies code and makes the costs of each stage explicit. --- core/src/stages/connect.cpp | 28 ++++++----------------- core/src/stages/fix_collision_objects.cpp | 5 ++++ core/src/stages/fixed_cartesian_poses.cpp | 4 ++++ core/src/stages/fixed_state.cpp | 6 ++++- core/src/stages/generate_pose.cpp | 3 +++ core/src/stages/modify_planning_scene.cpp | 6 ++++- core/src/stages/move_relative.cpp | 11 ++++----- core/src/stages/move_to.cpp | 11 ++++----- 8 files changed, 37 insertions(+), 37 deletions(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 0d07f52d..8d38da55 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(cost::PathLengthCost); + 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..0d0f9cc8 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(cost::ConstantCost(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..a78c2414 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(cost::ConstantCost(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..d76d37ac 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(cost::ConstantCost(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..85efbc44 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(cost::ConstantCost(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..36a45753 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(cost::ConstantCost(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..da398dcd 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(cost::PathLengthCost); + 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..21db9815 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(cost::PathLengthCost); + 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(); From f79625d1bd54be2fedc9e843155f53c9ca1afeec Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 May 2020 14:26:33 +0200 Subject: [PATCH 04/42] fully restrict CostTerms to primitive solutions simplify implementations. --- .../include/moveit/task_constructor/cost_terms.h | 5 ++--- core/include/moveit/task_constructor/stage.h | 3 +-- core/src/cost_terms.cpp | 16 ++-------------- core/src/stage.cpp | 9 +++++---- 4 files changed, 10 insertions(+), 23 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 586797ea..a1ad49ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -7,21 +7,20 @@ namespace task_constructor { namespace cost { /// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() - /// add a constant cost to each solution class ConstantCost { public: ConstantCost(double cost) : cost_(cost) {} - double operator()(const SolutionBase&) { return cost_; } + double operator()(const SubTrajectory&) { return cost_; } private: double cost_; }; /// define cost as execution duration of the whole trajectory -double PathLengthCost(const SolutionBase& s); +double PathLengthCost(const SubTrajectory& s); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index bcb9f1f3..2336b633 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,8 +213,7 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; - void setCostTerm(const CostTerm&); + using CostTerm = std::function; const ordered& solutions() const; const std::list& failures() const; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index cc0f3758..a508103a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -41,20 +41,8 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SolutionBase& s) { - auto* trajectory{ dynamic_cast(&s) }; - - if (trajectory) { - return trajectory->trajectory() ? trajectory->trajectory()->getDuration() : 0.0; - } else { - // there is no other alternative implementation of SolutionBase so static_cast is safe - auto& seq{ *static_cast(&s) }; - - double cost{ 0.0 }; - for (auto& sub : seq.solutions()) - cost += PathLengthCost(*sub); - return cost; - } +double PathLengthCost(const SubTrajectory& s) { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 45932615..0e29e674 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -217,8 +217,9 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { } bool StagePrivate::addCost(SolutionBase& solution) { - if (cost_term_) - solution.setCost(cost_term_(solution)); + auto* trajectory{ dynamic_cast(&solution) }; + if (trajectory && cost_term_) + solution.setCost(cost_term_(*trajectory)); return !solution.isFailure(); } @@ -319,8 +320,8 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } -void Stage::setCostTerm(const CostTerm& cost_term) { - pimpl()->cost_term_ = cost_term; +void Stage::setCostTerm(const CostTerm& term) { + pimpl()->cost_term_ = term; } const ordered& Stage::solutions() const { From f3fec26f5a3281996a037d23a259a916faa260a9 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 20 May 2020 14:27:33 +0200 Subject: [PATCH 05/42] add CostTransform CostTerms only apply to primitive solutions and generalizing them to Containers would make them quite unintuitive (and adds overhead). Instead CostTransform can be used in any container to scale, crop, square the cost of the solutions. I initially thought about adding scaling factors, but then again, other transforms are of interest just as well. --- core/include/moveit/task_constructor/stage.h | 21 +++++++++++++++++++ .../include/moveit/task_constructor/stage_p.h | 1 + core/src/stage.cpp | 21 +++++++++++++++++-- 3 files changed, 41 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 2336b633..397a56a6 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -214,6 +214,27 @@ public: void removeSolutionCallback(SolutionCallbackList::const_iterator which); using CostTerm = std::function; + using CostTransform = std::function; + + /* \brief set method to determine costs for solutions of this stage + * + * This interface supports only primitive solutions, no containers. + * + * In order to support containers, the associated CostTerms would need to be able + * to decide costs for arbitrary partial solutions of the container. + * These need to be determined for scheduling, but would make the terms too complex. + * + * See setCostTransform + */ + void setCostTerm(const CostTerm& term); + + /* \brief set CostTransform to be applied to costs of this stage's solution costs + * + * This interface can be used to modify the cost associated with each solution of this stage/container. + * The most common case would be linear weighting `[=a](double c){ return a*c; }` + * but any non-linear transform is obviously possible as well. + */ + void setCostTransform(const CostTransform& transform); const ordered& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 3fbabfa0..ad0f1bfa 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -173,6 +173,7 @@ protected: // user-configurable cost estimator Stage::CostTerm cost_term_; + Stage::CostTransform cost_transform_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 0e29e674..37396f8f 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_transform_([](auto x) { return x; }) + , total_compute_time_{} + , parent_(nullptr) + , introspection_(nullptr) {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -217,9 +222,17 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { } bool StagePrivate::addCost(SolutionBase& solution) { + double cost{ 0.0 }; + auto* trajectory{ dynamic_cast(&solution) }; if (trajectory && cost_term_) - solution.setCost(cost_term_(*trajectory)); + // CostTerm only applies to SubTrajectory + cost = cost_term_(*trajectory); + else + // everything else is just transformed + cost = solution.cost(); + + solution.setCost(cost_transform_(cost)); return !solution.isFailure(); } @@ -324,6 +337,10 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } +void Stage::setCostTransform(const CostTransform& transform) { + pimpl()->cost_transform_ = transform; +} + const ordered& Stage::solutions() const { return pimpl()->solutions_; } From 9060404037b9936d059a296d888a64b0e9da92d2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 19 Jun 2020 23:07:57 +0200 Subject: [PATCH 06/42] improve comments --- core/include/moveit/task_constructor/cost_terms.h | 2 +- core/include/moveit/task_constructor/stage.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index a1ad49ed..7a7f942b 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -19,7 +19,7 @@ private: double cost_; }; -/// define cost as execution duration of the whole trajectory +/// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 397a56a6..5a229f9d 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -231,7 +231,7 @@ public: /* \brief set CostTransform to be applied to costs of this stage's solution costs * * This interface can be used to modify the cost associated with each solution of this stage/container. - * The most common case would be linear weighting `[=a](double c){ return a*c; }` + * The most common case would be linear weighting `[](double cost){ return constant*cost; }` * but any non-linear transform is obviously possible as well. */ void setCostTransform(const CostTransform& transform); From 56268cb6cc0f51dfa6668e29084e4182e511f8c2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 19 Jun 2020 23:08:12 +0200 Subject: [PATCH 07/42] first attempts on ClearanceCost --- .../moveit/task_constructor/cost_terms.h | 3 ++ core/src/cost_terms.cpp | 36 +++++++++++++++++++ 2 files changed, 39 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 7a7f942b..821afd94 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -21,6 +21,9 @@ private: /// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); + +/// distance to self-collision +double ClearanceCost(const SubTrajectory& s); } } } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index a508103a..00f3754f 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -35,7 +35,12 @@ /* Authors: Michael Goerner */ #include +#include + #include +#include + +#include namespace moveit { namespace task_constructor { @@ -44,6 +49,37 @@ namespace cost { double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } + +double ClearanceCost(const SubTrajectory& s) { + collision_detection::DistanceRequest request; + request.type = collision_detection::DistanceRequestType::GLOBAL; + + // TODO: possibly parameterize hardcoded property name? + const std::string group{ "group" }; + auto& state_properties{ s.start()->properties() }; + auto& stage_properties{ s.creator()->properties() }; + request.group_name = state_properties.hasProperty(group) ? state_properties.get(group) : + stage_properties.get(group); + + request.enableGroup(s.start()->scene()->getRobotModel()); + request.acm = &s.start()->scene()->getAllowedCollisionMatrix(); + + collision_detection::DistanceResult result; + + s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + + if (result.minimum_distance.distance <= 0) { + // TODO: this should be a comment in the solution + ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" + << result.minimum_distance.link_names[0] << "' and '" + << result.minimum_distance.link_names[1] << "'"); + return std::numeric_limits::infinity(); + } else { + // ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << + // result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); + return 1.0 / (result.minimum_distance.distance + 1e-5); + } +} } } } From a15204b40b799aead184bf5f687c4e57d163560b Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 29 Jun 2020 22:24:35 +0200 Subject: [PATCH 08/42] allow wrappers to use CostTerm --- core/include/moveit/task_constructor/container.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 8aa557a9..f1767f98 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -81,7 +81,7 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); -private: + /// most containers should only use setCostTransform and leave the costs to their children using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); @@ -227,6 +227,9 @@ public: bool canCompute() const override; void compute() override; + // Wrappers sometimes do the real work (e.g., IK), so they can specify costs + using Stage::setCostTerm; + protected: WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer()); }; From f9ba3027287ca5d503cfe715e08f13b284902d17 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 09:53:58 +0200 Subject: [PATCH 09/42] enable costs to (optionally) return a comment With more complicated costs (such as the ClearanceCost), it's useful to get comments. --- .../moveit/task_constructor/cost_terms.h | 2 +- core/include/moveit/task_constructor/stage.h | 5 +++- core/src/cost_terms.cpp | 18 +++++++++------ core/src/stage.cpp | 23 +++++++++++++++---- 4 files changed, 34 insertions(+), 14 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 821afd94..91c0ab8f 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -23,7 +23,7 @@ private: double PathLengthCost(const SubTrajectory& s); /// distance to self-collision -double ClearanceCost(const SubTrajectory& s); +double ClearanceCost(const SubTrajectory& s, std::string& comment); } } } diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5a229f9d..857f2012 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,7 +213,9 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; + using CostTerm = std::function; + using CostTermShort = std::function; + using CostTransform = std::function; /* \brief set method to determine costs for solutions of this stage @@ -227,6 +229,7 @@ public: * See setCostTransform */ void setCostTerm(const CostTerm& term); + void setCostTerm(const CostTermShort& term); /* \brief set CostTransform to be applied to costs of this stage's solution costs * diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 00f3754f..769060c9 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -42,6 +42,8 @@ #include +#include + namespace moveit { namespace task_constructor { namespace cost { @@ -50,7 +52,7 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s) { +double ClearanceCost(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; @@ -68,15 +70,17 @@ double ClearanceCost(const SubTrajectory& s) { s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + const auto& links = result.minimum_distance.link_names; + if (result.minimum_distance.distance <= 0) { - // TODO: this should be a comment in the solution - ROS_ERROR_STREAM_NAMED("ClearanceCost", "allegedly valid solution has an unwanted collide between '" - << result.minimum_distance.link_names[0] << "' and '" - << result.minimum_distance.link_names[1] << "'"); + boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'"); + desc % links[0] % links[1]; + comment = desc.str(); return std::numeric_limits::infinity(); } else { - // ROS_INFO_STREAM_NAMED("ClearanceCost", result.minimum_distance.link_names[0] << " has distance " << - // result.minimum_distance.distance << " to " << result.minimum_distance.link_names[1]); + boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'"); + desc % result.minimum_distance.distance % links[0] % links[1]; + comment = desc.str(); return 1.0 / (result.minimum_distance.distance + 1e-5); } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 37396f8f..f5ced345 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -225,13 +225,22 @@ bool StagePrivate::addCost(SolutionBase& solution) { double cost{ 0.0 }; auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory && cost_term_) - // CostTerm only applies to SubTrajectory - cost = cost_term_(*trajectory); - else - // everything else is just transformed + if (trajectory && cost_term_) { + // compute specific cost + std::string comment; + cost = cost_term_(*trajectory, 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); + } + } else { + // otherwise forward current cost cost = solution.cost(); + } + // all costs are transformed solution.setCost(cost_transform_(cost)); return !solution.isFailure(); @@ -337,6 +346,10 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } +void Stage::setCostTerm(const CostTermShort& term) { + setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); +} + void Stage::setCostTransform(const CostTransform& transform) { pimpl()->cost_transform_ = transform; } From cf0e4e3b017daafd8a3c8a944438b5e43566f12e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:10:33 +0200 Subject: [PATCH 10/42] establish struct cost pattern for costs with multiple parameters, this option-style is more useful than verbose getter/setter. There is nothing to hide here. --- core/include/moveit/task_constructor/cost_terms.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 91c0ab8f..2c2d7af4 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -8,15 +8,14 @@ namespace cost { /// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// add a constant cost to each solution -class ConstantCost +struct ConstantCost { public: - ConstantCost(double cost) : cost_(cost) {} + ConstantCost(double c) : cost(c) {} - double operator()(const SubTrajectory&) { return cost_; } + double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } -private: - double cost_; + double cost; }; /// execution duration of the whole trajectory From 18ed51d7f3e26dd092e665039f3319eec1bf20b3 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:13:58 +0200 Subject: [PATCH 11/42] turn Clearance cost into parameterized struct There is a lot of variations for how to compute clearance. --- .../moveit/task_constructor/cost_terms.h | 20 ++++++++++++++-- core/src/cost_terms.cpp | 24 +++++++++++-------- 2 files changed, 32 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 2c2d7af4..b8ac02c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -21,8 +21,24 @@ public: /// execution duration of the whole trajectory double PathLengthCost(const SubTrajectory& s); -/// distance to self-collision -double ClearanceCost(const SubTrajectory& s, std::string& comment); +/** inverse distance to collision + * + * \arg interface compute distances using START or END interface of solution + * \arg with_world check distances to world objects as well or only look at self-collisions + * \arg group_property the name of the property which defines the group to look at + * */ +struct ClearanceCost +{ + ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, + std::string group_property = "group") + : interface(interface), with_world(with_world), group_property(group_property) {} + + double operator()(const SubTrajectory& s, std::string& comment); + + Interface::Direction interface; + bool with_world; + std::string group_property; +}; } } } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 769060c9..2b3e6b92 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -52,23 +52,27 @@ double PathLengthCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double ClearanceCost(const SubTrajectory& s, std::string& comment) { +double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = collision_detection::DistanceRequestType::GLOBAL; - // TODO: possibly parameterize hardcoded property name? - const std::string group{ "group" }; - auto& state_properties{ s.start()->properties() }; - auto& stage_properties{ s.creator()->properties() }; - request.group_name = state_properties.hasProperty(group) ? state_properties.get(group) : - stage_properties.get(group); + const auto& state = (interface == Interface::START) ? s.start() : s.end(); - request.enableGroup(s.start()->scene()->getRobotModel()); - request.acm = &s.start()->scene()->getAllowedCollisionMatrix(); + // 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(); collision_detection::DistanceResult result; - s.start()->scene()->getCollisionEnv()->distanceSelf(request, result, s.start()->scene()->getCurrentState()); + state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); const auto& links = result.minimum_distance.link_names; From a92753dafcf9af8e0bcaa0a915edcee31b7ac629 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 11:31:17 +0200 Subject: [PATCH 12/42] add cumulative distance to ClearanceCost --- .../moveit/task_constructor/cost_terms.h | 4 ++- core/src/cost_terms.cpp | 25 +++++++++++++++---- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index b8ac02c5..c32c192c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -25,11 +25,12 @@ double PathLengthCost(const SubTrajectory& s); * * \arg interface compute distances using START or END interface of solution * \arg with_world check distances to world objects as well or only 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 * */ struct ClearanceCost { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, + ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} @@ -37,6 +38,7 @@ struct ClearanceCost Interface::Direction interface; bool with_world; + bool cumulative; std::string group_property; }; } diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 2b3e6b92..e7edd2e2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -54,7 +54,8 @@ double PathLengthCost(const SubTrajectory& s) { double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; - request.type = collision_detection::DistanceRequestType::GLOBAL; + request.type = + cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; const auto& state = (interface == Interface::START) ? s.start() : s.end(); @@ -74,6 +75,16 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); + double distance{ 0.0 }; + if (cumulative) { + for (const auto& distance_of_pair : result.distances) { + assert(distance_of_pair.second.size() == 1); + distance += distance_of_pair.second[0].distance; + } + } else { + distance = result.minimum_distance.distance; + } + const auto& links = result.minimum_distance.link_names; if (result.minimum_distance.distance <= 0) { @@ -82,10 +93,14 @@ double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { comment = desc.str(); return std::numeric_limits::infinity(); } else { - boost::format desc("ClearCost: distance %1% between'%2%' and '%3%'"); - desc % result.minimum_distance.distance % links[0] % links[1]; - comment = desc.str(); - return 1.0 / (result.minimum_distance.distance + 1e-5); + if (cumulative) { + comment = "ClearCost: cumulative distance " + std::to_string(distance); + } else { + boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'"); + desc % result.minimum_distance.distance % links[0] % links[1]; + comment = desc.str(); + } + return 1.0 / (distance + 1e-5); } } } From 6dbc742f2071cff78bf1a054f05bf5edd41b5b53 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 17:01:24 +0200 Subject: [PATCH 13/42] implement correct PathLengthCost The previous implementation depends on the dynamics limits of the robot, which might be interesting in some cases, but shouldn't be a default anywhere. --- core/include/moveit/task_constructor/cost_terms.h | 5 ++++- core/src/cost_terms.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index c32c192c..5a6bc8ed 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -18,9 +18,12 @@ public: double cost; }; -/// execution duration of the whole trajectory +/// trajectory length (interpolated between waypoints) double PathLengthCost(const SubTrajectory& s); +/// execution duration of the whole trajectory +double TrajectoryDurationCost(const SubTrajectory& s); + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index e7edd2e2..49a16d1f 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -49,6 +49,18 @@ namespace task_constructor { namespace cost { double PathLengthCost(const SubTrajectory& s) { + 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 TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } From 9a96a7e4f66d7691f4a5af3480302583dae0839e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 20:39:56 +0200 Subject: [PATCH 14/42] implement LinkMotionCost --- .../moveit/task_constructor/cost_terms.h | 9 ++++++++ core/src/cost_terms.cpp | 23 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 5a6bc8ed..d16b6722 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -24,6 +24,15 @@ double PathLengthCost(const SubTrajectory& s); /// execution duration of the whole trajectory double TrajectoryDurationCost(const SubTrajectory& s); +struct LinkMotionCost +{ + LinkMotionCost(std::string link_name) : link_name(link_name) {} + + double operator()(const SubTrajectory&, std::string&); + + std::string link_name; +}; + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 49a16d1f..bbdad413 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,6 +64,29 @@ double TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } +double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { + 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::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { + Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position.vector() - position.vector()).norm(); + position = new_position; + } + return distance; +} + double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = From 14c885621f9d6e54cf1e4a762442974cae02ec7a Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 21:02:53 +0200 Subject: [PATCH 15/42] remove Cost suffix for elements in mtc::cost:: --- .../moveit/task_constructor/cost_terms.h | 20 +++++++++---------- core/src/cost_terms.cpp | 8 ++++---- core/src/stages/connect.cpp | 2 +- core/src/stages/fix_collision_objects.cpp | 2 +- core/src/stages/fixed_cartesian_poses.cpp | 2 +- core/src/stages/fixed_state.cpp | 2 +- core/src/stages/generate_pose.cpp | 2 +- core/src/stages/modify_planning_scene.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- 10 files changed, 22 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index d16b6722..8f551664 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -4,14 +4,14 @@ namespace moveit { namespace task_constructor { +/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() namespace cost { -/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() /// add a constant cost to each solution -struct ConstantCost +struct Constant { public: - ConstantCost(double c) : cost(c) {} + Constant(double c) : cost(c) {} double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } @@ -19,14 +19,14 @@ public: }; /// trajectory length (interpolated between waypoints) -double PathLengthCost(const SubTrajectory& s); +double PathLength(const SubTrajectory& s); /// execution duration of the whole trajectory -double TrajectoryDurationCost(const SubTrajectory& s); +double TrajectoryDuration(const SubTrajectory& s); -struct LinkMotionCost +struct LinkMotion { - LinkMotionCost(std::string link_name) : link_name(link_name) {} + LinkMotion(std::string link_name) : link_name(link_name) {} double operator()(const SubTrajectory&, std::string&); @@ -40,10 +40,10 @@ struct LinkMotionCost * \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 * */ -struct ClearanceCost +struct Clearance { - ClearanceCost(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, - std::string group_property = "group") + Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, + std::string group_property = "group") : interface(interface), with_world(with_world), group_property(group_property) {} double operator()(const SubTrajectory& s, std::string& comment); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index bbdad413..5dc139d3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -48,7 +48,7 @@ namespace moveit { namespace task_constructor { namespace cost { -double PathLengthCost(const SubTrajectory& s) { +double PathLength(const SubTrajectory& s) { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -60,11 +60,11 @@ double PathLengthCost(const SubTrajectory& s) { return path_length; } -double TrajectoryDurationCost(const SubTrajectory& s) { +double TrajectoryDuration(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) return distance; } -double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 8d38da55..cb66a78d 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 0d0f9cc8..05b820e8 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -56,7 +56,7 @@ namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index a78c2414..d6f0d96a 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -49,7 +49,7 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(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 d76d37ac..971b6da6 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name) : Generator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); } void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index 85efbc44..ebdb73ba 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -46,7 +46,7 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(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 36a45753..8343151e 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -47,7 +47,7 @@ namespace task_constructor { namespace stages { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { - setCostTerm(cost::ConstantCost(0.0)); + setCostTerm(cost::Constant(0.0)); } void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index da398dcd..218cd721 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 21db9815..c151f580 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLengthCost); + setCostTerm(cost::PathLength); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); From 7a00ac646e9ce8bda640e96f611b3241c18d88ce Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 15:39:57 +0200 Subject: [PATCH 16/42] add missing license --- .../moveit/task_constructor/cost_terms.h | 38 +++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 8f551664..2b46813d 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -1,3 +1,41 @@ +/********************************************************************* + * 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 From d2bd0a00377e58ff55cea338eb193b0b70dd8893 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 16:07:43 +0200 Subject: [PATCH 17/42] generalize Clearance cost - can now be used to estimate cost for either interface state or the trajectory - Introduced Interface::Direction NONE as a way of pointing to the trajectory in contrast to START or END --- .../moveit/task_constructor/cost_terms.h | 13 +-- .../include/moveit/task_constructor/storage.h | 1 + core/src/cost_terms.cpp | 85 +++++++++++++------ 3 files changed, 68 insertions(+), 31 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 2b46813d..26140060 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -73,23 +73,24 @@ struct LinkMotion /** inverse distance to collision * - * \arg interface compute distances using START or END interface of solution - * \arg with_world check distances to world objects as well or only look at self-collisions + * \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 * */ struct Clearance { - Clearance(Interface::Direction interface = Interface::START, bool with_world = true, bool cumulative = false, - std::string group_property = "group") - : interface(interface), with_world(with_world), group_property(group_property) {} + Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", + Interface::Direction interface = Interface::NONE) + : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} double operator()(const SubTrajectory& s, std::string& comment); - Interface::Direction interface; bool with_world; bool cumulative; std::string group_property; + Interface::Direction interface; }; } } diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 3bf0e08f..581602ca 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -163,6 +163,7 @@ public: enum Direction { + NONE = 0, FORWARD, BACKWARD, START = FORWARD, diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 5dc139d3..12cf4d55 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -88,11 +88,13 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { } double Clearance::operator()(const SubTrajectory& s, std::string& comment) { + const std::string PREFIX{ "Clearance: " }; + collision_detection::DistanceRequest request; request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; - const auto& state = (interface == Interface::START) ? s.start() : s.end(); + const auto& state{ (interface == Interface::END) ? 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?). @@ -106,37 +108,70 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) { request.enableGroup(state->scene()->getRobotModel()); request.acm = &state->scene()->getAllowedCollisionMatrix(); - collision_detection::DistanceResult result; + // 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) + state->scene()->getCollisionEnv()->distanceRobot(request, result, robot); + else + state->scene()->getCollisionEnv()->distanceSelf(request, result, robot); - state->scene()->getCollisionEnv()->distanceSelf(request, result, state->scene()->getCurrentState()); + 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 (cumulative) { - for (const auto& distance_of_pair : result.distances) { - assert(distance_of_pair.second.size() == 1); - distance += distance_of_pair.second[0].distance; + + if (interface == Interface::START || interface == Interface::END || + (interface == Interface::NONE && 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(); } - } else { - distance = result.minimum_distance.distance; - } - - const auto& links = result.minimum_distance.link_names; - - if (result.minimum_distance.distance <= 0) { - boost::format desc("ClearCost: allegedly valid solution has an unwanted collide between '%1%' and '%2%'"); - desc % links[0] % links[1]; - comment = desc.str(); - return std::numeric_limits::infinity(); - } else { - if (cumulative) { - comment = "ClearCost: cumulative distance " + std::to_string(distance); - } else { - boost::format desc("ClearCost: distance %1% between '%2%' and '%3%'"); - desc % result.minimum_distance.distance % links[0] % links[1]; + 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); } - return 1.0 / (distance + 1e-5); + } 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 1.0 / (distance + 1e-5); } } } From 1f8feaaf0a551b95aaef57b041e8d5d5d158e325 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 16:14:51 +0200 Subject: [PATCH 18/42] refactor addCost->computeCost --- core/include/moveit/task_constructor/stage_p.h | 2 +- core/src/stage.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index ad0f1bfa..208639d0 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -158,7 +158,7 @@ public: * * @return true if solution remains feasible (is no failure) */ - bool addCost(SolutionBase& solution); + bool computeCost(SolutionBase& solution); protected: // associated/owning Stage instance diff --git a/core/src/stage.cpp b/core/src/stage.cpp index f5ced345..8c9265e6 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -156,7 +156,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setStartState(from); solution->setEndState(*to_it); - if (!solution->isFailure() && addCost(*solution)) { + if (!solution->isFailure() && computeCost(*solution)) { nextStarts()->add(*to_it); } @@ -174,7 +174,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setStartState(*from_it); solution->setEndState(to); - if (!solution->isFailure() && addCost(*solution)) + if (!solution->isFailure() && computeCost(*solution)) prevEnds()->add(*from_it); newSolution(solution); @@ -191,7 +191,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setStartState(*from); solution->setEndState(*to); - if (!solution->isFailure() && addCost(*solution)) { + if (!solution->isFailure() && computeCost(*solution)) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -207,7 +207,7 @@ void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, solution->setEndState(to); if (!solution->isFailure()) - addCost(*solution); + computeCost(*solution); newSolution(solution); } @@ -221,7 +221,7 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } -bool StagePrivate::addCost(SolutionBase& solution) { +bool StagePrivate::computeCost(SolutionBase& solution) { double cost{ 0.0 }; auto* trajectory{ dynamic_cast(&solution) }; From bbf7d415f6627e747b2a0ab7cba88e34284304bc Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 16 Jul 2020 17:18:14 +0200 Subject: [PATCH 19/42] make cost operators const --- core/include/moveit/task_constructor/cost_terms.h | 4 ++-- core/src/cost_terms.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 26140060..d0ad411e 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -66,7 +66,7 @@ struct LinkMotion { LinkMotion(std::string link_name) : link_name(link_name) {} - double operator()(const SubTrajectory&, std::string&); + double operator()(const SubTrajectory&, std::string&) const; std::string link_name; }; @@ -85,7 +85,7 @@ struct Clearance Interface::Direction interface = Interface::NONE) : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} - double operator()(const SubTrajectory& s, std::string& comment); + double operator()(const SubTrajectory& s, std::string& comment) const; bool with_world; bool cumulative; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 12cf4d55..69ce13f2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,7 +64,7 @@ double TrajectoryDuration(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } -double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +87,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) { return distance; } -double Clearance::operator()(const SubTrajectory& s, std::string& comment) { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; From 53c0964618c9920faea0f931fe771fa80bd02b8f Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 28 Jul 2020 18:27:02 +0200 Subject: [PATCH 20/42] cost computation: provide valid interface states for Solution The CostTerm's should get only a single solution that is well-setup with its InterfaceStates. That's impossible though because these states are stored in different places depending on the cost. To avoid this, set stub states for cost computation and change them to the real states later on. This is motivated by the Clearance cost which can act on an InterfaceState only. --- .../include/moveit/task_constructor/storage.h | 29 ++++++-- core/src/stage.cpp | 74 +++++++++++++++---- core/src/storage.cpp | 10 +++ 3 files changed, 91 insertions(+), 22 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 581602ca..e706258a 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -197,6 +197,7 @@ class StagePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase { + friend StagePrivate; // for set[Start|End]StateUnsafe public: virtual ~SolutionBase() = default; @@ -207,18 +208,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_; } @@ -247,6 +252,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_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 8c9265e6..89e0241b 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -130,7 +130,6 @@ void StagePrivate::validateConnectivity() const { } bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { - solution->setCreator(me()); if (introspection_) introspection_->registerSolution(*solution); @@ -147,34 +146,59 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) { void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) { assert(nextStarts()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + // chicken-and-egg problem: we don't know whether/where we will store the solution, + // 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); + + computeCost(*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() && computeCost(*solution)) { + if (!solution->isFailure()) nextStarts()->add(*to_it); - } newSolution(solution); } void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) { assert(prevEnds()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*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() && computeCost(*solution)) + if (!solution->isFailure()) prevEnds()->add(*from_it); newSolution(solution); @@ -182,16 +206,27 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) { assert(prevEnds() && nextStarts()); + + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ state }, tmp_to{ state }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*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() && computeCost(*solution)) { + if (!solution->isFailure()) { prevEnds()->add(*from); nextStarts()->add(*to); } @@ -200,14 +235,21 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution } void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { + solution->setCreator(me()); + + if (!solution->isFailure()) { + InterfaceState tmp_from{ from }, tmp_to{ to }; + solution->setStartState(tmp_from); + solution->setEndState(tmp_to); + + computeCost(*solution); + } + if (!storeSolution(solution)) return; // solution dropped - solution->setStartState(from); - solution->setEndState(to); - - if (!solution->isFailure()) - computeCost(*solution); + solution->setStartStateUnsafe(from); + solution->setEndStateUnsafe(to); newSolution(solution); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ed53b4ec..330231eb 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; } From e53d943b758547714cae3ceb36ccb1d8df29a832 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:23:08 +0200 Subject: [PATCH 21/42] remove CostTransform If required, this can hopefully be implemented through Wrapper stages soon. --- core/include/moveit/task_constructor/stage.h | 11 ----------- core/include/moveit/task_constructor/stage_p.h | 1 - core/src/stage.cpp | 8 +------- 3 files changed, 1 insertion(+), 19 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 857f2012..9c0d7461 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -215,9 +215,6 @@ public: using CostTerm = std::function; using CostTermShort = std::function; - - using CostTransform = std::function; - /* \brief set method to determine costs for solutions of this stage * * This interface supports only primitive solutions, no containers. @@ -231,14 +228,6 @@ public: void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); - /* \brief set CostTransform to be applied to costs of this stage's solution costs - * - * This interface can be used to modify the cost associated with each solution of this stage/container. - * The most common case would be linear weighting `[](double cost){ return constant*cost; }` - * but any non-linear transform is obviously possible as well. - */ - void setCostTransform(const CostTransform& transform); - const ordered& solutions() const; const std::list& failures() const; size_t numFailures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 208639d0..221034e9 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -173,7 +173,6 @@ protected: // user-configurable cost estimator Stage::CostTerm cost_term_; - Stage::CostTransform cost_transform_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 89e0241b..4b5247bc 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -99,7 +99,6 @@ std::ostream& operator<<(std::ostream& os, const InitStageException& e) { StagePrivate::StagePrivate(Stage* me, const std::string& name) : me_(me) , name_(name) - , cost_transform_([](auto x) { return x; }) , total_compute_time_{} , parent_(nullptr) , introspection_(nullptr) {} @@ -282,8 +281,7 @@ bool StagePrivate::computeCost(SolutionBase& solution) { cost = solution.cost(); } - // all costs are transformed - solution.setCost(cost_transform_(cost)); + solution.setCost(cost); return !solution.isFailure(); } @@ -392,10 +390,6 @@ void Stage::setCostTerm(const CostTermShort& term) { setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); } -void Stage::setCostTransform(const CostTransform& transform) { - pimpl()->cost_transform_ = transform; -} - const ordered& Stage::solutions() const { return pimpl()->solutions_; } From 039b0a2c27ec24931bee73d5b92435d79a9522f8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:25:10 +0200 Subject: [PATCH 22/42] move more overhead to computeCost --- .../include/moveit/task_constructor/stage_p.h | 7 +-- core/src/stage.cpp | 61 ++++++------------- 2 files changed, 21 insertions(+), 47 deletions(-) diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 221034e9..5ada0542 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -154,11 +154,8 @@ public: total_compute_time_ += compute_stop_time - compute_start_time; } - /** compute cost for solution through configured CostTerm - * - * @return true if solution remains feasible (is no failure) - */ - bool computeCost(SolutionBase& solution); + /** compute cost for solution through configured CostTerm */ + void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution); protected: // associated/owning Stage instance diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 4b5247bc..767c4518 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -148,19 +148,11 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, solution->setCreator(me()); - if (!solution->isFailure()) { - // chicken-and-egg problem: we don't know whether/where we will store the solution, - // 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); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); @@ -180,16 +172,11 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ from }, tmp_to{ to }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); @@ -208,13 +195,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ state }, tmp_to{ state }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(state, state, *solution); if (!storeSolution(solution)) return; // solution dropped @@ -236,13 +217,7 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) { solution->setCreator(me()); - if (!solution->isFailure()) { - InterfaceState tmp_from{ from }, tmp_to{ to }; - solution->setStartState(tmp_from); - solution->setEndState(tmp_to); - - computeCost(*solution); - } + computeCost(from, to, *solution); if (!storeSolution(solution)) return; // solution dropped @@ -262,28 +237,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { parent()->onNewSolution(*solution); } -bool StagePrivate::computeCost(SolutionBase& solution) { - double cost{ 0.0 }; +void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { + // no reason to compute costs for a failed solution + if (solution.isFailure() || !cost_term_) + 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); auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory && cost_term_) { + if (trajectory) { // compute specific cost std::string comment; - cost = cost_term_(*trajectory, comment); + solution.setCost(cost_term_(*trajectory, 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); } - } else { - // otherwise forward current cost - cost = solution.cost(); } - - solution.setCost(cost); - - return !solution.isFailure(); } Stage::Stage(StagePrivate* impl) : pimpl_(impl) { From d68ab383b63817fcb8240ae880b7e203a798e5ae Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 17:38:03 +0200 Subject: [PATCH 23/42] allow setCostTerm for containers implement visitor pattern for cost computation on solutions - compute cost terms for solution subtrees instead of only for SubTrajectory - allows users to set cost terms for containers --- .../moveit/task_constructor/container.h | 6 ----- core/include/moveit/task_constructor/stage.h | 9 +------ .../include/moveit/task_constructor/stage_p.h | 2 +- .../include/moveit/task_constructor/storage.h | 17 +++++++++++++ core/src/stage.cpp | 24 ++++++------------ core/src/storage.cpp | 25 +++++++++++++++++++ 6 files changed, 52 insertions(+), 31 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index f1767f98..27b2fde0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -80,9 +80,6 @@ public: protected: ContainerBase(ContainerBasePrivate* impl); - - /// most containers should only use setCostTransform and leave the costs to their children - using Stage::setCostTerm; }; std::ostream& operator<<(std::ostream& os, const ContainerBase& stage); @@ -227,9 +224,6 @@ public: bool canCompute() const override; void compute() override; - // Wrappers sometimes do the real work (e.g., IK), so they can specify costs - using Stage::setCostTerm; - protected: WrapperBase(WrapperBasePrivate* impl, Stage::pointer&& child = Stage::pointer()); }; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 9c0d7461..9a40cf69 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -213,17 +213,10 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTerm = std::function; using CostTermShort = std::function; /* \brief set method to determine costs for solutions of this stage * - * This interface supports only primitive solutions, no containers. - * - * In order to support containers, the associated CostTerms would need to be able - * to decide costs for arbitrary partial solutions of the container. - * These need to be determined for scheduling, but would make the terms too complex. - * - * See setCostTransform + * For composite stages, costs are computed for each SubTrajectory and aggregated */ void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 5ada0542..0d094108 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -169,7 +169,7 @@ protected: InterfacePtr ends_; // user-configurable cost estimator - Stage::CostTerm cost_term_; + CostTerm cost_term_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index e706258a..f8e5e669 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -193,6 +193,13 @@ private: using base_type::remove_if; }; +class SubTrajectory; +/** Interface for cost terms to be configured via Stage::setCostTerm() + * + * Look at moveit/task_constructor/cost_terms.h for various implementations + */ +using CostTerm = std::function; + class StagePrivate; /// abstract base class for solutions (primitive and sequences) class SolutionBase @@ -245,6 +252,9 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; + /// compute cost of this solution according to the specified CostTerm + 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_; } @@ -294,6 +304,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_; @@ -319,6 +331,9 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + /// aggregate costs along the sequence + 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(); } @@ -348,6 +363,8 @@ public: : WrappedSolution(creator, wrapped, wrapped->cost()) {} void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; + /// compute cost of wrapped child + double computeCost(const CostTerm& cost, std::string& comment) const override; const SolutionBase* wrapped() const { return wrapped_; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 767c4518..b4e6e051 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -97,11 +97,7 @@ 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), total_compute_time_{}, parent_(nullptr), introspection_(nullptr) {} InterfaceFlags StagePrivate::interfaceFlags() const { InterfaceFlags f; @@ -249,17 +245,13 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& solution.setStartState(tmp_from); solution.setEndState(tmp_to); - auto* trajectory{ dynamic_cast(&solution) }; - if (trajectory) { - // compute specific cost - std::string comment; - solution.setCost(cost_term_(*trajectory, 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); - } + std::string comment; + 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); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 330231eb..ff096a12 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -178,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); } @@ -217,6 +221,22 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } } +double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { + double cost{ 0.0 }; + std::string subcomment; + for (auto& solution : subsolutions_) { + cost += solution->computeCost(f, subcomment); + if (!subcomment.empty()) { + if (!comment.empty()) + comment.append(", "); + comment.append(subcomment); + subcomment.clear(); + } + } + + return cost; +} + void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection) const { wrapped_->fillMessage(solution, introspection); @@ -227,5 +247,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 wrapped_->computeCost(f, comment); +} + } // namespace task_constructor } // namespace moveit From a0975e3afc4c7c805cb390bc2adfb4d16137dd3e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 21:13:51 +0200 Subject: [PATCH 24/42] avoid ambiguity of setCostTerm(nullptr) --- core/include/moveit/task_constructor/stage.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 9a40cf69..510d6070 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -220,6 +220,8 @@ public: */ void setCostTerm(const CostTerm& term); void setCostTerm(const CostTermShort& term); + // avoid overloading ambiguity for resetting the cost term + void setCostTerm(const std::nullptr_t) { setCostTerm(static_cast(nullptr)); } const ordered& solutions() const; const std::list& failures() const; From 939f0b7256e9d62779dc223629c0a5f6824580ea Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 18 Aug 2020 21:14:29 +0200 Subject: [PATCH 25/42] setCostTermShort(nullptr) must not set a valid function --- core/src/stage.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b4e6e051..a0c75b69 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -356,7 +356,10 @@ void Stage::setCostTerm(const CostTerm& term) { } void Stage::setCostTerm(const CostTermShort& term) { - setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); + if (term) + setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); + else + pimpl()->cost_term_ = nullptr; } const ordered& Stage::solutions() const { From 0e38730fd49fc993171876f9f5de943da49a135f Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 21:15:15 +0200 Subject: [PATCH 26/42] compute cost in liftSolution as well --- core/include/moveit/task_constructor/storage.h | 3 +++ core/src/container.cpp | 8 ++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index f8e5e669..58dcc0cb 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -201,10 +201,13 @@ class SubTrajectory; using CostTerm = std::function; 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; 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) From 1689f6cd95888823e397c4c28d5c8893a0e36580 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 22:41:09 +0200 Subject: [PATCH 27/42] introduce new trivial stage Forward This can be used together with a custom CostTerm to modify costs of a solution. --- .../moveit/task_constructor/stages/forward.h | 61 +++++++++++++++++ core/src/stages/CMakeLists.txt | 2 + core/src/stages/forward.cpp | 65 +++++++++++++++++++ 3 files changed, 128 insertions(+) create mode 100644 core/include/moveit/task_constructor/stages/forward.h create mode 100644 core/src/stages/forward.cpp diff --git a/core/include/moveit/task_constructor/stages/forward.h b/core/include/moveit/task_constructor/stages/forward.h new file mode 100644 index 00000000..6f54c35b --- /dev/null +++ b/core/include/moveit/task_constructor/stages/forward.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 Forward : public WrapperBase +{ +public: + Forward(const std::string& name = "Forward", Stage::pointer&& child = Stage::pointer()); + + void onNewSolution(const SolutionBase& s) override; +}; +} // namespace stages +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index c254762f..8becf93e 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h + ${PROJECT_INCLUDE}/stages/forward.h ${PROJECT_INCLUDE}/stages/generate_pose.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/generate_place_pose.h @@ -24,6 +25,7 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp + forward.cpp generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp diff --git a/core/src/stages/forward.cpp b/core/src/stages/forward.cpp new file mode 100644 index 00000000..2a650c71 --- /dev/null +++ b/core/src/stages/forward.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 { + +Forward::Forward(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} + +void Forward::onNewSolution(const SolutionBase& s) { + this->liftSolution(s); +} + +} // namespace stages +} // namespace task_constructor +} // namespace moveit From dcee7c408f33add8bb5fb9883ac0e410d4c739f8 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 19 Aug 2020 22:45:56 +0200 Subject: [PATCH 28/42] add basic tests for using cost terms --- core/test/CMakeLists.txt | 2 + core/test/test_cost_terms.cpp | 253 ++++++++++++++++++++++++++++++++++ 2 files changed, 255 insertions(+) create mode 100644 core/test/test_cost_terms.cpp 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..0598ca31 --- /dev/null +++ b/core/test/test_cost_terms.cpp @@ -0,0 +1,253 @@ +#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 }; + +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)); } + + bool canCompute() const override { return true; } + + void compute() override { + InterfaceState state(ps); + spawn(std::move(state), STAGE_COST); + } +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +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); + } +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class ForwardMockup : public PropagatingForward +{ + using PropagatingForward::PropagatingForward; + + void computeForward(const InterfaceState& from) override { + SubTrajectory solution; + solution.setCost(STAGE_COST); + InterfaceState to(from); + + sendForward(from, std::move(to), std::move(solution)); + }; +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +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)); + }; +}; +MOVEIT_CLASS_FORWARD(GeneratorMockup); + +class SerialContainerStandalone : public SerialContainer +{ + moveit::core::RobotModelConstPtr robot; + InterfacePtr dummy; + planning_scene::PlanningSceneConstPtr ps; + InterfaceStatePtr state_start, state_end; + +public: + SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) + : SerialContainer() + , robot(robot) + , dummy(std::make_shared()) + , ps(new planning_scene::PlanningScene(robot)) {} + + // reset and prepare for a compute step + void prepare() { + 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 }; + + pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); + pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); + SerialContainer::init(robot); + pimpl()->resolveInterface(flags); + + // feed interfaces as required for one computation + if (flags & READS_START) { + pimpl()->starts()->add(*state_start); + } + if (flags & READS_END) { + pimpl()->ends()->add(*state_end); + } + } + + void runCompute() { pimpl()->runCompute(); } + + void computeWithStage(StageUniquePtr&& stage) { + clear(); + add(std::move(stage)); + + prepare(); + runCompute(); + } + + void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) { + stage->setCostTerm(cost_term); + computeWithStage(std::move(stage)); + } +}; + +TEST(CostTerm, CostOverwrite) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + const moveit::core::RobotModelConstPtr robot{ getModel() }; + + SerialContainerStandalone container(robot); + + // by default return the cost the stage declared + container.computeWithStage(std::make_unique()); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + + // nullptr is the explicit default + container.computeWithStageCost(std::make_unique(), nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + + // custom cost overwrites the stage cost + cost::Constant constant_cost{ 1.0 }; + container.computeWithStageCost(std::make_unique(), constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); +} + +// test all primitive stage types +TEST(CostTerm, StageTypes) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + moveit::core::RobotModelPtr robot{ getModel() }; + + SerialContainerStandalone container(robot); + + const cost::Constant constant{ 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); + + container.computeWithStageCost(std::make_unique(), constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost); + + container.computeWithStageCost(std::make_unique(), constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost); +} + +// Forward uses cost from solution +TEST(CostTerm, ForwardUsesCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant_stage{ 84.0 }; + stage->setCostTerm(constant_stage); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + + container.computeWithStage(std::move(forward)); + + ASSERT_NE(nullptr, dynamic_cast(&(*forward_ptr->solutions().front()))); + + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_stage.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); +} + +// Forward can overwrite cost from solution +TEST(CostTerm, ForwardOverwritesCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant_stage{ 84.0 }; + stage->setCostTerm(constant_stage); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + + cost::Constant constant_forward{ 72.0 }; + forward->setCostTerm(constant_forward); + + container.computeWithStage(std::move(forward)); + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_forward.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); + + // Forward can modify cost from solution +} + +// Forward can modify cost from solution +TEST(CostTerm, ForwardCanModifyCost) { + moveit::core::RobotModelPtr robot{ getModel() }; + SerialContainerStandalone container(robot); + + auto stage{ std::make_unique() }; + cost::Constant constant{ 8.0 }; + stage->setCostTerm(constant); + auto forward{ std::make_unique("forward", std::move(stage)) }; + auto* forward_ptr{ forward.get() }; + forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + + container.computeWithStage(std::move(forward)); + auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); + EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); +} From 28b98e989c0e52c88a093fdb959f92efc88725cd Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 21 Aug 2020 20:36:21 +0200 Subject: [PATCH 29/42] improve cost term tests - add tests for nested containers - add descriptions to checks - refactor helper to support different containers --- core/test/test_cost_terms.cpp | 227 ++++++++++++++++++++++------------ 1 file changed, 150 insertions(+), 77 deletions(-) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0598ca31..0a3d24af 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -16,6 +16,8 @@ using namespace planning_scene; const double STAGE_COST{ 7.0 }; +const double TRAJECTORY_DURATION{ 9.0 }; + class GeneratorMockup : public Generator { PlanningScenePtr ps; @@ -30,7 +32,10 @@ public: pimpl()->setNextStarts(next); } - void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset(new PlanningScene(robot_model)); } + 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; } @@ -39,7 +44,6 @@ public: spawn(std::move(state), STAGE_COST); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class ConnectMockup : public Connecting { @@ -51,21 +55,32 @@ class ConnectMockup : public Connecting connect(from, to, solution); } }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); 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)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); class BackwardMockup : public PropagatingBackward { @@ -79,9 +94,9 @@ class BackwardMockup : public PropagatingBackward sendBackward(std::move(from), to, std::move(solution)); }; }; -MOVEIT_CLASS_FORWARD(GeneratorMockup); -class SerialContainerStandalone : public SerialContainer +template +class Standalone : public T { moveit::core::RobotModelConstPtr robot; InterfacePtr dummy; @@ -89,15 +104,13 @@ class SerialContainerStandalone : public SerialContainer InterfaceStatePtr state_start, state_end; public: - SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) - : SerialContainer() - , robot(robot) - , dummy(std::make_shared()) - , ps(new planning_scene::PlanningScene(robot)) {} + 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() { - reset(); + 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))); @@ -106,46 +119,55 @@ public: Stage* s{ this }; ContainerBase* b; - while ((b = dynamic_cast(s))) { + 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))) { + 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 }; - pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); - pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); - SerialContainer::init(robot); - pimpl()->resolveInterface(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) { - pimpl()->starts()->add(*state_start); + impl->starts()->add(*state_start); } if (flags & READS_END) { - pimpl()->ends()->add(*state_end); + impl->ends()->add(*state_end); } } - void runCompute() { pimpl()->runCompute(); } + void runCompute() { this->pimpl()->runCompute(); } - void computeWithStage(StageUniquePtr&& stage) { - clear(); - add(std::move(stage)); + 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(StageUniquePtr&& stage, const CostTerm& cost_term) { - stage->setCostTerm(cost_term); - computeWithStage(std::move(stage)); + void computeWithStageCost(std::initializer_list stages, const CostTerm& 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 CostTerm& cost_term) { + this->setCostTerm(cost_term); + computeWithStages(stages); } }; @@ -153,28 +175,24 @@ TEST(CostTerm, CostOverwrite) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); const moveit::core::RobotModelConstPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); - // by default return the cost the stage declared - container.computeWithStage(std::make_unique()); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStages({ std::make_unique() }); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default"; - // nullptr is the explicit default - container.computeWithStageCost(std::make_unique(), nullptr); - EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST); + container.computeWithStageCost({ std::make_unique() }, nullptr); + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; - // custom cost overwrites the stage cost cost::Constant constant_cost{ 1.0 }; - container.computeWithStageCost(std::make_unique(), constant_cost); - EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); + container.computeWithStageCost({ std::make_unique() }, constant_cost); + EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost"; } -// test all primitive stage types TEST(CostTerm, StageTypes) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); const cost::Constant constant{ 1.0 }; @@ -182,72 +200,127 @@ TEST(CostTerm, StageTypes) { // 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); + 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); + 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); + container.computeWithStageCost({ std::make_unique() }, constant); + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; } -// Forward uses cost from solution TEST(CostTerm, ForwardUsesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - container.computeWithStage(std::move(forward)); + container.computeWithStages({ std::move(stage) }); - ASSERT_NE(nullptr, dynamic_cast(&(*forward_ptr->solutions().front()))); - - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost"; } -// Forward can overwrite cost from solution TEST(CostTerm, ForwardOverwritesCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; cost::Constant constant_forward{ 72.0 }; - forward->setCostTerm(constant_forward); + container.setCostTerm(constant_forward); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_forward.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); - - // Forward can modify cost from solution + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; } -// Forward can modify cost from solution TEST(CostTerm, ForwardCanModifyCost) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - SerialContainerStandalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - auto forward{ std::make_unique("forward", std::move(stage)) }; - auto* forward_ptr{ forward.get() }; - forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); - container.computeWithStage(std::move(forward)); - auto& wrapped{ dynamic_cast(*forward_ptr->solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); - EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); + container.computeWithStages({ std::move(stage) }); + auto& wrapped{ dynamic_cast(*container.solutions().front()) }; + EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward 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() }; + cost::Constant constant{ 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() }; + cost::Constant constant1{ 1.0 }; + s1->setCostTerm(constant1); + s2->setCostTerm(constant1); + c1->add(std::move(s1)); + c1->add(std::move(s2)); + + auto s3{ std::make_unique() }; + cost::Constant constant2{ 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(cost::TrajectoryDuration); + 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"; +} } From 8ae1060b4df13ec4958daa4587669788219eb93b Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 21 Aug 2020 20:39:48 +0200 Subject: [PATCH 30/42] add CostAggregator for SerialContainer It can be useful to change the default addition to other operators. The simplest example is applying a Constant cost term to a container. As the tests show, the visitor-based cost computation ends up adding cost::Constant *for each subtrajectory*. --- .../moveit/task_constructor/container.h | 6 +++ .../moveit/task_constructor/container_p.h | 2 + .../include/moveit/task_constructor/storage.h | 10 +++-- core/src/container.cpp | 22 ++++++--- core/src/storage.cpp | 12 ++++- core/test/test_cost_terms.cpp | 45 +++++++++++++++++++ 6 files changed, 87 insertions(+), 10 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 27b2fde0..8ca032e7 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,6 +94,12 @@ public: bool canCompute() const override; void compute() override; + /** Specify how to aggregate costs of childrens' solutions + * + * By default costs are added up + */ + void setCostAggregator(const SolutionSequence::CostAggregator& agg); + protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index 35a903ad..cad54fcd 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,6 +184,8 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; + + SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 58dcc0cb..20d646ec 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -325,15 +325,18 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence() : SolutionBase() {} - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) - : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} + explicit SolutionSequence(); + + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; + using CostAggregator = std::function; + void setCostAggregator(const CostAggregator&); + /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -343,6 +346,7 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: + CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 18892591..1c1b0993 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,8 +360,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), - in.second + current.cost() + out.second); + InterfaceState::Priority prio( + static_cast(in.first.size() + 1 + out.first.size()), + pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -374,7 +375,9 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); + auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; + sequence->setCostAggregator(pimpl()->aggregator_); + sorted.insert(std::move(sequence)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -393,7 +396,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name) {} + : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -518,6 +521,10 @@ void SerialContainerPrivate::validateConnectivity() const { } } +void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { + pimpl()->aggregator_ = agg; +} + bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -549,11 +556,14 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - trace_cost += successor->cost(); + + double previous_trace_cost { trace_cost }; + + trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); traverse(*successor, cb, trace, trace_cost); - trace_cost -= successor->cost(); + trace_cost = previous_trace_cost; trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ff096a12..1133257e 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,10 +182,19 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } +SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} + +SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) + : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} + void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } +void SolutionSequence::setCostAggregator(const CostAggregator& agg) { + aggregator_ = agg; +} + void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -225,7 +234,8 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - cost += solution->computeCost(f, subcomment); + auto subcost{ solution->computeCost(f, subcomment) }; + cost = aggregator_(cost, subcost); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 0a3d24af..ca6f382f 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,6 +11,8 @@ #include +#include + using namespace moveit::task_constructor; using namespace planning_scene; @@ -323,4 +325,47 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } + +TEST(CostTerm, CompositeSolutionsContainerAggregator) { + ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); + Standalone container{ getModel() }; + + cost::Constant constant{ 17.0 }; + + { + auto s1{ std::make_unique() }; + auto s2{ std::make_unique() }; + auto s3{ std::make_unique() }; + + container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); + EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; + } + + { + container.setCostAggregator([](double a, double b) { return std::max(a, b); }); + container.computeWithContainerCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works with explicit cost term in container"; + } + + { + container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); + container.computeWithStageCost( + { + std::make_unique(), + std::make_unique(), + std::make_unique(), + }, + constant); + + EXPECT_EQ(container.solutions().front()->cost(), constant.cost) + << "custom aggregator works without container cost term"; + } } From 034a1553154a6b059e8f19e47bc335605b67c2f7 Mon Sep 17 00:00:00 2001 From: v4hn Date: Fri, 28 Aug 2020 16:16:54 +0200 Subject: [PATCH 31/42] provide CostTerm class for all CostTerms This pattern allows cost::Constant to override the hierarchical cost computation for the SerialContainer and avoid traversing the graph. I implemented the CostTerm::supports() pattern over a full double visitor pattern with overloads for each SolutionBase specialization, because the SerialContainer needs to know whether cost aggregation of the subsolutions should take place or whether the SolutionSequence should be forwarded to the CostTerm. This would not be possible with a `virtual double operator()(const SolutionSequence&)` callback in CostTerm. Alternatively, implementing the hierarchical aggregation in the default implementation of this operator would be possible as well, but breaks intuition: - the corresponding methods to handle `SubTrajectory` and `WrappedSolution` *have to* default to not touching the solution's cost at all so it is inappropriate to have the default implementation for the Sequence do something else - The SerialContainer also aggregates costs outside the `computeCost()` interface (in multiple places in `onNewSolution()` to aggregate costs along partial paths) and thus moving the hierarchical aggregation to the CostTerm methods requires the aggregator to be shared between the Container and the CostTerm, The only shortcoming of the implemented approach, by contrast, is that user implementations that want to handle WrappedSolution or SolutionSequence differently have to ensure the supports_ flags are set correctly. Notice that most custom CostTerms will only ever access SubTrajectories and this case is simplified with the provided CostTerm constructors. --- .../moveit/task_constructor/cost_terms.h | 85 ++++++++++++++----- core/include/moveit/task_constructor/stage.h | 5 +- .../include/moveit/task_constructor/stage_p.h | 1 + .../include/moveit/task_constructor/storage.h | 8 +- core/src/cost_terms.cpp | 64 ++++++++++++-- core/src/stage.cpp | 7 -- core/src/stages/connect.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- core/test/test_cost_terms.cpp | 5 +- 10 files changed, 129 insertions(+), 52 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index d0ad411e..82c07920 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -39,36 +39,80 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { -/// These structures all implement the Stage::CostTerm API and can be configured via Stage::setCostTerm() + +class CostTerm +{ +public: + CostTerm(std::function); + CostTerm(std::function); + CostTerm(std::nullptr_t); + CostTerm(); + + // is this a valid CostTerm? + operator bool() const; + + double operator()(const SolutionBase& s, std::string& comment) const; + + /** describes the supported types of solutions that should be forwarded to this CostTerm + * + * CostTerms that support more than `SubTrajectory`s should inherit and overwrite the internal supports_ flag + */ + enum class SolutionType + { + NONE = 0, + TRAJECTORY = 1 << 0, + SEQUENCE = 1 << 1, + WRAPPER = 1 << 2, + ALL = TRAJECTORY | SEQUENCE | WRAPPER + }; + Flags supports() const; + +protected: + CostTerm(std::function, Flags); + + std::function term_; + Flags supports_; +}; + namespace cost { /// add a constant cost to each solution -struct Constant +class Constant : public CostTerm { public: - Constant(double c) : cost(c) {} - - double operator()(const SubTrajectory&, std::string& /* unused */) const { return cost; } + Constant(double c); double cost; }; /// trajectory length (interpolated between waypoints) -double PathLength(const SubTrajectory& s); +class PathLength : public CostTerm +{ +public: + PathLength(); + // TODO(v4hn): allow to consider specific joints only +}; /// execution duration of the whole trajectory -double TrajectoryDuration(const SubTrajectory& s); - -struct LinkMotion +class TrajectoryDuration : public CostTerm { - LinkMotion(std::string link_name) : link_name(link_name) {} +public: + TrajectoryDuration(); +}; - double operator()(const SubTrajectory&, std::string&) const; +class LinkMotion : public CostTerm +{ +public: + LinkMotion(std::string link_name); std::string link_name; + +protected: + double compute(const SubTrajectory&, std::string&) const; }; /** inverse distance to collision @@ -79,19 +123,20 @@ struct LinkMotion * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over * trajectory * */ -struct Clearance +class Clearance : public CostTerm { +public: Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", - Interface::Direction interface = Interface::NONE) - : with_world(with_world), cumulative(cumulative), group_property(group_property), interface(interface) {} - - double operator()(const SubTrajectory& s, std::string& comment) const; - + Interface::Direction interface = Interface::NONE); bool with_world; bool cumulative; std::string group_property; Interface::Direction interface; + +protected: + double compute(const SubTrajectory& s, std::string& comment) const; }; -} -} -} + +} // 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 510d6070..57eeb4de 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,6 +138,7 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); +class CostTerm; class ContainerBase; class StagePrivate; class Stage @@ -213,15 +214,11 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - using CostTermShort = std::function; /* \brief set method to determine costs for solutions of this stage * * For composite stages, costs are computed for each SubTrajectory and aggregated */ void setCostTerm(const CostTerm& term); - void setCostTerm(const CostTermShort& term); - // avoid overloading ambiguity for resetting the cost term - void setCostTerm(const std::nullptr_t) { setCostTerm(static_cast(nullptr)); } const ordered& solutions() const; const std::list& failures() const; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 0d094108..b9536b0c 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 diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 20d646ec..f68226cc 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -193,13 +193,7 @@ private: using base_type::remove_if; }; -class SubTrajectory; -/** Interface for cost terms to be configured via Stage::setCostTerm() - * - * Look at moveit/task_constructor/cost_terms.h for various implementations - */ -using CostTerm = std::function; - +class CostTerm; class StagePrivate; class ContainerBasePrivate; /// abstract base class for solutions (primitive and sequences) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 69ce13f2..15a43bbe 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -42,13 +42,45 @@ #include +#include + #include namespace moveit { namespace task_constructor { + +CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} + +CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} + +CostTerm::CostTerm(std::function t) + : term_{ [t](auto&& s, auto&& c) { return t(static_cast(s), c); } } + , supports_{ SolutionType::TRAJECTORY } {} + +CostTerm::CostTerm(std::function t) + : term_{ [t](auto&& s, auto&&) { return t(static_cast(s)); } } + , supports_{ SolutionType::TRAJECTORY } {} + +CostTerm::CostTerm(std::function term, Flags flags) + : term_{ term }, supports_{ flags } {} + +CostTerm::operator bool() const { + return supports_ != Flags{ SolutionType::NONE }; +} + +double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { + assert(bool{ term_ }); + return term_(s, comment); +} + namespace cost { -double PathLength(const SubTrajectory& s) { +Constant::Constant(double c) + : cost{ c }, CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL } {} + +namespace { + +double pathLength(const SubTrajectory& s) { const auto& traj = s.trajectory(); if (traj == nullptr) @@ -60,11 +92,18 @@ double PathLength(const SubTrajectory& s) { return path_length; } -double TrajectoryDuration(const SubTrajectory& s) { - return s.trajectory() ? s.trajectory()->getDuration() : 0.0; -} +} // namespace -double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { +PathLength::PathLength() : CostTerm{ pathLength } {} + +TrajectoryDuration::TrajectoryDuration() + : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} + +LinkMotion::LinkMotion(std::string link) + : CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } + , link_name{ std::move(link) } {} + +double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -87,7 +126,14 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons return distance; } -double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) + : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } + , with_world(with_world) + , cumulative(cumulative) + , group_property(group_property) + , interface(interface) {} + +double Clearance::compute(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; @@ -173,6 +219,6 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const return 1.0 / (distance + 1e-5); } -} -} -} +} // namespace cost +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index a0c75b69..d62eb2bf 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -355,13 +355,6 @@ void Stage::setCostTerm(const CostTerm& term) { pimpl()->cost_term_ = term; } -void Stage::setCostTerm(const CostTermShort& term) { - if (term) - setCostTerm([=](auto&& solution, auto&&) { return term(solution); }); - else - pimpl()->cost_term_ = nullptr; -} - const ordered& Stage::solutions() const { return pimpl()->solutions_; } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index cb66a78d..afa23fe3 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 218cd721..0c4db5d5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index c151f580..0c8385c9 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength); + setCostTerm(cost::PathLength{}); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index ca6f382f..9d09a868 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -254,7 +254,8 @@ TEST(CostTerm, ForwardCanModifyCost) { auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); + container.setCostTerm(static_cast>( + [](const SubTrajectory& s) { return s.cost() * s.cost(); })); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; @@ -319,7 +320,7 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { auto s3{ std::make_unique() }; - container.setCostTerm(cost::TrajectoryDuration); + container.setCostTerm(cost::TrajectoryDuration{}); container.computeWithStages({ std::move(c1), std::move(s3) }); EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION) << "container cost term overwrites stage costs"; From 05932ad5f16a40b9e6f2a3cf1968d15b7a58deb0 Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 1 Sep 2020 14:43:25 +0200 Subject: [PATCH 32/42] enable implicit construction of CostTerm from lambdas without the need for explicit casts to the correct signature. --- .../moveit/task_constructor/cost_terms.h | 19 ++++++++++-- core/src/cost_terms.cpp | 21 +++++++------- core/test/test_cost_terms.cpp | 29 +++++++++++++++++-- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 82c07920..f1dd680c 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -47,8 +47,15 @@ namespace task_constructor { class CostTerm { public: - CostTerm(std::function); - CostTerm(std::function); + using SubTrajectorySig = std::function; + using SubTrajectoryShortSig = std::function; + + // accept lambdas according to either signature above + template + CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {} + + CostTerm(const SubTrajectorySig&); + CostTerm(const SubTrajectoryShortSig&); CostTerm(std::nullptr_t); CostTerm(); @@ -72,10 +79,16 @@ public: Flags supports() const; protected: - CostTerm(std::function, Flags); + CostTerm(const std::function&, Flags); std::function term_; Flags supports_; + +private: + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{}); + template + auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{}); }; namespace cost { diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 15a43bbe..612ff7b3 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -1,4 +1,4 @@ -/********************************************************************* +/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2020, Hamburg University @@ -53,15 +53,15 @@ CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&& c) { return t(static_cast(s), c); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectorySig& term) + : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function t) - : term_{ [t](auto&& s, auto&&) { return t(static_cast(s)); } } - , supports_{ SolutionType::TRAJECTORY } {} +CostTerm::CostTerm(const SubTrajectoryShortSig& term) + : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); }, + SolutionType::TRAJECTORY } {} -CostTerm::CostTerm(std::function term, Flags flags) +CostTerm::CostTerm(const std::function& term, Flags flags) : term_{ term }, supports_{ flags } {} CostTerm::operator bool() const { @@ -76,7 +76,7 @@ double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { namespace cost { Constant::Constant(double c) - : cost{ c }, CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL } {} + : CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL }, cost{ c } {} namespace { @@ -100,8 +100,7 @@ TrajectoryDuration::TrajectoryDuration() : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} LinkMotion::LinkMotion(std::string link) - : CostTerm{ [this](const SubTrajectory& s, std::string& c) { return this->compute(s, c); } } - , link_name{ std::move(link) } {} + : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {} double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 9d09a868..80488f30 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -173,6 +173,32 @@ public: } }; +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() }; @@ -254,8 +280,7 @@ TEST(CostTerm, ForwardCanModifyCost) { auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; stage->setCostTerm(constant); - container.setCostTerm(static_cast>( - [](const SubTrajectory& s) { return s.cost() * s.cost(); })); + container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); }); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; From 64e2440c53d45379b07aae33aa1ee44fb7ed6635 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 15:42:25 +0200 Subject: [PATCH 33/42] simplify LinkMotion computation review feedback --- core/src/cost_terms.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 612ff7b3..d22ec5d4 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -116,10 +116,10 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { } double distance{ 0.0 }; - Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { - Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; - distance += (new_position.vector() - position.vector()).norm(); + const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position - position).norm(); position = new_position; } return distance; From cdc7cb04fc8dbfc004219b3431e4024907b1e87b Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 16:07:09 +0200 Subject: [PATCH 34/42] refactor Forward -> PassThrough based on review feedback --- .../stages/{forward.h => passthrough.h} | 4 +-- core/src/stages/CMakeLists.txt | 4 +-- .../stages/{forward.cpp => passthrough.cpp} | 6 ++--- core/test/test_cost_terms.cpp | 26 +++++++++---------- 4 files changed, 20 insertions(+), 20 deletions(-) rename core/include/moveit/task_constructor/stages/{forward.h => passthrough.h} (94%) rename core/src/stages/{forward.cpp => passthrough.cpp} (91%) diff --git a/core/include/moveit/task_constructor/stages/forward.h b/core/include/moveit/task_constructor/stages/passthrough.h similarity index 94% rename from core/include/moveit/task_constructor/stages/forward.h rename to core/include/moveit/task_constructor/stages/passthrough.h index 6f54c35b..61f1c40d 100644 --- a/core/include/moveit/task_constructor/stages/forward.h +++ b/core/include/moveit/task_constructor/stages/passthrough.h @@ -49,10 +49,10 @@ namespace stages { * Useful to set a custom CostTransform via Stage::setCostTerm * to change a solution's cost without loosing the original value */ -class Forward : public WrapperBase +class PassThrough : public WrapperBase { public: - Forward(const std::string& name = "Forward", Stage::pointer&& child = Stage::pointer()); + PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer()); void onNewSolution(const SolutionBase& s) override; }; diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 8becf93e..857a4977 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -5,11 +5,11 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/fixed_state.h ${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h - ${PROJECT_INCLUDE}/stages/forward.h ${PROJECT_INCLUDE}/stages/generate_pose.h ${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 @@ -25,11 +25,11 @@ add_library(${PROJECT_NAME}_stages current_state.cpp fixed_state.cpp fixed_cartesian_poses.cpp - forward.cpp generate_pose.cpp generate_grasp_pose.cpp generate_place_pose.cpp compute_ik.cpp + passthrough.cpp predicate_filter.cpp connect.cpp diff --git a/core/src/stages/forward.cpp b/core/src/stages/passthrough.cpp similarity index 91% rename from core/src/stages/forward.cpp rename to core/src/stages/passthrough.cpp index 2a650c71..786221e1 100644 --- a/core/src/stages/forward.cpp +++ b/core/src/stages/passthrough.cpp @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Michael 'v4hn' Goerner */ -#include +#include #include #include @@ -54,9 +54,9 @@ namespace moveit { namespace task_constructor { namespace stages { -Forward::Forward(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} +PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {} -void Forward::onNewSolution(const SolutionBase& s) { +void PassThrough::onNewSolution(const SolutionBase& s) { this->liftSolution(s); } diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 80488f30..c819c343 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include @@ -238,10 +238,10 @@ TEST(CostTerm, StageTypes) { EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator"; } -TEST(CostTerm, ForwardUsesCost) { +TEST(CostTerm, PassThroughUsesCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; @@ -250,32 +250,32 @@ TEST(CostTerm, ForwardUsesCost) { container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost"; + EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals PassThrough cost"; } -TEST(CostTerm, ForwardOverwritesCost) { +TEST(CostTerm, PassThroughOverwritesCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant_stage{ 84.0 }; stage->setCostTerm(constant_stage); - cost::Constant constant_forward{ 72.0 }; - container.setCostTerm(constant_forward); + cost::Constant constant_passthrough{ 72.0 }; + container.setCostTerm(constant_passthrough); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost"; + EXPECT_EQ(wrapped.cost(), constant_passthrough.cost) << "PassThrough can apply custom cost"; EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; } -TEST(CostTerm, ForwardCanModifyCost) { +TEST(CostTerm, PassThroughCanModifyCost) { ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); moveit::core::RobotModelPtr robot{ getModel() }; - Standalone container(robot); + Standalone container(robot); auto stage{ std::make_unique() }; cost::Constant constant{ 8.0 }; @@ -284,7 +284,7 @@ TEST(CostTerm, ForwardCanModifyCost) { container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child"; + 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"; } From f63d5b6892dc65a6dbb967e67503524689eeff16 Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 17:41:58 +0200 Subject: [PATCH 35/42] Make transform in cost::Clearance configurable In theory this can be done with a PassThrough with a modifying transform, but this specific mapping is intrinsic to the definition of Clearance as a cost. --- core/include/moveit/task_constructor/cost_terms.h | 2 ++ core/src/cost_terms.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index f1dd680c..f25a27c5 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -146,6 +146,8 @@ public: std::string group_property; Interface::Direction interface; + std::function distance_to_cost; + protected: double compute(const SubTrajectory& s, std::string& comment) const; }; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index d22ec5d4..72c99750 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -127,10 +127,11 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } - , with_world(with_world) - , cumulative(cumulative) - , group_property(group_property) - , interface(interface) {} + , with_world{ with_world } + , cumulative{ cumulative } + , group_property{ group_property } + , interface{ interface } + , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::compute(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; @@ -216,7 +217,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { comment = desc.str(); } - return 1.0 / (distance + 1e-5); + return distance_to_cost(distance); } } // namespace cost } // namespace task_constructor From cce7380f9def62c53956bce494a38e3c2a3b681c Mon Sep 17 00:00:00 2001 From: v4hn Date: Sat, 12 Sep 2020 20:23:24 +0200 Subject: [PATCH 36/42] Clearance: provide better interface for different modes Interface::Direction should always designate a valid direction. --- core/include/moveit/task_constructor/cost_terms.h | 13 +++++++++++-- core/include/moveit/task_constructor/storage.h | 1 - core/src/cost_terms.cpp | 10 +++++----- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index f25a27c5..6872a024 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -139,12 +139,21 @@ protected: class Clearance : public CostTerm { 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", - Interface::Direction interface = Interface::NONE); + Mode mode = Mode::AUTO); bool with_world; bool cumulative; std::string group_property; - Interface::Direction interface; + + Mode mode; std::function distance_to_cost; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index f68226cc..52138ae2 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -163,7 +163,6 @@ public: enum Direction { - NONE = 0, FORWARD, BACKWARD, START = FORWARD, diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 72c99750..0298a122 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -125,12 +125,12 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { return distance; } -Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Interface::Direction interface) +Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } , with_world{ with_world } , cumulative{ cumulative } , group_property{ group_property } - , interface{ interface } + , mode{ mode } , distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {} double Clearance::compute(const SubTrajectory& s, std::string& comment) const { @@ -140,7 +140,7 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { request.type = cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL; - const auto& state{ (interface == Interface::END) ? s.end() : s.start() }; + 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?). @@ -186,8 +186,8 @@ double Clearance::compute(const SubTrajectory& s, std::string& comment) const { double distance{ 0.0 }; - if (interface == Interface::START || interface == Interface::END || - (interface == Interface::NONE && s.trajectory() == nullptr)) { + 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); From 45e6cdb47db40025dc9de3ff887f6079289e921c Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 14 Sep 2020 19:24:01 +0200 Subject: [PATCH 37/42] Revert "add CostAggregator for SerialContainer" This reverts commit dc7ce9bdfac97eb468f5a850adcb27cc118b5fd7. It turns out multiple places in SerialContainer's cost inference expect 0.0 as the neutral element (which is why std::min and multiply did not work). While these additional issues can be fixed, it would make the interface much less elegant. We should consider adding it back if an actual use-case is there to discuss. --- .../moveit/task_constructor/container.h | 6 --- .../moveit/task_constructor/container_p.h | 2 - .../include/moveit/task_constructor/storage.h | 10 ++--- core/src/container.cpp | 22 +++------ core/src/storage.cpp | 12 +---- core/test/test_cost_terms.cpp | 45 ------------------- 6 files changed, 10 insertions(+), 87 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 8ca032e7..27b2fde0 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -94,12 +94,6 @@ public: bool canCompute() const override; void compute() override; - /** Specify how to aggregate costs of childrens' solutions - * - * By default costs are added up - */ - void setCostAggregator(const SolutionSequence::CostAggregator& agg); - protected: /// called by a (direct) child when a new solution becomes available void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/container_p.h b/core/include/moveit/task_constructor/container_p.h index cad54fcd..35a903ad 100644 --- a/core/include/moveit/task_constructor/container_p.h +++ b/core/include/moveit/task_constructor/container_p.h @@ -184,8 +184,6 @@ protected: // validate that child's interface matches mine (considering start or end only as determined by mask) template void validateInterface(const StagePrivate& child, InterfaceFlags required) const; - - SolutionSequence::CostAggregator aggregator_; }; PIMPL_FUNCTIONS(SerialContainer) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 52138ae2..5bf81a1f 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -318,18 +318,15 @@ class SolutionSequence : public SolutionBase public: using container_type = std::vector; - explicit SolutionSequence(); - - SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr); + explicit SolutionSequence() : SolutionBase() {} + SolutionSequence(container_type&& subsolutions, double cost = 0.0, Stage* creator = nullptr) + : SolutionBase(creator, cost), subsolutions_(std::move(subsolutions)) {} void push_back(const SolutionBase& solution); /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; - using CostAggregator = std::function; - void setCostAggregator(const CostAggregator&); - /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; @@ -339,7 +336,6 @@ public: inline const InterfaceState* internalEnd() const { return subsolutions_.back()->end(); } private: - CostAggregator aggregator_; /// series of sub solutions container_type subsolutions_; }; diff --git a/core/src/container.cpp b/core/src/container.cpp index 1c1b0993..18892591 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -360,9 +360,8 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { solution.reserve(children.size()); for (auto& in : incoming.solutions) { for (auto& out : outgoing.solutions) { - InterfaceState::Priority prio( - static_cast(in.first.size() + 1 + out.first.size()), - pimpl()->aggregator_(pimpl()->aggregator_(in.second, current.cost()), out.second)); + InterfaceState::Priority prio(static_cast(in.first.size() + 1 + out.first.size()), + in.second + current.cost() + out.second); // found a complete solution path connecting start to end? if (prio.depth() == children.size()) { if (std::isinf(prio.cost())) @@ -375,9 +374,7 @@ void SerialContainer::onNewSolution(const SolutionBase& current) { // insert outgoing solutions in normal order solution.insert(solution.end(), out.first.begin(), out.first.end()); // store solution in sorted list - auto sequence{ std::make_shared(std::move(solution), prio.cost(), this) }; - sequence->setCostAggregator(pimpl()->aggregator_); - sorted.insert(std::move(sequence)); + sorted.insert(std::make_shared(std::move(solution), prio.cost(), this)); } else if (prio.depth() > 1) { // update state priorities along the whole partial solution path updateStateCosts(in.first, prio); @@ -396,7 +393,7 @@ SerialContainer::SerialContainer(SerialContainerPrivate* impl) : ContainerBase(i SerialContainer::SerialContainer(const std::string& name) : SerialContainer(new SerialContainerPrivate(this, name)) {} SerialContainerPrivate::SerialContainerPrivate(SerialContainer* me, const std::string& name) - : ContainerBasePrivate(me, name), aggregator_(std::plus{}) {} + : ContainerBasePrivate(me, name) {} void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2) { InterfaceFlags flags1 = stage1.requiredInterface(); @@ -521,10 +518,6 @@ void SerialContainerPrivate::validateConnectivity() const { } } -void SerialContainer::setCostAggregator(const SolutionSequence::CostAggregator& agg) { - pimpl()->aggregator_ = agg; -} - bool SerialContainer::canCompute() const { for (const auto& stage : pimpl()->children()) { if (stage->pimpl()->canCompute()) @@ -556,14 +549,11 @@ void SerialContainer::traverse(const SolutionBase& start, const SolutionProcesso else for (SolutionBase* successor : solutions) { trace.push_back(successor); - - double previous_trace_cost { trace_cost }; - - trace_cost = pimpl()->aggregator_(trace_cost, successor->cost()); + trace_cost += successor->cost(); traverse(*successor, cb, trace, trace_cost); - trace_cost = previous_trace_cost; + trace_cost -= successor->cost(); trace.pop_back(); } } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 1133257e..ff096a12 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -182,19 +182,10 @@ double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const return f(*this, comment); } -SolutionSequence::SolutionSequence() : SolutionBase(), aggregator_(std::plus{}) {} - -SolutionSequence::SolutionSequence(container_type&& subsolutions, double cost, Stage* creator) - : SolutionBase(creator, cost), aggregator_(std::plus{}), subsolutions_(std::move(subsolutions)) {} - void SolutionSequence::push_back(const SolutionBase& solution) { subsolutions_.push_back(&solution); } -void SolutionSequence::setCostAggregator(const CostAggregator& agg) { - aggregator_ = agg; -} - void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const { moveit_task_constructor_msgs::SubSolution sub_msg; SolutionBase::fillInfo(sub_msg.info, introspection); @@ -234,8 +225,7 @@ double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) co double cost{ 0.0 }; std::string subcomment; for (auto& solution : subsolutions_) { - auto subcost{ solution->computeCost(f, subcomment) }; - cost = aggregator_(cost, subcost); + cost += solution->computeCost(f, subcomment); if (!subcomment.empty()) { if (!comment.empty()) comment.append(", "); diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index c819c343..7809a81b 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -11,8 +11,6 @@ #include -#include - using namespace moveit::task_constructor; using namespace planning_scene; @@ -351,47 +349,4 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { << "container cost term overwrites stage costs"; EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected"; } - -TEST(CostTerm, CompositeSolutionsContainerAggregator) { - ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); - Standalone container{ getModel() }; - - cost::Constant constant{ 17.0 }; - - { - auto s1{ std::make_unique() }; - auto s2{ std::make_unique() }; - auto s3{ std::make_unique() }; - - container.computeWithStageCost({ std::move(s1), std::move(s2), std::move(s3) }, constant); - EXPECT_EQ(container.solutions().front()->cost(), 3 * constant.cost) << "default cost aggregates through stages"; - } - - { - container.setCostAggregator([](double a, double b) { return std::max(a, b); }); - container.computeWithContainerCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works with explicit cost term in container"; - } - - { - container.setCostAggregator([](auto a, auto b) { return std::max(a, b); }); - container.computeWithStageCost( - { - std::make_unique(), - std::make_unique(), - std::make_unique(), - }, - constant); - - EXPECT_EQ(container.solutions().front()->cost(), constant.cost) - << "custom aggregator works without container cost term"; - } } From 62586c2688a884db41076c08feb44abc771b4e52 Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 16 Sep 2020 19:08:53 +0200 Subject: [PATCH 38/42] rework CostTerm's from support flags to vtable requested in review. Without support for custom aggregators, which we dropped again after finding more flaws with it, I agree that this is the nicer solution. On the downside, it converts the interfaces from copyable objects to another round of shared_ptrs. I added shortcuts for lambda costs to keep support for `stage->setCostTerm([](auto&& s){ return 42; })` without the additional `stage->setCostTerm(LambdaCostTerm{ [](auto&& s){ return 42; } } )` --- .../moveit/task_constructor/cost_terms.h | 96 ++++++++++--------- core/include/moveit/task_constructor/stage.h | 16 ++-- .../include/moveit/task_constructor/stage_p.h | 2 +- core/src/cost_terms.cpp | 85 +++++++++------- core/src/stage.cpp | 20 +++- core/src/stages/connect.cpp | 2 +- core/src/stages/fix_collision_objects.cpp | 2 +- core/src/stages/fixed_cartesian_poses.cpp | 2 +- core/src/stages/fixed_state.cpp | 2 +- core/src/stages/generate_pose.cpp | 2 +- core/src/stages/modify_planning_scene.cpp | 2 +- core/src/stages/move_relative.cpp | 2 +- core/src/stages/move_to.cpp | 2 +- core/src/storage.cpp | 16 +--- core/test/test_cost_terms.cpp | 49 +++++----- 15 files changed, 163 insertions(+), 137 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 6872a024..d61dce15 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -44,51 +44,56 @@ 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: - using SubTrajectorySig = std::function; - using SubTrajectoryShortSig = std::function; + 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 - CostTerm(const Term& t) : CostTerm{ decltype(sigMatcher(t)){ t } } {} + template ()))> + LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {} - CostTerm(const SubTrajectorySig&); - CostTerm(const SubTrajectoryShortSig&); - CostTerm(std::nullptr_t); - CostTerm(); + LambdaCostTerm(const SubTrajectorySignature&); + LambdaCostTerm(const SubTrajectoryShortSignature&); - // is this a valid CostTerm? - operator bool() const; - - double operator()(const SolutionBase& s, std::string& comment) const; - - /** describes the supported types of solutions that should be forwarded to this CostTerm - * - * CostTerms that support more than `SubTrajectory`s should inherit and overwrite the internal supports_ flag - */ - enum class SolutionType - { - NONE = 0, - TRAJECTORY = 1 << 0, - SEQUENCE = 1 << 1, - WRAPPER = 1 << 2, - ALL = TRAJECTORY | SEQUENCE | WRAPPER - }; - Flags supports() const; + double operator()(const SubTrajectory& s, std::string& comment) const override; protected: - CostTerm(const std::function&, Flags); - - std::function term_; - Flags supports_; + SubTrajectorySignature term_; private: template - auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSig{}); + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{}); template - auto sigMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySig{}); + static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{}); }; namespace cost { @@ -97,35 +102,39 @@ namespace cost { class Constant : public CostTerm { public: - Constant(double c); + 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 CostTerm +class PathLength : public TrajectoryCostTerm { -public: - PathLength(); // 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 CostTerm +class TrajectoryDuration : public TrajectoryCostTerm { public: - TrajectoryDuration(); + double operator()(const SubTrajectory&, std::string&) const override; }; -class LinkMotion : public CostTerm +/** length of Cartesian trajection of a link */ +class LinkMotion : public TrajectoryCostTerm { public: LinkMotion(std::string link_name); std::string link_name; -protected: - double compute(const SubTrajectory&, std::string&) const; + double operator()(const SubTrajectory&, std::string&) const override; }; /** inverse distance to collision @@ -136,7 +145,7 @@ protected: * \arg interface compute distances using START or END interface of solution *only*, instead of averaging over * trajectory * */ -class Clearance : public CostTerm +class Clearance : public TrajectoryCostTerm { public: enum class Mode @@ -157,8 +166,7 @@ public: std::function distance_to_cost; -protected: - double compute(const SubTrajectory& s, std::string& comment) const; + double operator()(const SubTrajectory& s, std::string& comment) const override; }; } // namespace cost diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 57eeb4de..c6ac9b57 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -138,7 +138,8 @@ private: }; std::ostream& operator<<(std::ostream& os, const InitStageException& e); -class CostTerm; +MOVEIT_CLASS_FORWARD(CostTerm); +class LambdaCostTerm; class ContainerBase; class StagePrivate; class Stage @@ -214,11 +215,14 @@ public: /// remove function callback void removeSolutionCallback(SolutionCallbackList::const_iterator which); - /* \brief set method to determine costs for solutions of this stage - * - * For composite stages, costs are computed for each SubTrajectory and aggregated - */ - void setCostTerm(const CostTerm& term); + /** 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; diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index b9536b0c..3982f7c6 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -170,7 +170,7 @@ protected: InterfacePtr ends_; // user-configurable cost estimator - CostTerm cost_term_; + CostTermConstPtr cost_term_; // The total compute time std::chrono::duration total_compute_time_; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 0298a122..09439fdb 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -49,38 +49,60 @@ namespace moveit { namespace task_constructor { -CostTerm::CostTerm() : term_{ nullptr }, supports_{ SolutionType::NONE } {} - -CostTerm::CostTerm(std::nullptr_t) : CostTerm{} {} - -CostTerm::CostTerm(const SubTrajectorySig& term) - : CostTerm{ [term](const SolutionBase& s, std::string& c) { return term(static_cast(s), c); }, - SolutionType::TRAJECTORY } {} - -CostTerm::CostTerm(const SubTrajectoryShortSig& term) - : CostTerm{ [term](const SolutionBase& s, std::string&) { return term(static_cast(s)); }, - SolutionType::TRAJECTORY } {} - -CostTerm::CostTerm(const std::function& term, Flags flags) - : term_{ term }, supports_{ flags } {} - -CostTerm::operator bool() const { - return supports_ != Flags{ SolutionType::NONE }; +double CostTerm::operator()(const SubTrajectory& s, std::string&) const { + return s.cost(); } -double CostTerm::operator()(const SolutionBase& s, std::string& comment) const { +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 { -Constant::Constant(double c) - : CostTerm{ [this](auto&&, auto&&) { return this->cost; }, SolutionType::ALL }, cost{ c } {} +double Constant::operator()(const SubTrajectory&, std::string&) const { + return cost; +} -namespace { +double Constant::operator()(const SolutionSequence&, std::string&) const { + return cost; +} -double pathLength(const SubTrajectory& s) { +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) @@ -92,17 +114,13 @@ double pathLength(const SubTrajectory& s) { return path_length; } -} // namespace +double TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const { + return s.trajectory() ? s.trajectory()->getDuration() : 0.0; +} -PathLength::PathLength() : CostTerm{ pathLength } {} +LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {} -TrajectoryDuration::TrajectoryDuration() - : CostTerm{ [](auto&& s, auto&&) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } } {} - -LinkMotion::LinkMotion(std::string link) - : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } }, link_name{ std::move(link) } {} - -double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { +double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const { const auto& traj{ s.trajectory() }; if (traj == nullptr || traj->getWayPointCount() == 0) @@ -126,14 +144,13 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const { } Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode) - : CostTerm{ [this](auto&& s, auto&& c) { return this->compute(s, c); } } - , with_world{ with_world } + : 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::compute(const SubTrajectory& s, std::string& comment) const { +double Clearance::operator()(const SubTrajectory& s, std::string& comment) const { const std::string PREFIX{ "Clearance: " }; collision_detection::DistanceRequest request; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index d62eb2bf..3d9c6f0e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -235,7 +235,7 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) { void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) { // no reason to compute costs for a failed solution - if (solution.isFailure() || !cost_term_) + if (solution.isFailure()) return; // chicken-and-egg problem: we don't know whether/where we will store the solution yet, @@ -246,7 +246,9 @@ void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& solution.setEndState(tmp_to); std::string comment; - solution.setCost(solution.computeCost(cost_term_, 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 + ")"); @@ -297,8 +299,13 @@ void Stage::reset() { } void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { - // init properties once from parent auto impl = pimpl(); + + // add a default cost term if none was given by the user + if (!impl->cost_term_) + impl->cost_term_ = std::make_unique(); + + // init properties once from parent impl->properties_.reset(); if (impl->parent()) { try { @@ -351,8 +358,11 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) { pimpl()->solution_cbs_.erase(which); } -void Stage::setCostTerm(const CostTerm& term) { - pimpl()->cost_term_ = term; +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 { diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index afa23fe3..c9f4cd06 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -48,7 +48,7 @@ namespace stages { Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) { setTimeout(1.0); - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); diff --git a/core/src/stages/fix_collision_objects.cpp b/core/src/stages/fix_collision_objects.cpp index 05b820e8..4546454e 100644 --- a/core/src/stages/fix_collision_objects.cpp +++ b/core/src/stages/fix_collision_objects.cpp @@ -56,7 +56,7 @@ namespace stages { FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) { // TODO: possibly weight solutions based on the required displacement? - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); auto& p = properties(); p.declare("max_penetration", "maximally corrected penetration depth"); diff --git a/core/src/stages/fixed_cartesian_poses.cpp b/core/src/stages/fixed_cartesian_poses.cpp index d6f0d96a..068f2759 100644 --- a/core/src/stages/fixed_cartesian_poses.cpp +++ b/core/src/stages/fixed_cartesian_poses.cpp @@ -49,7 +49,7 @@ namespace stages { using PosesList = std::vector; FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::Constant(0.0)); + 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 971b6da6..fddc9e4d 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -44,7 +44,7 @@ namespace task_constructor { namespace stages { FixedState::FixedState(const std::string& name) : Generator(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); } void FixedState::setState(const planning_scene::PlanningScenePtr& scene) { diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp index ebdb73ba..8315bc95 100644 --- a/core/src/stages/generate_pose.cpp +++ b/core/src/stages/generate_pose.cpp @@ -46,7 +46,7 @@ namespace task_constructor { namespace stages { GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) { - setCostTerm(cost::Constant(0.0)); + 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 8343151e..c14605a6 100644 --- a/core/src/stages/modify_planning_scene.cpp +++ b/core/src/stages/modify_planning_scene.cpp @@ -47,7 +47,7 @@ namespace task_constructor { namespace stages { ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) { - setCostTerm(cost::Constant(0.0)); + setCostTerm(std::make_unique(0.0)); } void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach) { diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0c4db5d5..e5266825 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -49,7 +49,7 @@ namespace stages { MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 0c8385c9..f9f915c3 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -50,7 +50,7 @@ namespace stages { MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner) : PropagatingEitherWay(name), planner_(planner) { - setCostTerm(cost::PathLength{}); + setCostTerm(std::make_unique()); auto& p = properties(); p.property("timeout").setDefaultValue(1.0); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index ff096a12..be1690ab 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -222,19 +222,7 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg, } double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const { - double cost{ 0.0 }; - std::string subcomment; - for (auto& solution : subsolutions_) { - cost += solution->computeCost(f, subcomment); - if (!subcomment.empty()) { - if (!comment.empty()) - comment.append(", "); - comment.append(subcomment); - subcomment.clear(); - } - } - - return cost; + return f(*this, comment); } void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution, @@ -249,7 +237,7 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti } double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const { - return wrapped_->computeCost(f, comment); + return f(*this, comment); } } // namespace task_constructor diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 7809a81b..95fbcfd3 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -158,14 +158,14 @@ public: runCompute(); } - void computeWithStageCost(std::initializer_list stages, const CostTerm& cost_term) { + 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 CostTerm& cost_term) { + void computeWithContainerCost(std::initializer_list stages, const CostTermPtr& cost_term) { this->setCostTerm(cost_term); computeWithStages(stages); } @@ -209,9 +209,9 @@ TEST(CostTerm, CostOverwrite) { container.computeWithStageCost({ std::make_unique() }, nullptr); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost"; - cost::Constant constant_cost{ 1.0 }; + 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"; + EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost"; } TEST(CostTerm, StageTypes) { @@ -220,20 +220,20 @@ TEST(CostTerm, StageTypes) { Standalone container(robot); - const cost::Constant constant{ 1.0 }; + 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"; + 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"; + 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"; + EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator"; } TEST(CostTerm, PassThroughUsesCost) { @@ -242,14 +242,14 @@ TEST(CostTerm, PassThroughUsesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant_stage{ 84.0 }; + auto constant_stage{ std::make_shared(84.0) }; stage->setCostTerm(constant_stage); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "PassThrough forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals PassThrough cost"; + EXPECT_EQ(wrapped.cost(), constant_stage->cost) << "PassThrough forwards children's cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "Child cost equals PassThrough cost"; } TEST(CostTerm, PassThroughOverwritesCost) { @@ -258,16 +258,16 @@ TEST(CostTerm, PassThroughOverwritesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant_stage{ 84.0 }; + auto constant_stage{ std::make_shared(84.0) }; stage->setCostTerm(constant_stage); - cost::Constant constant_passthrough{ 72.0 }; + auto constant_passthrough{ std::make_shared(72.0) }; container.setCostTerm(constant_passthrough); container.computeWithStages({ std::move(stage) }); auto& wrapped{ dynamic_cast(*container.solutions().front()) }; - EXPECT_EQ(wrapped.cost(), constant_passthrough.cost) << "PassThrough can apply custom cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected"; + EXPECT_EQ(wrapped.cost(), constant_passthrough->cost) << "PassThrough can apply custom cost"; + EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "child's cost is not affected"; } TEST(CostTerm, PassThroughCanModifyCost) { @@ -276,14 +276,14 @@ TEST(CostTerm, PassThroughCanModifyCost) { Standalone container(robot); auto stage{ std::make_unique() }; - cost::Constant constant{ 8.0 }; + 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"; + 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) { @@ -300,12 +300,12 @@ TEST(CostTerm, CompositeSolutions) { { auto s1{ std::make_unique() }; - cost::Constant constant{ 1.0 }; + 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) + EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost) << "mix of explicit and implicit cost terms works"; } @@ -313,18 +313,18 @@ TEST(CostTerm, CompositeSolutions) { auto s1{ std::make_unique() }; auto s2{ std::make_unique() }; auto c1{ std::make_unique() }; - cost::Constant constant1{ 1.0 }; + 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() }; - cost::Constant constant2{ 9.0 }; + 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) + EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1->cost + constant2->cost) << "cost aggregation works across multiple levels"; } } @@ -343,10 +343,9 @@ TEST(CostTerm, CompositeSolutionsContainerCost) { auto s3{ std::make_unique() }; - container.setCostTerm(cost::TrajectoryDuration{}); + 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"; } -} From 687fcb6a6dc313bc9e08defecd5c92836375c340 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Sep 2020 12:18:30 +0200 Subject: [PATCH 39/42] Update comments to solution's computeCost() methods --- core/include/moveit/task_constructor/storage.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 5bf81a1f..25aa8c51 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -248,7 +248,7 @@ public: Introspection* introspection = nullptr) const = 0; void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const; - /// compute cost of this solution according to the specified CostTerm + /// 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 @@ -327,7 +327,6 @@ public: /// append all subsolutions to solution void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override; - /// aggregate costs along the sequence double computeCost(const CostTerm& cost, std::string& comment) const override; const container_type& solutions() const { return subsolutions_; } @@ -359,7 +358,7 @@ public: : WrappedSolution(creator, wrapped, wrapped->cost()) {} void fillMessage(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const override; - /// compute cost of wrapped child + double computeCost(const CostTerm& cost, std::string& comment) const override; const SolutionBase* wrapped() const { return wrapped_; } From 5d0fd577549a12ceb8cc65d7827b94c998330d18 Mon Sep 17 00:00:00 2001 From: v4hn Date: Thu, 17 Sep 2020 22:56:19 +0200 Subject: [PATCH 40/42] initialize default CostTerm in constructor It seems to be too fragile to rely on `init` being called. On the other hand we should really enforce this anyway. -.- --- core/src/stage.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 3d9c6f0e..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; @@ -301,10 +306,6 @@ void Stage::reset() { void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) { auto impl = pimpl(); - // add a default cost term if none was given by the user - if (!impl->cost_term_) - impl->cost_term_ = std::make_unique(); - // init properties once from parent impl->properties_.reset(); if (impl->parent()) { From 04a8afb16c9675113cff69f7665e6355551c8e01 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 21 Sep 2020 14:00:49 +0200 Subject: [PATCH 41/42] melodic compatibility --- core/src/cost_terms.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 09439fdb..a1384c0a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -175,9 +175,18 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const 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; From 198f8f53898c2ebee770f45ecb8f0b429c406639 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 17 Sep 2020 15:22:59 +0200 Subject: [PATCH 42/42] clearer naming of cost variables --- core/test/test_cost_terms.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/core/test/test_cost_terms.cpp b/core/test/test_cost_terms.cpp index 95fbcfd3..ee62f0c8 100644 --- a/core/test/test_cost_terms.cpp +++ b/core/test/test_cost_terms.cpp @@ -242,14 +242,14 @@ TEST(CostTerm, PassThroughUsesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - auto constant_stage{ std::make_shared(84.0) }; - stage->setCostTerm(constant_stage); + 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_stage->cost) << "PassThrough forwards children's cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "Child cost equals PassThrough cost"; + 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) { @@ -258,16 +258,16 @@ TEST(CostTerm, PassThroughOverwritesCost) { Standalone container(robot); auto stage{ std::make_unique() }; - auto constant_stage{ std::make_shared(84.0) }; - stage->setCostTerm(constant_stage); + auto constant_inner{ std::make_shared(84.0) }; + stage->setCostTerm(constant_inner); - auto constant_passthrough{ std::make_shared(72.0) }; - container.setCostTerm(constant_passthrough); + 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_passthrough->cost) << "PassThrough can apply custom cost"; - EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage->cost) << "child's cost is not affected"; + 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) {