Merge PR #183: CostTerm API

This commit is contained in:
Robert Haschke 2020-09-22 07:47:42 +02:00 committed by GitHub
commit e9c56c9c64
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
22 changed files with 1101 additions and 57 deletions

View File

@ -0,0 +1,174 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Hamburg University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael 'v4hn' Goerner
Desc: Define implementations for general CostTerm's to use with Stage::setCostTerm()
*/
#pragma once
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
namespace moveit {
namespace task_constructor {
/** basic interface to compute costs for solutions
*
* If your cost term will only work on SubTrajectory solution objects,
* inherit from TrajectoryCostTerm instead.
*/
MOVEIT_CLASS_FORWARD(CostTerm);
class CostTerm
{
public:
CostTerm() = default;
CostTerm(std::nullptr_t) : CostTerm{} {}
virtual double operator()(const SubTrajectory& s, std::string& comment) const;
virtual double operator()(const SolutionSequence& s, std::string& comment) const;
virtual double operator()(const WrappedSolution& s, std::string& comment) const;
};
/** base class for cost terms that only work on SubTrajectory solutions
*
*/
class TrajectoryCostTerm : public CostTerm
{
public:
double operator()(const WrappedSolution& s, std::string& comment) const override;
};
class LambdaCostTerm : public TrajectoryCostTerm
{
public:
using SubTrajectorySignature = std::function<double(const SubTrajectory&, std::string&)>;
using SubTrajectoryShortSignature = std::function<double(const SubTrajectory&)>;
// accept lambdas according to either signature above
template <typename Term, typename Signature = decltype(signatureMatcher(std::declval<Term>()))>
LambdaCostTerm(const Term& t) : LambdaCostTerm{ Signature{ t } } {}
LambdaCostTerm(const SubTrajectorySignature&);
LambdaCostTerm(const SubTrajectoryShortSignature&);
double operator()(const SubTrajectory& s, std::string& comment) const override;
protected:
SubTrajectorySignature term_;
private:
template <typename T>
static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}), SubTrajectoryShortSignature{});
template <typename T>
static auto signatureMatcher(const T& t) -> decltype(t(SubTrajectory{}, std::string{}), SubTrajectorySignature{});
};
namespace cost {
/// add a constant cost to each solution
class Constant : public CostTerm
{
public:
Constant(double c) : cost{ c } {};
double operator()(const SubTrajectory&, std::string&) const override;
double operator()(const SolutionSequence& s, std::string& comment) const override;
double operator()(const WrappedSolution& s, std::string& comment) const override;
double cost;
};
/// trajectory length (interpolated between waypoints)
class PathLength : public TrajectoryCostTerm
{
// TODO(v4hn): allow to consider specific joints only
public:
double operator()(const SubTrajectory&, std::string&) const override;
};
/// execution duration of the whole trajectory
class TrajectoryDuration : public TrajectoryCostTerm
{
public:
double operator()(const SubTrajectory&, std::string&) const override;
};
/** length of Cartesian trajection of a link */
class LinkMotion : public TrajectoryCostTerm
{
public:
LinkMotion(std::string link_name);
std::string link_name;
double operator()(const SubTrajectory&, std::string&) const override;
};
/** inverse distance to collision
*
* \arg with_world check distances to world objects or look at self-collisions
* \arg cumulative if true, compute clearance as aggregated distance of all bodies
* \arg group_property the name of the property which defines the group to look at
* \arg interface compute distances using START or END interface of solution *only*, instead of averaging over
* trajectory
* */
class Clearance : public TrajectoryCostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Mode mode = Mode::AUTO);
bool with_world;
bool cumulative;
std::string group_property;
Mode mode;
std::function<double(double)> distance_to_cost;
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
} // namespace cost
} // namespace task_constructor
} // namespace moveit

View File

