mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge PR #183: CostTerm API
This commit is contained in:
commit
e9c56c9c64
174
core/include/moveit/task_constructor/cost_terms.h
Normal file
174
core/include/moveit/task_constructor/cost_terms.h
Normal 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
|
||||
@ -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:
|
||||
|
||||
@ -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_;
|
||||
|
||||
|
||||
61
core/include/moveit/task_constructor/stages/passthrough.h
Normal file
61
core/include/moveit/task_constructor/stages/passthrough.h
Normal 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
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
250
core/src/cost_terms.cpp
Normal 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
|
||||
@ -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_;
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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)));
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
65
core/src/stages/passthrough.cpp
Normal file
65
core/src/stages/passthrough.cpp
Normal 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
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
|
||||
351
core/test/test_cost_terms.cpp
Normal file
351
core/test/test_cost_terms.cpp
Normal 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";
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user