@ -138,6 +138,8 @@ private:
};
std::ostream& operator<<(std::ostream& os, const InitStageException& e);
MOVEIT_CLASS_FORWARD(CostTerm);
class LambdaCostTerm;
class ContainerBase;
class StagePrivate;
class Stage
@ -159,6 +161,7 @@ public:
PARENT = 2,
INTERFACE = 4,
};
virtual ~Stage();
/// auto-convert Stage to StagePrivate* when needed
@ -212,6 +215,15 @@ public:
/// remove function callback
void removeSolutionCallback(SolutionCallbackList::const_iterator which);
/** set method to determine costs for solutions of this stage */
void setCostTerm(const CostTermConstPtr& term);
// overload to accept appropriate lambda expressions
template <typename T, typename = std::enable_if_t<std::is_constructible<LambdaCostTerm, T>::value>>
void setCostTerm(T term) {
setCostTerm(std::make_shared<LambdaCostTerm>(term));
}
const ordered<SolutionBaseConstPtr>& solutions() const;
const std::list<SolutionBaseConstPtr>& failures() const;
size_t numFailures() const;
@ -227,8 +239,10 @@ public:
void setProperty(const std::string& name, const boost::any& value);
/// overload: const char* values are stored as std::string
inline void setProperty(const std::string& name, const char* value) { setProperty(name, std::string(value)); }
/// analyze source of error and report accordingly
[[noreturn]] void reportPropertyError(const Property::error& e);
double getTotalComputeTime() const;
protected:

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/cost_queue.h>
#include <ros/ros.h>
@ -154,8 +155,13 @@ public:
total_compute_time_ += compute_stop_time - compute_start_time;
}
/** compute cost for solution through configured CostTerm */
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
protected:
Stage* me_; // associated/owning Stage instance
// associated/owning Stage instance
Stage* me_;
std::string name_;
PropertyMap properties_;
@ -163,6 +169,9 @@ protected:
InterfacePtr starts_;
InterfacePtr ends_;
// user-configurable cost estimator
CostTermConstPtr cost_term_;
// The total compute time
std::chrono::duration<double> total_compute_time_;

View File

@ -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 <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit {
namespace task_constructor {
namespace stages {
/** Generic Wrapper that passes on a solution
*
* Useful to set a custom CostTransform via Stage::setCostTerm
* to change a solution's cost without loosing the original value
*/
class PassThrough : public WrapperBase
{
public:
PassThrough(const std::string& name = "PassThrough", Stage::pointer&& child = Stage::pointer());
void onNewSolution(const SolutionBase& s) override;
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -192,10 +192,15 @@ private:
using base_type::remove_if;
};
class CostTerm;
class StagePrivate;
class ContainerBasePrivate;
/// abstract base class for solutions (primitive and sequences)
class SolutionBase
{
friend StagePrivate; // for set[Start|End]StateUnsafe
friend ContainerBasePrivate;
public:
virtual ~SolutionBase() = default;
@ -206,18 +211,22 @@ public:
template <Interface::Direction dir>
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<InterfaceState&>(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<InterfaceState&>(state).addIncoming(this);
setEndStateUnsafe(state);
}
inline const Stage* creator() const { return creator_; }
@ -239,6 +248,9 @@ public:
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable
virtual double computeCost(const CostTerm& cost, std::string& comment) const = 0;
/// order solutions by their cost
bool operator<(const SolutionBase& other) const { return this->cost_ < other.cost_; }
@ -246,6 +258,18 @@ protected:
SolutionBase(Stage* creator = nullptr, double cost = 0.0, std::string comment = "")
: creator_(creator), cost_(cost), comment_(std::move(comment)) {}
/** unsafe setter for start_state_
*
* must only be used if the previously set state removes its link to this solution
*/
void setStartStateUnsafe(const InterfaceState& state);
/** unsafe setter for end_state_
*
* must only be used if the previously set state removes its link to this solution
*/
void setEndStateUnsafe(const InterfaceState& state);
private:
// back-pointer to creating stage, allows to access sub-solutions
Stage* creator_;
@ -276,6 +300,8 @@ public:
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
private:
// actual trajectory, might be empty
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
@ -301,6 +327,8 @@ public:
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
const container_type& solutions() const { return subsolutions_; }
inline const InterfaceState* internalStart() const { return subsolutions_.front()->start(); }
@ -331,6 +359,8 @@ public:
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
const SolutionBase* wrapped() const { return wrapped_; }
private:

View File

@ -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

View File

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

250
core/src/cost_terms.cpp Normal file
View File

@ -0,0 +1,250 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holders nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner */
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/collision_detection/collision_common.h>
#include <Eigen/Geometry>
#include <boost/format.hpp>
namespace moveit {
namespace task_constructor {
double CostTerm::operator()(const SubTrajectory& s, std::string&) const {
return s.cost();
}
double CostTerm::operator()(const SolutionSequence& s, std::string& comment) const {
double cost{ 0.0 };
std::string subcomment;
for (auto& solution : s.solutions()) {
cost += solution->computeCost((*this), subcomment);
if (!subcomment.empty()) {
if (!comment.empty())
comment.append(", ");
comment.append(subcomment);
subcomment.clear();
}
}
return cost;
}
double CostTerm::operator()(const WrappedSolution& s, std::string& /*comment*/) const {
return s.cost();
}
double TrajectoryCostTerm::operator()(const WrappedSolution& s, std::string& comment) const {
return s.wrapped()->computeCost(*this, comment);
}
LambdaCostTerm::LambdaCostTerm(const SubTrajectorySignature& term)
: term_{ [term](const SolutionBase& s, std::string& c) { return term(static_cast<const SubTrajectory&>(s), c); } } {}
LambdaCostTerm::LambdaCostTerm(const SubTrajectoryShortSignature& term)
: term_{ [term](const SolutionBase& s, std::string&) { return term(static_cast<const SubTrajectory&>(s)); } } {}
double LambdaCostTerm::operator()(const SubTrajectory& s, std::string& comment) const {
assert(bool{ term_ });
return term_(s, comment);
}
namespace cost {
double Constant::operator()(const SubTrajectory&, std::string&) const {
return cost;
}
double Constant::operator()(const SolutionSequence&, std::string&) const {
return cost;
}
double Constant::operator()(const WrappedSolution&, std::string&) const {
return cost;
}
double PathLength::operator()(const SubTrajectory& s, std::string&) const {
const auto& traj = s.trajectory();
if (traj == nullptr)
return 0.0;
double path_length{ 0.0 };
for (size_t i = 1; i < traj->getWayPointCount(); ++i)
path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i));
return path_length;
}
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string&) const {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}
LinkMotion::LinkMotion(std::string link) : link_name{ std::move(link) } {}
double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) const {
const auto& traj{ s.trajectory() };
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) {
boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory");
desc % link_name;
comment = desc.str();
return std::numeric_limits<double>::infinity();
}
double distance{ 0.0 };
Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() };
for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) {
const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
distance += (new_position - position).norm();
position = new_position;
}
return distance;
}
Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode)
: with_world{ with_world }
, cumulative{ cumulative }
, group_property{ group_property }
, mode{ mode }
, distance_to_cost{ [](double d) { return 1.0 / (d + 1e-5); } } {}
double Clearance::operator()(const SubTrajectory& s, std::string& comment) const {
const std::string PREFIX{ "Clearance: " };
collision_detection::DistanceRequest request;
request.type =
cumulative ? collision_detection::DistanceRequestType::SINGLE : collision_detection::DistanceRequestType::GLOBAL;
const auto& state{ (mode == Mode::END_INTERFACE) ? s.end() : s.start() };
// prefer interface state property over stage property to find group_name
// TODO: This pattern is general enough to justify its own interface (in the properties?).
auto& state_properties{ state->properties() };
auto& stage_properties{ s.creator()->properties() };
request.group_name = state_properties.hasProperty(group_property) ?
state_properties.get<std::string>(group_property) :
stage_properties.get<std::string>(group_property);
// look at all forbidden collisions involving group_name
request.enableGroup(state->scene()->getRobotModel());
request.acm = &state->scene()->getAllowedCollisionMatrix();
// compute relevant distance data for state & robot
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else
#if MOVEIT_MASTER
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
#endif
if (result.minimum_distance.distance <= 0) {
return result.minimum_distance;
}
if (cumulative) {
double distance{ 0.0 };
for (const auto& distance_of_pair : result.distances) {
assert(distance_of_pair.second.size() == 1);
distance += distance_of_pair.second[0].distance;
}
result.minimum_distance.distance = distance;
}
return result.minimum_distance;
} };
auto collision_comment{ [=](const auto& distance) {
boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" };
desc % distance.link_names[0] % distance.link_names[1];
return desc.str();
} };
double distance{ 0.0 };
if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE ||
(mode == Mode::AUTO && s.trajectory() == nullptr)) {
auto distance_data{ check_distance(state, state->scene()->getCurrentState()) };
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
distance = distance_data.distance;
if (!cumulative) {
boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" };
desc % distance % distance_data.link_names[0] % distance_data.link_names[1];
comment = desc.str();
} else {
comment = PREFIX + "cumulative distance " + std::to_string(distance);
}
} else { // check trajectory
for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) {
auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i));
if (distance_data.distance < 0) {
comment = collision_comment(distance_data);
return std::numeric_limits<double>::infinity();
}
distance += distance_data.distance;
}
distance /= s.trajectory()->getWayPointCount();
boost::format desc(PREFIX + "average%1% distance: %2%");
desc % (cumulative ? " cumulative" : "") % distance;
comment = desc.str();
}
return distance_to_cost(distance);
}
} // namespace cost
} // namespace task_constructor
} // namespace moveit

View File

@ -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<CostTerm>() }
, total_compute_time_{}
, parent_{ nullptr }
, introspection_{ nullptr } {}
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
@ -125,7 +130,6 @@ void StagePrivate::validateConnectivity() const {
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
solution->setCreator(me());
if (introspection_)
introspection_->registerSolution(*solution);
@ -142,14 +146,21 @@ bool StagePrivate::storeSolution(const SolutionBasePtr& solution) {
void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, const SolutionBasePtr& solution) {
assert(nextStarts());
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
me()->forwardProperties(from, to);
auto to_it = states_.insert(states_.end(), std::move(to));
solution->setStartState(from);
solution->setEndState(*to_it);
// register stored interfaces with solution
solution->setStartStateUnsafe(from);
solution->setEndStateUnsafe(*to_it);
if (!solution->isFailure())
nextStarts()->add(*to_it);
@ -159,14 +170,20 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution) {
assert(prevEnds());
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
me()->forwardProperties(to, from);
auto from_it = states_.insert(states_.end(), std::move(from));
solution->setStartState(*from_it);
solution->setEndState(to);
solution->setStartStateUnsafe(*from_it);
solution->setEndStateUnsafe(to);
if (!solution->isFailure())
prevEnds()->add(*from_it);
@ -176,14 +193,19 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
assert(prevEnds() && nextStarts());
solution->setCreator(me());
computeCost(state, state, *solution);
if (!storeSolution(solution))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto to = states_.insert(states_.end(), std::move(state));
solution->setStartState(*from);
solution->setEndState(*to);
solution->setStartStateUnsafe(*from);
solution->setEndStateUnsafe(*to);
if (!solution->isFailure()) {
prevEnds()->add(*from);
@ -194,11 +216,15 @@ void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution
}
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
solution->setCreator(me());
computeCost(from, to, *solution);
if (!storeSolution(solution))
return; // solution dropped
solution->setStartState(from);
solution->setEndState(to);
solution->setStartStateUnsafe(from);
solution->setEndStateUnsafe(to);
newSolution(solution);
}
@ -212,6 +238,30 @@ void StagePrivate::newSolution(const SolutionBasePtr& solution) {
parent()->onNewSolution(*solution);
}
void StagePrivate::computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
// no reason to compute costs for a failed solution
if (solution.isFailure())
return;
// chicken-and-egg problem: we don't know whether/where we will store the solution yet,
// but need to store it properly to compute the cost with a clean interface:
// set state copies for solution before computing cost:
InterfaceState tmp_from{ from }, tmp_to{ to };
solution.setStartState(tmp_from);
solution.setEndState(tmp_to);
std::string comment;
assert(cost_term_);
solution.setCost(solution.computeCost(*cost_term_, comment));
// If a comment was specified, add it to the solution
if (!comment.empty() && !solution.comment().empty()) {
solution.setComment(solution.comment() + " (" + comment + ")");
} else if (!comment.empty()) {
solution.setComment(comment);
}
}
Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
assert(impl);
auto& p = properties();
@ -254,8 +304,9 @@ void Stage::reset() {
}
void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
// init properties once from parent
auto impl = pimpl();
// init properties once from parent
impl->properties_.reset();
if (impl->parent()) {
try {
@ -308,6 +359,13 @@ void Stage::removeSolutionCallback(SolutionCallbackList::const_iterator which) {
pimpl()->solution_cbs_.erase(which);
}
void Stage::setCostTerm(const CostTermConstPtr& term) {
if (!term)
pimpl()->cost_term_ = std::make_unique<CostTerm>();
else
pimpl()->cost_term_ = std::move(term);
}
const ordered<SolutionBaseConstPtr>& Stage::solutions() const {
return pimpl()->solutions_;
}

View File

@ -9,6 +9,7 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
${PROJECT_INCLUDE}/stages/connect.h
@ -28,6 +29,7 @@ add_library(${PROJECT_NAME}_stages
generate_grasp_pose.cpp
generate_place_pose.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
connect.cpp

View File

@ -40,12 +40,16 @@
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
namespace moveit {
namespace task_constructor {
namespace stages {
Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : Connecting(name), planner_(planners) {
setTimeout(1.0);
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
@ -181,15 +185,6 @@ Connect::makeSequential(const std::vector<robot_trajectory::RobotTrajectoryConst
planning_scene::PlanningSceneConstPtr start_ps = *scene_it;
const InterfaceState* state = &from;
// calculate cost
double cost = 0;
for (const auto& trajectory : sub_trajectories) {
if (!trajectory)
continue;
for (const double& distance : trajectory->getWayPointDurations())
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<robot_trajectory::RobotTrajectoryConst
start_ps = end_ps;
}
return std::make_shared<SolutionSequence>(std::move(sub_solutions), cost);
return std::make_shared<SolutionSequence>(std::move(sub_solutions));
}
SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningSceneConstPtr>& 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<SubTrajectory>(sub_trajectories[0], cost);
return std::make_shared<SubTrajectory>(sub_trajectories[0]);
auto jmg = merged_jmg_.get();
assert(jmg);
@ -242,7 +228,7 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
properties().get<moveit_msgs::Constraints>("path_constraints")))
return SubTrajectoryPtr();
return std::make_shared<SubTrajectory>(trajectory, cost);
return std::make_shared<SubTrajectory>(trajectory);
}
} // namespace stages
} // namespace task_constructor

View File

@ -40,6 +40,8 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
#include <rviz_marker_tools/marker_creation.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
@ -53,6 +55,9 @@ namespace task_constructor {
namespace stages {
FixCollisionObjects::FixCollisionObjects(const std::string& name) : PropagatingEitherWay(name) {
// TODO: possibly weight solutions based on the required displacement?
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<double>("max_penetration", "maximally corrected penetration depth");
p.declare<geometry_msgs::Vector3>("direction", "direction vector to use for corrections");

View File

@ -36,7 +36,9 @@
#include <moveit/task_constructor/stages/fixed_cartesian_poses.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
@ -47,6 +49,8 @@ namespace stages {
using PosesList = std::vector<geometry_msgs::PoseStamped>;
FixedCartesianPoses::FixedCartesianPoses(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<PosesList>("poses", PosesList(), "target poses to spawn");
}

View File

@ -35,13 +35,17 @@
/* Authors: Robert Haschke */
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
FixedState::FixedState(const std::string& name) : Generator(name) {}
FixedState::FixedState(const std::string& name) : Generator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
}
void FixedState::setState(const planning_scene::PlanningScenePtr& scene) {
scene_ = scene;

View File

@ -36,6 +36,7 @@
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
@ -45,6 +46,8 @@ namespace task_constructor {
namespace stages {
GeneratePose::GeneratePose(const std::string& name) : MonitoringGenerator(name) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("pose", "target pose to pass on in spawned states");
}

View File

@ -38,13 +38,17 @@
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {}
ModifyPlanningScene::ModifyPlanningScene(const std::string& name) : PropagatingEitherWay(name) {
setCostTerm(std::make_unique<cost::Constant>(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)));

View File

@ -37,6 +37,8 @@
*/
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
@ -47,6 +49,8 @@ namespace stages {
MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("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;

View File

@ -37,6 +37,8 @@
*/
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
@ -48,6 +50,8 @@ namespace stages {
MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& planner)
: PropagatingEitherWay(name), planner_(planner) {
setCostTerm(std::make_unique<cost::PathLength>());
auto& p = properties();
p.property("timeout").setDefaultValue(1.0);
p.declare<std::string>("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();

View File

@ -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 <moveit/task_constructor/stages/passthrough.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <chrono>
#include <functional>
#include <iterator>
#include <ros/console.h>
namespace moveit {
namespace task_constructor {
namespace stages {
PassThrough::PassThrough(const std::string& name, Stage::pointer&& child) : WrapperBase(name, std::move(child)) {}
void PassThrough::onNewSolution(const SolutionBase& s) {
this->liftSolution(s);
}
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -141,6 +141,16 @@ void SolutionBase::setCreator(Stage* creator) {
creator_ = creator;
}
void SolutionBase::setStartStateUnsafe(const InterfaceState& state) {
start_ = &state;
const_cast<InterfaceState&>(state).addOutgoing(this);
}
void SolutionBase::setEndStateUnsafe(const InterfaceState& state) {
end_ = &state;
const_cast<InterfaceState&>(state).addIncoming(this);
}
void SolutionBase::setCost(double cost) {
cost_ = cost;
}
@ -168,6 +178,10 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int
this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
}
double SubTrajectory::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
void SolutionSequence::push_back(const SolutionBase& solution) {
subsolutions_.push_back(&solution);
}
@ -207,6 +221,10 @@ void SolutionSequence::fillMessage(moveit_task_constructor_msgs::Solution& msg,
}
}
double SolutionSequence::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection) const {
wrapped_->fillMessage(solution, introspection);
@ -217,5 +235,10 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
sub_msg.sub_solution_id.push_back(introspection ? introspection->solutionId(*wrapped_) : 0);
solution.sub_solution.insert(solution.sub_solution.begin(), std::move(sub_msg));
}
double WrappedSolution::computeCost(const CostTerm& f, std::string& comment) const {
return f(*this, comment);
}
} // namespace task_constructor
} // namespace moveit

View File

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

View File

@ -0,0 +1,351 @@
#include "models.h"
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stages/passthrough.h>
#include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
const double STAGE_COST{ 7.0 };
const double TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator
{
PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
public:
GeneratorMockup() : Generator("generator") {
prev.reset(new Interface);
next.reset(new Interface);
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
spawn(std::move(state), STAGE_COST);
}
};
class ConnectMockup : public Connecting
{
using Connecting::Connecting;
void compute(const InterfaceState& from, const InterfaceState& to) override {
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(STAGE_COST);
connect(from, to, solution);
}
};
class ForwardMockup : public PropagatingForward
{
PlanningScenePtr ps;
public:
using PropagatingForward::PropagatingForward;
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
PropagatingForward::init(robot_model);
}
void computeForward(const InterfaceState& from) override {
SubTrajectory solution;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
ps->getRobotModel()->getJointModelGroups()[0]) };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
sendForward(from, std::move(to), std::move(solution));
};
};
class BackwardMockup : public PropagatingBackward
{
using PropagatingBackward::PropagatingBackward;
void computeBackward(const InterfaceState& to) override {
SubTrajectory solution;
solution.setCost(STAGE_COST);
InterfaceState from(to);
sendBackward(std::move(from), to, std::move(solution));
};
};
template <typename T>
class Standalone : public T
{
moveit::core::RobotModelConstPtr robot;
InterfacePtr dummy;
planning_scene::PlanningSceneConstPtr ps;
InterfaceStatePtr state_start, state_end;
public:
Standalone(const moveit::core::RobotModelConstPtr& robot)
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
// reset and prepare for a compute step
void prepare() {
auto impl{ this->pimpl() };
this->reset();
state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
// infer and setup interface from children
Stage* s{ this };
ContainerBase* b;
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().front());
InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK };
s = this;
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().back());
InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK };
InterfaceFlags flags{ start_flags | end_flags };
impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr);
impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr);
T::init(robot);
impl->resolveInterface(flags);
// feed interfaces as required for one computation
if (flags & READS_START) {
impl->starts()->add(*state_start);
}
if (flags & READS_END) {
impl->ends()->add(*state_end);
}
}
void runCompute() { this->pimpl()->runCompute(); }
void computeWithStages(std::initializer_list<StageUniquePtr> stages) {
this->clear();
// initializer_list offers only const access (a known shortcoming in C++)
for (const auto& stage : stages) {
this->add(const_cast<StageUniquePtr&&>(stage));
}
prepare();
runCompute();
}
void computeWithStageCost(std::initializer_list<StageUniquePtr> stages, const CostTermConstPtr& cost_term) {
this->setCostTerm(nullptr);
for (const auto& stage : stages)
const_cast<StageUniquePtr&&>(stage)->setCostTerm(cost_term);
computeWithStages(stages);
}
void computeWithContainerCost(std::initializer_list<StageUniquePtr> stages, const CostTermPtr& cost_term) {
this->setCostTerm(cost_term);
computeWithStages(stages);
}
};
TEST(CostTerm, SetLambdaCostTerm) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<GeneratorMockup>() };
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<GeneratorMockup>();
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<GeneratorMockup>();
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<const SolutionSequence>(container.solutions().front());
EXPECT_EQ(sol->cost(), 1.0);
EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment";
}
TEST(CostTerm, CostOverwrite) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
container.computeWithStages({ std::make_unique<GeneratorMockup>() });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost";
}
TEST(CostTerm, StageTypes) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto constant{ std::make_shared<cost::Constant>(1.0) };
// already tested above
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect";
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator";
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator";
}
TEST(CostTerm, PassThroughUsesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant->cost) << "PassThrough forwards children's cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "Child cost equals PassThrough cost";
}
TEST(CostTerm, PassThroughOverwritesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant_inner);
auto constant_outer{ std::make_shared<cost::Constant>(72.0) };
container.setCostTerm(constant_outer);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_outer->cost) << "PassThrough can apply custom cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_inner->cost) << "child's cost is not affected";
}
TEST(CostTerm, PassThroughCanModifyCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(8.0) };
stage->setCostTerm(constant);
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant->cost * constant->cost) << "PassThrough can compute cost based on child";
EXPECT_EQ(wrapped.wrapped()->cost(), constant->cost) << "child's cost is not affected";
}
TEST(CostTerm, CompositeSolutions) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
Standalone<SerialContainer> container{ getModel() };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
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<ForwardMockup>() };
auto constant{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant);
auto s2{ std::make_unique<ForwardMockup>() };
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<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
auto constant1{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant1);
s2->setCostTerm(constant1);
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
auto constant2{ std::make_shared<cost::Constant>(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<SerialContainer> container{ getModel() };
auto s1{ std::make_unique<ForwardMockup>() };
auto s1_ptr{ s1.get() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
container.setCostTerm(std::make_unique<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";
}