This commit is contained in:
JafarAbdi 2022-05-26 21:42:26 +00:00
commit 11b81856d5
38 changed files with 1277 additions and 408 deletions

View File

@ -39,7 +39,7 @@ jobs:
-e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
-e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
env:
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file

View File

@ -15,7 +15,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v3.4.0
rev: v4.0.1
hooks:
- id: check-added-large-files
- id: check-case-conflict

View File

@ -14,9 +14,9 @@ find_package(rviz_marker_tools REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(visualization_msgs REQUIRED)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
add_compile_options(-fvisibility-inlines-hidden)

View File

@ -150,6 +150,7 @@ public:
void onNewSolution(const SolutionBase& s) override;
};
class FallbacksPrivate;
/** Plan for different alternatives in sequence.
*
* Try to find feasible solutions using first child. Only if this fails,
@ -158,17 +159,23 @@ public:
*/
class Fallbacks : public ParallelContainerBase
{
mutable Stage* active_child_ = nullptr;
inline void replaceImpl();
public:
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
PRIVATE_CLASS(Fallbacks);
Fallbacks(const std::string& name = "fallbacks");
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
private:
// not needed, we directly use corresponding virtual methods of FallbacksPrivate
bool canCompute() const final { return false; }
void compute() final {}
};
class MergerPrivate;

View File

@ -76,7 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
public:
using container_type = StagePrivate::container_type;
@ -131,10 +131,11 @@ public:
inline const auto& externalToInternalMap() const { return internal_external_.by<EXTERNAL>(); }
/// called by a (direct) child when a solution failed
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
virtual void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to);
protected:
ContainerBasePrivate(ContainerBase* me, const std::string& name);
ContainerBasePrivate& operator=(ContainerBasePrivate&& other);
// Set child's push interfaces: allow pushing if child requires it.
inline void setChildsPushBackwardInterface(StagePrivate* child) {
@ -148,14 +149,19 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
/// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction
template <Interface::Direction dir>
void setStatus(const InterfaceState* s, InterfaceState::Status status);
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map
/// Copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level
void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated);
/// Lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to);
@ -228,12 +234,91 @@ protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
private:
/// callback for new externally received states
/// notify callback for new externally received interface states
template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, bool updated);
void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
// override to customize behavior on received interface states (default: propagateStateToAllChildren())
virtual void initializeExternalInterfaces();
};
PIMPL_FUNCTIONS(ParallelContainerBase)
/* The Fallbacks container needs to implement different behaviour based on its interface.
* Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
* FallbacksPrivate is the common base class for all of them, defining the common API
* to be used by the Fallbacks container.
* The actual interface-specific class is instantiated in initializeExternalInterfaces()
* resp. Fallbacks::replaceImpl() when the actual interface is known.
* The key difference between the 3 variants is how they advance to the next job. */
class FallbacksPrivate : public ParallelContainerBasePrivate
{
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other);
void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
// virtual methods specific to each variant
virtual void onNewSolution(const SolutionBase& s);
virtual void reset() {}
};
PIMPL_FUNCTIONS(Fallbacks)
/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
which both have the notion of a currently active child stage */
class FallbacksPrivateCommon : public FallbacksPrivate
{
public:
FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
/// Advance to next child
inline void nextChild();
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() = 0;
void reset() override;
bool canCompute() const override;
void compute() override;
container_type::const_iterator current_; // currently active child
};
/// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{
FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override;
};
/// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{
FallbacksPrivatePropagator(FallbacksPrivate&& old);
void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state
bool job_has_solutions_; // flag indicating whether the current job generated solutions
};
/// Fallbacks implementation for CONNECT interface
struct FallbacksPrivateConnect : FallbacksPrivate
{
FallbacksPrivateConnect(FallbacksPrivate&& old);
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
template <Interface::Direction dir>
void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
mutable container_type::const_iterator active_; // child picked for compute()
};
class WrapperBasePrivate : public ParallelContainerBasePrivate
{
friend class WrapperBase;

View File

@ -39,6 +39,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
namespace moveit {
namespace task_constructor {
@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
*/
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization);
} // namespace task_constructor
} // namespace moveit

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/version.h>
#include <moveit/macros/class_forward.h>
#define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
@ -47,4 +48,11 @@
// use object shape poses relative to a single object pose
#define MOVEIT_HAS_OBJECT_POSE 1
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 0
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK 1
#if !MOVEIT_VERSION_GE(3, 0, 0)
// the pointers are not yet available
namespace trajectory_processing {
MOVEIT_CLASS_FORWARD(TimeParameterization);
}
#endif

View File

@ -61,7 +61,6 @@ class StagePrivate
{
friend class Stage;
friend std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
public:
/// container type used to store children
@ -100,17 +99,11 @@ public:
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
/// direction-based access to pull/push interfaces
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
inline InterfacePtr pushInterface(Interface::Direction dir) {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
inline InterfaceConstPtr pullInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? starts_ : ends_;
}
inline InterfaceConstPtr pushInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
/// direction-based access to pull interface
template <Interface::Direction dir>
inline InterfacePtr pullInterface();
// non-template version
inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
/// set parent of stage
/// enforce only one parent exists
@ -165,6 +158,8 @@ public:
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
protected:
StagePrivate& operator=(StagePrivate&& other);
// associated/owning Stage instance
Stage* me_;
@ -205,6 +200,15 @@ private:
PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::FORWARD>() {
return starts_;
}
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::BACKWARD>() {
return ends_;
}
template <>
inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
const SolutionBasePtr& solution) {
@ -301,6 +305,7 @@ PIMPL_FUNCTIONS(MonitoringGenerator)
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
friend struct FallbacksPrivateConnect;
public:
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
@ -311,18 +316,15 @@ public:
}
static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number
unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
bool lhs = lhsA.enabled() && lhsB.enabled();
bool rhs = rhsA.enabled() && rhsB.enabled();
if (lhs == rhs) // if enabled status is identical
return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions
// one of the states in each pair should be enabled
assert(lhs != 0b00 && rhs != 0b00);
// both states valid (b11)
if (lhs == 0b11)
return true;
if (rhs == 0b11)
return false;
return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component
// sort both-enabled pairs first
static_assert(true > false, "Comparing enabled states requires true > false");
return lhs > rhs;
}
};
@ -334,7 +336,7 @@ public:
// Check whether there are pending feasible states that could connect to source
template <Interface::Direction dir>
bool hasPendingOpposites(const InterfaceState* source) const;
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
@ -343,9 +345,9 @@ private:
template <Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
// get informed when new start or end state becomes available
// notify callback to get informed about newly inserted (or updated) start or end states
template <Interface::Direction other>
void newState(Interface::iterator it, bool updated);
void newState(Interface::iterator it, Interface::UpdateFlags updated);
// ordered list of pending state pairs
ordered<StatePair> pending;

View File

@ -44,6 +44,7 @@
#include <moveit/task_constructor/cost_queue.h>
#include <moveit_task_constructor_msgs/msg/solution.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <moveit/task_constructor/utils.h>
#include <list>
#include <vector>
@ -83,9 +84,11 @@ public:
enum Status
{
ENABLED, // state is actively considered during planning
PRUNED, // state is disabled because a required connected state failed
FAILED, // state that failed, causing the whole partial solution to be disabled
ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state
PRUNED, // disabled state on a pruned solution branch
};
static const char* STATUS_COLOR[];
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
@ -101,8 +104,6 @@ public:
inline Status status() const { return std::get<0>(*this); }
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
inline bool failed() const { return std::get<0>(*this) == FAILED; }
inline bool pruned() const { return std::get<0>(*this) == PRUNED; }
inline unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); }
@ -129,6 +130,7 @@ public:
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);
InterfaceState(InterfaceState&& other) = default;
InterfaceState& operator=(const InterfaceState& other) = default;
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
@ -139,13 +141,21 @@ public:
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
inline const Priority& priority() const { return priority_; }
/// Update priority and call owner's notify() if possible
void updatePriority(const InterfaceState::Priority& priority);
/// Update status, but keep current priority
void updateStatus(Status status);
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
// Set new priority without updating the owning interface (USE WITH CARE)
inline void setPriority(const Priority& prio) { priority_ = prio; }
private:
planning_scene::PlanningSceneConstPtr scene_;
@ -170,6 +180,7 @@ public:
class iterator : public base_type::iterator
{
public:
iterator() = default;
iterator(base_type::iterator other) : base_type::iterator(other) {}
InterfaceState& operator*() const noexcept { return *base_type::iterator::operator*(); }
@ -191,10 +202,27 @@ public:
{
FORWARD,
BACKWARD,
START = FORWARD,
END = BACKWARD
};
using NotifyFunction = std::function<void(iterator, bool)>;
enum Update
{
STATUS = 1 << 0,
PRIORITY = 1 << 1,
ALL = STATUS | PRIORITY,
};
using UpdateFlags = utils::Flags<Update>;
using NotifyFunction = std::function<void(iterator, UpdateFlags)>;
class DisableNotify
{
Interface& if_;
Interface::NotifyFunction old_;
public:
DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
~DisableNotify() { old_.swap(if_.notify_); }
};
friend class DisableNotify;
Interface(const NotifyFunction& notify = NotifyFunction());
/// add a new InterfaceState
@ -205,9 +233,10 @@ public:
/// update state's priority (and call notify_ if it really has changed)
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
private:
const NotifyFunction notify_;
NotifyFunction notify_;
// restrict access to some functions to ensure consistency
// (we need to set/unset InterfaceState::owner_)
@ -220,6 +249,17 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
std::ostream& operator<<(std::ostream& os, Interface::Direction);
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T>
size_t getIndex(const T& container, typename T::const_iterator search) {
size_t index = 1;
for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
if (it == search)
return index;
return 0;
}
class CostTerm;
class StagePrivate;

View File

@ -47,6 +47,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <moveit/utils/moveit_error_code.h>
#include <rclcpp/node.hpp>
@ -119,11 +120,11 @@ public:
void init();
/// reset, init scene (if not yet done), and init all stages, then start planning
bool plan(size_t max_solutions = 0);
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
/// interrupt current planning (or execution)
void preempt();
/// execute solution, return the result
moveit_msgs::msg::MoveItErrorCodes execute(const SolutionBase& s);
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const;

View File

@ -51,15 +51,14 @@ namespace task_constructor {
class TaskPrivate : public WrapperBasePrivate
{
friend class Task;
TaskPrivate& operator=(TaskPrivate&& other);
public:
TaskPrivate(Task* me, const std::string& ns);
const std::string& ns() const { return ns_; }
const ContainerBase* stages() const;
protected:
static void swap(StagePrivate*& lhs, StagePrivate*& rhs);
private:
std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;

View File

@ -140,8 +140,6 @@ private:
Int i;
};
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame);

View File

@ -37,8 +37,11 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <memory>
@ -49,16 +52,70 @@
#include <functional>
using namespace std::placeholders;
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
conn->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
, pending_backward_(new Interface)
, pending_forward_(new Interface) {}
ContainerBasePrivate& ContainerBasePrivate::operator=(ContainerBasePrivate&& other) {
assert(internal_external_.empty() && other.internal_external_.empty());
// move StagePrivate members
this->StagePrivate::operator=(std::move(other));
// swapping of container members needed to maintain valid pending_* interfaces
// and children (e.g. for TaskPrivate)
required_interface_ = other.required_interface_;
std::swap(pending_backward_, other.pending_backward_);
std::swap(pending_forward_, other.pending_forward_);
std::swap(children_, other.children_);
// redirect all children's parent pointers to the new parent
auto reparent_children = [](ContainerBasePrivate& self) {
for (auto it = self.children_.begin(), end = self.children_.end(); it != end; ++it) {
auto cimpl = (*it)->pimpl();
cimpl->unparent();
cimpl->setParent(static_cast<ContainerBase*>(self.me_));
cimpl->setParentPosition(it);
}
};
reparent_children(*this);
reparent_children(other);
return *this;
}
ContainerBasePrivate::const_iterator ContainerBasePrivate::childByIndex(int index, bool for_insert) const {
if (!for_insert && index < 0)
--index;
@ -106,6 +163,74 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute();
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status) {
if (status != InterfaceState::Status::ENABLED && creator) {
if (const auto* conn = dynamic_cast<const Connecting*>(creator)) {
auto cimpl = conn->pimpl();
// if creator is a Connecting stage and target has enabled opposite states (other than source)
if (cimpl->hasPendingOpposites<dir>(source, target))
return; // don't prune
}
}
if (target->priority().status() == status)
return; // nothing changing
// Skip disabling the state, if there are alternative enabled solutions
if (status != InterfaceState::ENABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*target);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
const_cast<InterfaceState*>(target)->updateStatus(status);
// if possible (i.e. if target has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*target).empty()) {
// TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers
auto external{ internalToExternalMap().find(target) };
if (external != internalToExternalMap().end()) { // do we have an external state?
// only escalate if there is no other *enabled* internal state connected to the same external one
// all internal states linked to external
auto internals{ externalToInternalMap().equal_range(external->get<EXTERNAL>()) };
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(nullptr, nullptr, external->get<EXTERNAL>(), status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with ARMED and all other states down the tree with PRUNED.
// This allows us to re-enable the ARMED state, but not the PRUNED states,
// when new states arrive in a Connecting stage.
// For details, https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
if (status == InterfaceState::Status::ARMED)
status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*target))
setStatus<dir>(successor->creator(), target, state<dir>(*successor), status);
}
// recursively update state priorities along solution path
template <Interface::Direction dir>
inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) {
InterfaceState::Priority priority(prio, s.priority().status());
if (s.priority() == priority)
return;
const_cast<InterfaceState&>(s).updatePriority(priority);
for (const SolutionBase* successor : trajectories<dir>(s))
updateStatePrios<dir>(*state<dir>(*successor), prio);
}
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "'" << child.name() << "' generated a failure");
switch (child.pimpl()->interfaceFlags()) {
@ -115,95 +240,37 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::PRUNED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::PRUNED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl();
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) {
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
}
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) {
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Pruning"), "prune forward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
}
}
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::ARMED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::ARMED);
break;
}
// printChildrenInterfaces(*this, false, child);
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
if (s->priority().status() == status)
return; // nothing changing
// if we should disable the state, only do so when there is no enabled alternative path
if (status == InterfaceState::PRUNED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*s);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
if (s->owner()) {
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*s).empty()) {
auto external{ internalToExternalMap().find(s) };
if (external != internalToExternalMap().end()) { // do we have an external state?
// only escalate if there is no other *enabled* internal state connected to the same external one
// all internal states linked to external
auto internals{ externalToInternalMap().equal_range(external->get<EXTERNAL>()) };
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(external->get<EXTERNAL>(), status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with FAILED and all other states down the tree only with PRUNED.
// This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again,
// when new states arrive in a Connecting stage.
// All PRUNED states are only re-enabled if the FAILED state actually gets connected.
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
if (status == InterfaceState::Status::FAILED)
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status);
}
template void ContainerBasePrivate::setStatus<Interface::FORWARD>(const InterfaceState*, InterfaceState::Status);
template void ContainerBasePrivate::setStatus<Interface::BACKWARD>(const InterfaceState*, InterfaceState::Status);
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated) {
if (updated) {
auto internals{ externalToInternalMap().equal_range(&*external) };
for (auto& i = internals.first; i != internals.second; ++i) {
setStatus<dir>(i->second, external->priority().status());
}
auto prio = external->priority();
auto internals = externalToInternalMap().equal_range(&*external);
if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies
for (auto& i = internals.first; i != internals.second; ++i)
setStatus<dir>(nullptr, nullptr, i->second, prio.status());
} else if (updated.testFlag(Interface::Update::PRIORITY)) {
for (auto& i = internals.first; i != internals.second; ++i)
updateStatePrios<opposite<dir>()>(*i->second, prio);
} else
assert(false); // Expecting either STATUS or PRIORITY updates, not both!
return;
}
// create a clone of external state within target interface (child's starts() or ends())
auto internal = states_.insert(states_.end(), InterfaceState(*external));
target->add(*internal);
@ -211,6 +278,14 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
internalToExternalMap().insert(std::make_pair(&*internal, &*external));
}
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated) {
if (dir == Interface::FORWARD)
copyState<Interface::FORWARD>(external, target, updated);
else
copyState<Interface::BACKWARD>(external, target, updated);
}
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to) {
computeCost(*internal_from, *internal_to, *solution);
@ -374,31 +449,6 @@ std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
return os;
}
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const Connecting* conn = dynamic_cast<const Connecting*>(&creator))
conn->pimpl()->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.pimpl()->children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
/** Collect all partial solution sequences originating from start into given direction */
template <Interface::Direction dir>
struct SolutionCollector
@ -431,20 +481,6 @@ struct SolutionCollector
SolutionSequence::container_type trace;
};
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
// update depth + cost, but keep current status
InterfaceState::Priority(prio, state->priority().status()));
}
template <Interface::Direction dir>
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
const InterfaceState::Priority& prio) {
for (const SolutionBase* solution : partial_solution_path)
updateStatePrio(state<dir>(*solution), prio);
}
void SerialContainer::onNewSolution(const SolutionBase& current) {
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("SerialContainer"), "'" << this->name()
<< "' received solution of child stage '"
@ -453,10 +489,6 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
// failures should never trigger this callback
assert(!current.isFailure());
// states of solution must be active, otherwise this would not have been computed
assert(current.start()->priority().enabled());
assert(current.end()->priority().enabled());
auto impl = pimpl();
const Stage* creator = current.creator();
auto& children = impl->children();
@ -494,14 +526,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStatePrio(current.start(), prio);
updateStatePrio(current.end(), prio);
updateStatePrios<Interface::BACKWARD>(in.first, prio);
updateStatePrios<Interface::FORWARD>(out.first, prio);
updateStatePrios<Interface::BACKWARD>(*current.start(), prio);
updateStatePrios<Interface::FORWARD>(*current.end(), prio);
}
}
}
// printChildrenInterfaces(*this, true, *current.creator());
// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
// finally, store + announce new solutions to external interface
for (const auto& solution : sorted)
@ -566,9 +596,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
starts_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::FORWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -579,6 +609,7 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
child_impl = (**it).pimpl(); // re-assign as pimpl_ pointer of a Fallback container will change!
connect(*previous_impl, *child_impl);
} catch (InitStageException& e) {
exceptions.append(e);
@ -591,9 +622,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
ends_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::BACKWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -649,8 +680,7 @@ bool SerialContainer::canCompute() const {
void SerialContainer::compute() {
for (const auto& stage : pimpl()->children()) {
if (!stage->pimpl()->canCompute())
continue;
if (stage->pimpl()->canCompute())
stage->pimpl()->runCompute();
}
}
@ -672,8 +702,8 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
child_impl->resolveInterface(expected);
validateInterfaces(*child_impl, expected, first);
// initialize push connections of children according to their demands
setChildsPushForwardInterface(child_impl);
setChildsPushBackwardInterface(child_impl);
setChildsPushForwardInterface(child_impl);
first = false;
} catch (InitStageException& e) {
exceptions.append(e);
@ -684,17 +714,21 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
if (exceptions)
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::FORWARD>(external, updated);
}));
if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState<Interface::BACKWARD>(external, updated);
}));
required_interface_ = expected;
initializeExternalInterfaces();
}
void ParallelContainerBasePrivate::initializeExternalInterfaces() {
// States received by the container need to be copied to all children's pull interfaces.
if (requiredInterface() & READS_START)
starts() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::FORWARD>(external, updated);
});
if (requiredInterface() & READS_END)
ends() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::BACKWARD>(external, updated);
});
}
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@ -729,9 +763,9 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
}
template <Interface::Direction dir>
void ParallelContainerBasePrivate::onNewExternalState(Interface::iterator external, bool updated) {
void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated) {
for (const Stage::pointer& stage : children())
copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
}
ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}
@ -802,44 +836,240 @@ void Alternatives::onNewSolution(const SolutionBase& s) {
liftSolution(s);
}
Fallbacks::Fallbacks(const std::string& name) : Fallbacks(new FallbacksPrivate(this, name)) {}
Fallbacks::Fallbacks(FallbacksPrivate* impl) : ParallelContainerBase(impl) {}
void Fallbacks::reset() {
active_child_ = nullptr;
ParallelContainerBase::reset();
pimpl()->reset();
}
void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
ParallelContainerBase::init(robot_model);
active_child_ = pimpl()->children().front().get();
}
bool Fallbacks::canCompute() const {
while (active_child_) {
StagePrivate* child = active_child_->pimpl();
if (child->canCompute())
return true;
// active child failed, continue with next
auto next = child->it();
++next;
if (next == pimpl()->children().end())
active_child_ = nullptr;
else
active_child_ = next->get();
}
return false;
}
void Fallbacks::compute() {
if (!active_child_)
return;
active_child_->pimpl()->runCompute();
pimpl()->reset();
}
void Fallbacks::onNewSolution(const SolutionBase& s) {
liftSolution(s);
pimpl()->onNewSolution(s);
}
inline void Fallbacks::replaceImpl() {
FallbacksPrivate *impl = pimpl();
switch (pimpl()->requiredInterface()) {
case GENERATE:
impl = new FallbacksPrivateGenerator(std::move(*impl));
break;
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:
impl = new FallbacksPrivatePropagator(std::move(*impl));
break;
case CONNECT:
// For now, we only support Connecting children
for (const auto& child : impl->children())
if (!dynamic_cast<Connecting*>(child.get()))
throw std::runtime_error("CONNECT-like interface is only supported for Connecting children");
impl = new FallbacksPrivateConnect(std::move(*impl));
break;
}
delete pimpl_;
pimpl_ = impl;
}
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
: ParallelContainerBasePrivate(me, name) {}
FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
// move contents of other
this->ParallelContainerBasePrivate::operator=(std::move(other));
}
void FallbacksPrivate::initializeExternalInterfaces() {
// Here we know the final interface of the container (and all its children)
// Thus replace, this pimpl() with a new interface-specific one:
static_cast<Fallbacks*>(me())->replaceImpl();
}
void FallbacksPrivate::onNewSolution(const SolutionBase& s) {
// printChildrenInterfaces(*this, true, *s.creator());
static_cast<Fallbacks*>(me())->liftSolution(s);
}
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
// This override is deliberately empty.
// The method prunes solution paths when a child failed to find a valid solution for it,
// but in Fallbacks the next child might still yield a successful solution
// Thus pruning must only occur once the last child is exhausted (inside computePropagate)
// printChildrenInterfaces(*this, false, child);
(void)child;
}
void FallbacksPrivateCommon::reset() {
current_ = children().begin();
}
bool FallbacksPrivateCommon::canCompute() const {
while(current_ != children().end() && // not completely exhausted
!(*current_)->pimpl()->canCompute()) // but current child cannot compute
return const_cast<FallbacksPrivateCommon*>(this)->nextJob(); // advance to next job
// return value: current child is well defined and thus can compute?
return current_ != children().end();
}
void FallbacksPrivateCommon::compute() {
(*current_)->pimpl()->runCompute();
}
inline void FallbacksPrivateCommon::nextChild() {
if (std::next(current_) != children().end())
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("Fallbacks"), "Child '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child
}
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
: FallbacksPrivateCommon(std::move(old)) { FallbacksPrivateCommon::reset(); }
bool FallbacksPrivateGenerator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
// don't advance to next child when we already produced solutions
if (!solutions_.empty()) {
current_ = children().end(); // indicate that we are exhausted
return false;
}
do { nextChild(); }
while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
// return value shall indicate current_->canCompute()
return current_ != children().end();
}
FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
: FallbacksPrivateCommon(std::move(old)) {
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD;
starts() = std::make_shared<Interface>();
break;
case PROPAGATE_BACKWARDS:
dir_ = Interface::BACKWARD;
ends() = std::make_shared<Interface>();
break;
default:
assert(false);
}
FallbacksPrivatePropagator::reset();
}
void FallbacksPrivatePropagator::reset() {
FallbacksPrivateCommon::reset();
job_ = pullInterface(dir_)->end(); // indicate fresh start
job_has_solutions_ = false;
}
void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) {
job_has_solutions_ = true;
FallbacksPrivateCommon::onNewSolution(s);
}
bool FallbacksPrivatePropagator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
const auto jobs = pullInterface(dir_);
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!job_has_solutions_) // job didn't produce solutions -> feed to next child
nextChild();
else
current_ = children().end(); // indicate that this job is exhausted on all children
}
job_has_solutions_ = false;
if (current_ == children().end()) { // all children processed the job_
if (job_ != jobs->end()) {
jobs->remove(job_); // we don't need the job in our interface list anymore
job_ = jobs->end(); // indicate that we need to fetch a new job
}
current_ = children().begin(); // start next job with first child again
}
// pick next job if needed and possible
if (job_ == jobs->end()) { // need to pick next job
if (!jobs->empty() && jobs->front()->priority().enabled())
job_ = jobs->begin();
else
return false; // no more jobs available
}
// When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that.
copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags());
return true;
}
FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) {
starts_ = std::make_shared<Interface>(
std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::BACKWARD>, this, std::placeholders::_1, std::placeholders::_2));
FallbacksPrivateConnect::reset();
}
void FallbacksPrivateConnect::reset() {
active_ = children().end();
}
template <Interface::Direction dir>
void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) {
copyState<dir>(external, children().front()->pimpl()->pullInterface(dir), updated);
// As we use the Interface* from the first child for all children (we just populate their pending lists)
// there is no need to explicitly propagate state updates to other children.
}
bool FallbacksPrivateConnect::canCompute() const {
for (auto it=children().begin(), end=children().end(); it!=end; ++it)
if ((*it)->pimpl()->canCompute()) {
active_ = it;
return true;
}
active_ = children().end();
return false;
}
void FallbacksPrivateConnect::compute() {
// Alternatively, we could also compute() all children that canCompute()
assert(active_ != children().end());
(*active_)->pimpl()->runCompute();
}
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// expect failure to be reported from active child
assert(active_ != children().end() && active_->get() == &child);
// ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_);
auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
auto it = std::find(interface.begin(), interface.end(), state);
assert(it != interface.end());
return it;
};
if (next != children().end()) { // pass job to next child
auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
auto fromIt = findIteratorFor(from, *first_con->starts());
auto toIt = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(fromIt, toIt));
} else // or report failure to parent
parent()->pimpl()->onNewFailure(*me(), from, to);
}
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
@ -859,7 +1089,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) {
}
}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {
properties().declare<TimeParameterizationPtr>("time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Merger::reset() {
ParallelContainerBase::reset();
@ -1012,7 +1245,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
auto timing = me_->properties().get<TimeParameterizationPtr>("time_parameterization");
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing);
} catch (const std::runtime_error& e) {
SubTrajectory t;
t.markAsFailure();

View File

@ -36,7 +36,6 @@
/* Authors: Luca Lach, Robert Haschke */
#include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/algorithm/string/join.hpp>
@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization) {
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
@ -166,8 +166,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
}
// add timing
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0);
return merged_traj;
}
} // namespace task_constructor

View File

@ -40,9 +40,11 @@
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/robot_state/cartesian_interpolator.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
namespace solvers {
@ -101,8 +103,8 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return achieved_fraction >= props.get<double>("min_fraction");

View File

@ -37,8 +37,9 @@
*/
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <chrono>
@ -48,6 +49,8 @@ namespace solvers {
static const auto LOGGER = rclcpp::get_logger("JointInterpolationPlanner");
using namespace trajectory_processing;
JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
@ -91,9 +94,8 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
if (from->isStateColliding(to_state, jmg->getName()))
return false;
// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return true;

View File

@ -141,6 +141,7 @@ void PipelinePlanner::init(const core::RobotModelConstPtr& robot_model) {
Specification spec;
spec.model = robot_model;
spec.pipeline = pipeline_name_;
spec.ns = pipeline_name_;
planner_ = create(node_, spec);
} else if (robot_model != planner_->getRobotModel()) {
throw std::runtime_error(

View File

@ -37,6 +37,10 @@
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -46,6 +50,7 @@ PlannerInterface::PlannerInterface() {
auto& p = properties();
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
}
} // namespace solvers
} // namespace task_constructor

View File

@ -106,6 +106,33 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
, parent_{ nullptr }
, introspection_{ nullptr } {}
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
assert(typeid(*this) == typeid(other));
assert(states_.empty() && other.states_.empty());
assert((!starts_ || starts_->empty()) && (!other.starts_ || other.starts_->empty()));
assert((!ends_ || ends_->empty()) && (!other.ends_ || other.ends_->empty()));
assert(solutions_.empty() && other.solutions_.empty());
assert(failures_.empty() && other.failures_.empty());
// me_ must not be changed!
name_ = std::move(other.name_);
properties_ = std::move(other.properties_);
cost_term_ = std::move(other.cost_term_);
solution_cbs_ = std::move(other.solution_cbs_);
starts_ = std::move(other.starts_);
ends_ = std::move(other.ends_);
prev_ends_ = std::move(other.prev_ends_);
next_starts_ = std::move(other.next_starts_);
parent_ = std::move(other.parent_);
it_ = std::move(other.it_);
other.unparent();
return *this;
}
InterfaceFlags StagePrivate::interfaceFlags() const {
InterfaceFlags f;
if (starts())
@ -316,6 +343,7 @@ void Stage::reset() {
impl->next_starts_.reset();
// reset inherited properties
impl->properties_.reset();
impl->total_compute_time_ = std::chrono::duration<double>::zero();
}
void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
@ -483,14 +511,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
case PropagatingEitherWay::FORWARD:
required_interface_ = PROPAGATE_FORWARDS;
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface());
starts_ = std::make_shared<Interface>();
ends_.reset();
return;
case PropagatingEitherWay::BACKWARD:
required_interface_ = PROPAGATE_BACKWARDS;
starts_.reset();
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface());
ends_ = std::make_shared<Interface>();
return;
case PropagatingEitherWay::AUTO:
required_interface_ = UNKNOWN;
@ -688,10 +716,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
}
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
starts_ = std::make_shared<Interface>(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this,
std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
@ -709,56 +737,108 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
return StatePair(second, first);
}
template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
if (updated) { // many pairs might be affected: resort
if (it->priority().pruned())
// remove all pending pairs involving this state
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
else
// TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right?
template <Interface::Direction dir>
void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) {
auto parent_pimpl = parent()->pimpl();
// disable current interface to break loop (jumping back and forth between both interfaces)
// this will be checked by notifyEnabled() below
Interface::DisableNotify disable_source_interface(*pullInterface<dir>());
if (updated) {
if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed
pullInterface<opposite<dir>()>()->notifyEnabled()) // suppressing recursive loop?
{
// If status has changed, propagate the update to the opposite side
auto status = it->priority().status();
if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side
status = InterfaceState::Status::ARMED; // (only for pending state pairs)
for (const auto& candidate : this->pending) {
if (std::get<opposite<dir>()>(candidate) != it) // only consider pairs with source state == state
continue;
auto oit = std::get<dir>(candidate); // opposite target state
auto ostatus = oit->priority().status();
if (ostatus != status) {
if (status != InterfaceState::Status::ENABLED) {
// quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative
bool cancel = false; // if found, cancel propagation of new status
for (const auto alternative : *it->owner())
if ((cancel = alternative->priority().enabled()))
break;
if (cancel)
continue;
}
// pass creator=nullptr to skip hasPendingOpposites() check
parent_pimpl->setStatus<opposite<dir>()>(nullptr, nullptr, &*oit, status);
}
}
}
// many pairs will have changed priorities: resort pending list
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface(other);
InterfacePtr other_interface = pullInterface<dir>();
bool have_enabled_opposites = false;
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
// Don't re-enable states that are marked DISABLED
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) {
// re-enable the opposing state oit if its status is FAILED
if (oit->priority().status() == InterfaceState::Status::FAILED)
oit->owner()->updatePriority(&*oit,
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
pending.insert(make_pair<other>(it, oit));
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
// https://github.com/ros-planning/moveit_task_constructor/pull/309#issuecomment-974636202
if (oit->priority().status() == InterfaceState::Status::ARMED)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (oit->priority().enabled())
have_enabled_opposites = true;
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
if (!have_enabled_opposites) // prune new state and associated branch if necessary
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
}
#if 0
auto& os = std::cerr;
for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
if (d == Interface::FORWARD)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << std::endl;
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);
// std::cerr << std::endl;
os << std::setw(15) << " ";
printPendingPairs(os) << std::endl;
#endif
}
// Check whether there are pending feasible states that could connect to source.
// If not, we exhausted all solution candidates for source and thus should mark it as failure.
// Check whether there are pending feasible states (other than source) that could connect to target.
// If not, we exhausted all solution candidates for target and thus should mark it as failure.
template <Interface::Direction dir>
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const {
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const {
for (const auto& candidate : this->pending) {
static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto src = std::get<dir>(candidate);
static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto tgt = std::get<opposite<dir>()>(candidate);
static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1,
"This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const InterfaceState* src = &*std::get<dir>(candidate);
const InterfaceState* tgt = &*std::get<opposite<dir>()>(candidate);
if (&*src == source && tgt->priority().enabled())
if (tgt == target && src != source && src->priority().enabled())
return true;
// early stopping when only infeasible pairs are to come
if (!std::get<0>(candidate)->priority().enabled())
if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled())
break;
}
return false;
}
// explicitly instantiate templates for both directions
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* start,
const InterfaceState* end) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* end,
const InterfaceState* start) const;
bool ConnectingPrivate::canCompute() const {
// Do we still have feasible pending state pairs?
@ -775,24 +855,15 @@ void ConnectingPrivate::compute() {
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
static const char* red = "\033[31m";
static const char* reset = "\033[m";
const char* reset = InterfaceState::STATUS_COLOR[3];
for (const auto& candidate : pending) {
if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled())
os << " " << red;
// find indeces of InterfaceState pointers in start/end Interfaces
unsigned int first = 0, second = 0;
std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) {
++first;
return &*candidate.first == s;
});
std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) {
++second;
return &*candidate.second == s;
});
os << first << ":" << second << " ";
size_t first = getIndex(*starts(), candidate.first);
size_t second = getIndex(*ends(), candidate.second);
os << InterfaceState::STATUS_COLOR[candidate.first->priority().status()] << first << reset << ":"
<< InterfaceState::STATUS_COLOR[candidate.second->priority().status()] << second << reset << " ";
}
os << reset;
if (pending.empty())
os << "---";
return os;
}
static const rclcpp::Logger LOGGER = rclcpp::get_logger("Connecting");

View File

@ -311,24 +311,26 @@ void ComputeIK::compute() {
bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
// markers used for failures
std::deque<visualization_msgs::msg::Marker> failure_markers;
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
std::deque<visualization_msgs::msg::Marker> frame_markers;
rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame");
// end-effector markers
std::deque<visualization_msgs::msg::Marker> eef_markers;
// visualize placed end-effector
auto appender = [&failure_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) {
auto appender = [&eef_markers](visualization_msgs::msg::Marker& marker, const std::string& /*name*/) {
marker.ns = "ik target";
marker.color.a *= 0.5;
failure_markers.push_back(marker);
eef_markers.push_back(marker);
};
const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()
->getDescendantLinkModels();
if (colliding) {
SubTrajectory solution;
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure();
// TODO: visualize collisions
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
@ -389,10 +391,7 @@ void ComputeIK::compute() {
planning_scene::PlanningScenePtr solution_scene = scene->diff();
SubTrajectory solution;
solution.setComment(s.comment());
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
if (succeeded && i + 1 == ik_solutions.size())
// compute cost as distance to compare_pose
@ -407,6 +406,10 @@ void ComputeIK::compute() {
InterfaceState state(solution_scene);
forwardProperties(*s.start(), state);
// ik target link placement
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(std::move(state), std::move(solution));
}
@ -423,9 +426,17 @@ void ComputeIK::compute() {
solution.markAsFailure();
solution.setComment(s.comment() + " no IK found");
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
// ik target link placement
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std_msgs::msg::ColorRGBA tint_color;
tint_color.r = 1.0;
tint_color.g = 0.0;
tint_color.b = 0.0;
tint_color.a = 0.5;
for (auto& marker : eef_markers)
marker.color = tint_color;
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(InterfaceState(scene), std::move(solution));
}

View File

@ -38,10 +38,14 @@
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
namespace stages {
@ -56,6 +60,8 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
"constraints to maintain during trajectory");
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Connect::reset() {
@ -221,7 +227,8 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
if (!trajectory)
return SubTrajectoryPtr();

View File

@ -45,6 +45,7 @@ namespace stages {
FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene)
: Generator(name), scene_(scene) {
properties().declare("ignore_collisions", false);
setCostTerm(std::make_unique<cost::Constant>(0.0));
}
@ -62,7 +63,12 @@ bool FixedState::canCompute() const {
}
void FixedState::compute() {
spawn(InterfaceState(scene_), 0.0);
SubTrajectory trajectory;
if (!properties().get<bool>("ignore_collisions") && scene_->isStateColliding()) {
trajectory.markAsFailure("in collision");
}
spawn(InterfaceState(scene_), std::move(trajectory));
ran_ = true;
}
} // namespace stages

View File

@ -61,6 +61,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam
auto& p = properties();
p.declare<std::string>("object");
p.declare<geometry_msgs::msg::PoseStamped>("ik_frame");
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
}
void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
@ -116,7 +117,7 @@ void GeneratePlacePose::compute() {
// spawn the nominal target object pose, considering flip about z and rotations about z-axis
auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips,
uint z_rotations = 10) {
for (uint flip = 0; flip < z_flips; ++flip) {
for (uint flip = 0; flip <= z_flips; ++flip) {
// flip about object's x-axis
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
for (uint i = 0; i < z_rotations; ++i) {
@ -144,20 +145,21 @@ void GeneratePlacePose::compute() {
}
};
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
if (object->getShapes().size() == 1) {
switch (object->getShapes()[0]->type) {
case shapes::CYLINDER:
spawner(target_pose, 2);
spawner(target_pose, z_flips);
return;
case shapes::BOX: { // consider 180/90 degree rotations about z axis
const double* dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size;
spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
return;
}
case shapes::SPHERE: // keep original orientation and rotate about world's z
target_pose.linear() = orig_object_pose.linear();
spawner(target_pose, 1);
spawner(target_pose, z_flips);
return;
default:
break;

View File

@ -249,9 +249,9 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// compute absolute transform for link
linear = frame_pose.linear() * linear;
angular = frame_pose.linear() * angular;
target_eigen = link_pose;
target_eigen = ik_pose_world;
target_eigen.linear() =
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular);
target_eigen.linear() * Eigen::AngleAxisd(angular_norm, ik_pose_world.linear().transpose() * angular);
target_eigen.translation() += linear;
goto COMPUTE;
} catch (const boost::bad_any_cast&) { /* continue with Vector */
@ -276,7 +276,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
// compute absolute transform for link
linear = frame_pose.linear() * linear;
target_eigen = link_pose;
target_eigen = ik_pose_world;
target_eigen.translation() += linear;
} catch (const boost::bad_any_cast&) {
solution.markAsFailure(std::string("invalid direction type: ") + direction.type().name());
@ -285,7 +285,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
COMPUTE:
// transform target pose such that ik frame will reach there if link does
target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
target_eigen = target_eigen * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);

View File

@ -84,6 +84,21 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
return cost() < other.cost();
}
void InterfaceState::updatePriority(const InterfaceState::Priority& priority) {
// Never overwrite ARMED with PRUNED
if (priority.status() == InterfaceState::Status::PRUNED && priority_.status() == InterfaceState::Status::ARMED)
return;
if (owner()) {
owner()->updatePriority(this, priority);
} else {
setPriority(priority);
}
}
void InterfaceState::updateStatus(Status status) {
updatePriority(InterfaceState::Priority(priority_, status));
}
Interface::Interface(const Interface::NotifyFunction& notify) : notify_(notify) {}
// Announce a new InterfaceState
@ -117,7 +132,7 @@ void Interface::add(InterfaceState& state) {
moveFrom(it, container);
// and finally call notify callback
if (notify_)
notify_(it, false);
notify_(it, UpdateFlags());
}
Interface::container_type Interface::remove(iterator it) {
@ -128,15 +143,23 @@ Interface::container_type Interface::remove(iterator it) {
}
void Interface::updatePriority(InterfaceState* state, const InterfaceState::Priority& priority) {
if (priority == state->priority())
const auto old_prio = state->priority();
if (priority == old_prio)
return; // nothing to do
auto it = std::find(begin(), end(), state); // find iterator to state
assert(it != end()); // state should be part of this interface
state->priority_ = priority; // update priority
update(it); // update position in ordered list
if (notify_)
notify_(it, true); // notify callback
if (notify_) {
UpdateFlags updated(Update::ALL);
if (old_prio.status() == priority.status())
updated &= ~STATUS;
notify_(it, updated); // notify callback
}
}
std::ostream& operator<<(std::ostream& os, const Interface& interface) {
@ -146,15 +169,20 @@ std::ostream& operator<<(std::ostream& os, const Interface& interface) {
os << istate->priority() << " ";
return os;
}
const char* InterfaceState::STATUS_COLOR[] = {
"\033[32m", // ENABLED - green
"\033[33m", // ARMED - yellow
"\033[31m", // PRUNED - red
"\033[m" // reset
};
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio) {
// maps InterfaceState::Status values to output (color-changing) prefix
static const char* prefix[] = {
"\033[32me:", // ENABLED - green
"\033[33md:", // PRUNED - yellow
"\033[31mf:", // FAILED - red
};
static const char* color_reset = "\033[m";
os << prefix[prio.status()] << prio.depth() << ":" << prio.cost() << color_reset;
os << InterfaceState::STATUS_COLOR[prio.status()] << prio.depth() << ":" << prio.cost()
<< InterfaceState::STATUS_COLOR[3];
return os;
}
std::ostream& operator<<(std::ostream& os, Interface::Direction dir) {
os << (dir == Interface::FORWARD ? "" : "");
return os;
}
@ -169,8 +197,13 @@ void SolutionBase::setCost(double cost) {
void SolutionBase::markAsFailure(const std::string& msg) {
setCost(std::numeric_limits<double>::infinity());
if (!msg.empty())
setComment(msg + "\n" + comment());
if (!msg.empty()) {
std::stringstream ss;
ss << msg;
if (!comment().empty())
ss << "\n" << comment();
setComment(ss.str());
}
}
void SolutionBase::fillInfo(moveit_task_constructor_msgs::msg::SolutionInfo& info, Introspection* introspection) const {

View File

@ -76,30 +76,14 @@ namespace task_constructor {
TaskPrivate::TaskPrivate(Task* me, const std::string& ns)
: WrapperBasePrivate(me, std::string()), ns_(rosNormalizeName(ns)), preempt_requested_(false) {}
void swap(StagePrivate*& lhs, StagePrivate*& rhs) {
// It only makes sense to swap pimpl instances of a Task!
// However, due to member protection rules, we can only implement it here
assert(typeid(lhs) == typeid(rhs));
// swap instances
::std::swap(lhs, rhs);
// as well as their me_ pointers
::std::swap(lhs->me_, rhs->me_);
// and redirect the parent pointers of children to new parents
auto& lhs_children = static_cast<ContainerBasePrivate*>(lhs)->children_;
for (auto it = lhs_children.begin(), end = lhs_children.end(); it != end; ++it) {
(*it)->pimpl()->unparent();
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(lhs->me_));
(*it)->pimpl()->setParentPosition(it);
}
auto& rhs_children = static_cast<ContainerBasePrivate*>(rhs)->children_;
for (auto it = rhs_children.begin(), end = rhs_children.end(); it != end; ++it) {
(*it)->pimpl()->unparent();
(*it)->pimpl()->setParent(static_cast<ContainerBase*>(rhs->me_));
(*it)->pimpl()->setParentPosition(it);
}
TaskPrivate& TaskPrivate::operator=(TaskPrivate&& other) {
this->WrapperBasePrivate::operator=(std::move(other));
ns_ = std::move(other.ns_);
introspection_ = std::move(other.introspection_);
robot_model_ = std::move(other.robot_model_);
robot_model_loader_ = std::move(other.robot_model_loader_);
task_cbs_ = std::move(other.task_cbs_);
return *this;
}
const ContainerBase* TaskPrivate::stages() const {
@ -124,7 +108,7 @@ Task::Task(Task&& other) // NOLINT(performance-noexcept-move-constructor)
Task& Task::operator=(Task&& other) { // NOLINT(performance-noexcept-move-constructor)
clear(); // remove all stages of current task
swap(this->pimpl_, other.pimpl_);
*static_cast<TaskPrivate*>(pimpl_) = std::move(*static_cast<TaskPrivate*>(other.pimpl_));
return *this;
}
@ -248,30 +232,37 @@ void Task::compute() {
stages()->pimpl()->runCompute();
}
bool Task::plan(size_t max_solutions) {
moveit::core::MoveItErrorCode Task::plan(size_t max_solutions) {
auto impl = pimpl();
init();
// Print state and return success if there are solutions otherwise the input error_code
const auto success_or = [this](const int32_t error_code) {
printState();
return numSolutions() > 0 ? moveit::core::MoveItErrorCode::SUCCESS : error_code;
};
impl->preempt_requested_ = false;
const double available_time = timeout();
const auto start_time = std::chrono::steady_clock::now();
while (!impl->preempt_requested_ && canCompute() && (max_solutions == 0 || numSolutions() < max_solutions) &&
std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < available_time) {
while (canCompute() && (max_solutions == 0 || numSolutions() < max_solutions)) {
if (impl->preempt_requested_)
return success_or(moveit::core::MoveItErrorCode::PREEMPTED);
if (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() > available_time)
return success_or(moveit::core::MoveItErrorCode::TIMED_OUT);
compute();
for (const auto& cb : impl->task_cbs_)
cb(*this);
if (impl->introspection_)
impl->introspection_->publishTaskState();
}
printState();
return numSolutions() > 0;
};
return success_or(moveit::core::MoveItErrorCode::PLANNING_FAILED);
}
void Task::preempt() {
pimpl()->preempt_requested_ = true;
}
moveit_msgs::msg::MoveItErrorCodes Task::execute(const SolutionBase& s) {
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
// Add random ID to prevent warnings about multiple publishers within the same node
rclcpp::NodeOptions options;
options.arguments(

View File

@ -26,6 +26,7 @@ if (BUILD_TESTING)
if(TEST_FILE) # Add launch test if .launch.py file was specified
ament_add_gtest_executable(${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
add_launch_test(${TEST_FILE}
TARGET ${TEST_NAME}
ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-${TEST_NAME}>")
else()
if("${TYPE}" STREQUAL "gtest")
@ -55,6 +56,7 @@ if (BUILD_TESTING)
mtc_add_gmock(test_interface_state.cpp)
mtc_add_gtest(test_move_to.cpp move_to.launch.py)
mtc_add_gtest(test_move_relative.cpp move_to.launch.py)
# building these integration tests works without moveit config packages
ament_add_gtest_executable(pick_ur5 pick_ur5.cpp)

View File

@ -586,10 +586,6 @@ TEST(Task, move) {
Task t2 = std::move(t1);
EXPECT_EQ(t2.stages()->numChildren(), 2u);
EXPECT_EQ(t1.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
t1 = std::move(t2);
EXPECT_EQ(t1.stages()->numChildren(), 2u);
EXPECT_EQ(t2.stages()->numChildren(), 0u); // NOLINT(clang-analyzer-cplusplus.Move)
}
TEST(Task, reuse) {
@ -655,7 +651,7 @@ TEST(Task, timeout) {
// zero timeout fails
t.reset();
t.setTimeout(0.0);
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.plan(), moveit::core::MoveItErrorCode::TIMED_OUT);
// time for 1 solution
t.reset();

View File

@ -5,6 +5,10 @@
#include <gtest/gtest.h>
#include <gmock/gmock-matchers.h>
#ifndef TYPED_TEST_SUITE
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
#endif
namespace mtc = moveit::task_constructor;
// type-trait functions for OrderedTest<T>
@ -62,11 +66,7 @@ protected:
};
// set of template types to test for
using TypeInstances = ::testing::Types<int, int*, mtc::SolutionBasePtr, mtc::SolutionBaseConstPtr>;
#ifdef TYPED_TEST_SUITE
TYPED_TEST_SUITE(ValueOrPointeeLessTest, TypeInstances);
#else
TYPED_TEST_CASE(ValueOrPointeeLessTest, TypeInstances);
#endif
TYPED_TEST(ValueOrPointeeLessTest, less) {
EXPECT_TRUE(this->less(2, 3));
EXPECT_FALSE(this->less(1, 1));
@ -105,11 +105,7 @@ protected:
SCOPED_TRACE("pushAndValidate(" #cost ", " #__VA_ARGS__ ")"); \
this->pushAndValidate(cost, __VA_ARGS__); \
}
#ifdef TYPED_TEST_SUITE
TYPED_TEST_SUITE(OrderedTest, TypeInstances);
#else
TYPED_TEST_CASE(OrderedTest, TypeInstances);
#endif
TYPED_TEST(OrderedTest, sorting) {
pushAndValidate(2, { 2 });
pushAndValidate(1, { 1, 2 });

View File

@ -18,7 +18,7 @@ using namespace moveit::task_constructor;
using FallbacksFixtureGenerator = TaskTestBase;
TEST_F(FallbacksFixtureGenerator, DISABLED_stayWithFirstSuccessful) {
TEST_F(FallbacksFixtureGenerator, stayWithFirstSuccessful) {
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(INF)));
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(1.0)));
@ -56,7 +56,7 @@ TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
@ -68,7 +68,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStageOnly) {
EXPECT_EQ(t.numSolutions(), 1u);
}
TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutionOnly) {
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStagePerSolutionOnly) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0 })));
// duplicate generator solutions with resulting costs: 4, 2 | 3, 1
t.add(std::make_unique<ForwardMockup>(PredefinedCosts({ 2.0, 0.0, 2.0, 0.0 }), 2));
@ -79,10 +79,11 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ComputeFirstSuccessfulStagePerSolutio
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 211, 222));
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221));
}
TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) {
// requires individual job control in Fallbacks's children
TEST_F(FallbacksFixturePropagate, DISABLED_updateSolutionOrder) {
t.add(std::make_unique<BackwardMockup>(PredefinedCosts({ 10.0, 0.0 })));
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
// available solutions (sorted) in individual runs of fallbacks: 1 | 11, 2 | 2, 11
@ -101,7 +102,8 @@ TEST_F(FallbacksFixturePropagate, DISABLED_UpdateSolutionOrder) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result
}
TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
// requires individual job control in Fallbacks's children
TEST_F(FallbacksFixturePropagate, DISABLED_multipleActivePendingStates) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 2.0, 1.0, 3.0 })));
// use a fallback container to delay computation: the 1st child never succeeds, but only the 2nd
auto inner = std::make_unique<Fallbacks>("Inner");
@ -118,7 +120,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_MultipleActivePendingStates) {
// check that first solution is not marked as pruned
}
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -130,7 +132,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
@ -142,7 +144,7 @@ TEST_F(FallbacksFixturePropagate, DISABLED_successfulWithMixedSolutions2) {
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
TEST_F(FallbacksFixturePropagate, activeChildReset) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
@ -160,18 +162,18 @@ TEST_F(FallbacksFixturePropagate, DISABLED_ActiveChildReset) {
using FallbacksFixtureConnect = TaskTestBase;
TEST_F(FallbacksFixtureConnect, DISABLED_ConnectStageInsideFallbacks) {
TEST_F(FallbacksFixtureConnect, connectStageInsideFallbacks) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, 2.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts({ 0.0, 0.0, INF, 0.0 })));
fallbacks->add(std::make_unique<ConnectMockup>(PredefinedCosts::constant(100.0)));
t.add(std::move(fallbacks));
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 10.0, 20.0 })));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 21, 22));
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 12, 22, 121));
}
int main(int argc, char** argv) {

View File

@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) {
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger
auto dstart = InterfaceState::Status::FAILED;
auto dstart = InterfaceState::Status::ARMED;
EXPECT_TRUE(Prio(0, 0, dstart) == Prio(0, 0, dstart));
EXPECT_TRUE(Prio(1, 0, dstart) < Prio(0, 0, dstart));
EXPECT_TRUE(Prio(1, 42, dstart) < Prio(0, 0, dstart));
@ -68,7 +68,7 @@ TEST(Interface, update) {
i.updatePriority(*i.rbegin(), Prio(5, 0.0));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 5, 3 }));
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::FAILED));
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
}
@ -87,8 +87,11 @@ TEST(StatePairs, compare) {
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
auto good = InterfaceState::Status::ENABLED;
auto bad = InterfaceState::Status::FAILED;
EXPECT_TRUE(pair(good, good) < pair(good, bad));
EXPECT_TRUE(pair(good, good) < pair(bad, good));
EXPECT_TRUE(pair(bad, good) < pair(good, bad));
auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good));
ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation
for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) {
EXPECT_TRUE(good_good < pair(bad, good));
EXPECT_TRUE(good_good < pair(good, bad));
EXPECT_TRUE(good_good < pair(bad, bad));
}
}

View File

@ -0,0 +1,128 @@
#include "models.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
using namespace moveit::core;
constexpr double TAU{ 2 * M_PI };
constexpr double EPS{ 1e-6 };
// provide a basic test fixture that prepares a Task
struct PandaMoveRelative : public testing::Test
{
Task t;
stages::MoveRelative* move;
PlanningScenePtr scene;
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative");
const JointModelGroup* group;
PandaMoveRelative() {
t.setRobotModel(loadModel(node));
group = t.getRobotModel()->getJointModelGroup("panda_arm");
scene = std::make_shared<PlanningScene>(t.getRobotModel());
scene->getCurrentStateNonConst().setToDefaultValues();
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
t.add(std::make_unique<stages::FixedState>("start", scene));
auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
move_relative->setGroup(group->getName());
move = move_relative.get();
t.add(std::move(move_relative));
}
};
moveit_msgs::msg::AttachedCollisionObject createAttachedObject(const std::string& id) {
moveit_msgs::msg::AttachedCollisionObject aco;
aco.link_name = "panda_hand";
aco.object.header.frame_id = aco.link_name;
aco.object.operation = aco.object.ADD;
aco.object.id = id;
aco.object.primitives.resize(1, [] {
shape_msgs::msg::SolidPrimitive p;
p.type = p.SPHERE;
p.dimensions.resize(1);
p.dimensions[p.SPHERE_RADIUS] = 0.01;
return p;
}());
geometry_msgs::msg::Pose p;
p.position.x = 0.1;
p.orientation.w = 1.0;
#if MOVEIT_HAS_OBJECT_POSE
aco.object.pose = p;
#else
aco.object.primitive_poses.resize(1, p);
aco.object.primitive_poses[0] = p;
#endif
return aco;
}
inline auto position(const PlanningSceneConstPtr& scene, const std::string& frame) {
return scene->getFrameTransform(frame).translation();
}
TEST_F(PandaMoveRelative, cartesianRotateEEF) {
move->setDirection([] {
geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "world";
twist.twist.angular.z = TAU / 8.0;
return twist;
}());
ASSERT_TRUE(t.plan()) << "Failed to plan";
const auto& tip_name{ group->getOnlyOneEndEffectorTip()->getName() };
const auto start_eef_position{ position(scene, tip_name) };
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), tip_name) };
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
<< "Cartesian rotation unexpectedly changed position of '" << tip_name << "' (must only change orientation)\n"
<< start_eef_position << "\nvs\n"
<< end_eef_position;
}
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
const std::string ATTACHED_OBJECT{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
move->setIKFrame(ATTACHED_OBJECT);
move->setDirection([] {
geometry_msgs::msg::TwistStamped twist;
twist.header.frame_id = "world";
twist.twist.angular.z = TAU / 8.0;
return twist;
}());
ASSERT_TRUE(t.plan());
const auto start_eef_position{ position(scene, ATTACHED_OBJECT) };
const auto end_eef_position{ position(move->solutions().front()->end()->scene(), ATTACHED_OBJECT) };
EXPECT_TRUE(start_eef_position.isApprox(end_eef_position, EPS))
<< "Cartesian rotation unexpectedly changed position of ik frame (must only change orientation)\n"
<< start_eef_position << "\nvs\n"
<< end_eef_position;
}
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
return RUN_ALL_TESTS();
}

View File

@ -8,6 +8,10 @@
#include <gtest/gtest.h>
#ifndef TYPED_TEST_SUITE
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
#endif
using namespace moveit::task_constructor;
using Pruning = TaskTestBase;
@ -40,7 +44,54 @@ TEST_F(Pruning, PruningMultiForward) {
EXPECT_EQ((*t.solutions().begin())->cost(), 0u);
}
// The 2nd failing FW attempt would prune the path through CON,
// but shouldn't because there exist two more GEN2 solutions
TEST_F(Pruning, NoPruningIfAlternativesExist) {
add(t, new GeneratorMockup(PredefinedCosts({ 0.0 })));
add(t, new ConnectMockup());
add(t, new GeneratorMockup(std::list<double>{ 0, 10, 20, 30 }, 2));
add(t, new ForwardMockup({ INF, INF, 0.0, INF }));
t.plan();
EXPECT_EQ(t.solutions().size(), 1u);
}
TEST_F(Pruning, ConnectReactivatesPrunedPaths) {
add(t, new BackwardMockup);
add(t, new GeneratorMockup({ 0 }));
add(t, new ConnectMockup());
// the solution here should re-activate the initially pruned backward path
add(t, new GeneratorMockup({ 0 }));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 1u);
}
// same as before, but wrapping Connect into a container
template <typename T>
struct PruningContainerTests : public Pruning
{
void test() {
add(t, new BackwardMockup);
add(t, new GeneratorMockup({ 0 }));
auto c = new T();
add(*c, new ConnectMockup());
add(t, c);
add(t, new GeneratorMockup({ 0 }));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 1u);
}
};
using ContainerTypes = ::testing::Types<SerialContainer>; // TODO: fails for Fallbacks!
TYPED_TEST_SUITE(PruningContainerTests, ContainerTypes);
TYPED_TEST(PruningContainerTests, ConnectReactivatesPrunedPaths) {
this->test();
}
TEST_F(Pruning, ConnectConnectForward) {
add(t, new BackwardMockup());
add(t, new GeneratorMockup());
auto c1 = add(t, new ConnectMockup({ INF, 0, 0 })); // 1st attempt is a failue
add(t, new GeneratorMockup({ 0, 10, 20 }));
@ -62,6 +113,7 @@ TEST_F(Pruning, ConnectConnectForward) {
}
TEST_F(Pruning, ConnectConnectBackward) {
add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 1, 2, 3 }));
auto c1 = add(t, new ConnectMockup());
add(t, new BackwardMockup());
@ -141,3 +193,17 @@ TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
// the failure in one branch of Alternatives must not prune computing back
EXPECT_EQ(back->runs_, 1u);
}
TEST_F(Pruning, TwoConnects) {
add(t, new GeneratorMockup({ 0 }));
add(t, new ForwardMockup({ INF }));
add(t, new ConnectMockup());
add(t, new GeneratorMockup());
add(t, new ConnectMockup());
add(t, new GeneratorMockup());
add(t, new ForwardMockup());
EXPECT_FALSE(t.plan());
}

View File

@ -1,9 +1,7 @@
cmake_minimum_required(VERSION 3.5)
project(moveit_task_constructor_demo)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
@ -54,6 +52,7 @@ demo(cartesian)
demo(modular)
demo(alternative_path_costs)
demo(ik_clearance_cost)
demo(fallbacks_move_to)
demo(pick_place_demo)
target_link_libraries(${PROJECT_NAME}_pick_place_demo ${PROJECT_NAME}_pick_place_task)

View File

@ -0,0 +1,29 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.planning_pipelines(pipelines=["ompl", "pilz_industrial_motion_planner"])
.to_moveit_configs()
)
print(moveit_config.planning_pipelines)
fallbacks_move_to_task = Node(
package="moveit_task_constructor_demo",
executable="fallbacks_move_to",
output="screen",
parameters=[
moveit_config.cartesian_limits,
moveit_config.joint_limits,
moveit_config.planning_pipelines,
moveit_config.robot_description,
moveit_config.robot_description_kinematics,
moveit_config.robot_description_semantic,
],
)
return LaunchDescription([fallbacks_move_to_task])

View File

@ -0,0 +1,141 @@
#include <rclcpp/rclcpp.hpp>
#include <moveit/robot_model/robot_model.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/stages.h>
constexpr double TAU = 2 * M_PI;
using namespace moveit::task_constructor;
/** CurrentState -> Fallbacks( MoveTo<CartesianPath>, MoveTo<PTP>, MoveTo<OMPL> )*/
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("mtc_tutorial");
std::thread spinning_thread([node] { rclcpp::spin(node); });
// setup Task
Task t;
t.loadRobotModel(node);
const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() };
assert(robot->getName() == "panda");
// setup solvers
auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setJumpThreshold(2.0);
const auto ptp = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "pilz_industrial_motion_planner") };
pp->setPlannerId("PTP");
return pp;
}();
const auto rrtconnect = [&node]() {
auto pp{ std::make_shared<solvers::PipelinePlanner>(node, "ompl") };
pp->setPlannerId("RRTConnect");
return pp;
}();
// target state for Task
std::map<std::string, double> target_state;
robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state);
target_state["panda_joint1"] = +TAU / 8;
// define initial scenes
auto initial_scene{ std::make_shared<planning_scene::PlanningScene>(robot) };
initial_scene->getCurrentStateNonConst().setToDefaultValues(robot->getJointModelGroup("panda_arm"), "ready");
auto initial_alternatives = std::make_unique<Alternatives>("initial states");
{
// can reach target with Cartesian motion
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
{
// Cartesian motion to target is impossible, but PTP is collision-free
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({
{ "panda_joint1", +TAU / 8 },
{ "panda_joint4", 0 },
});
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
{
// Cartesian and PTP motion to target would be in collision
auto fixed = std::make_unique<stages::FixedState>("current state");
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
scene->processCollisionObjectMsg([]() {
moveit_msgs::msg::CollisionObject co;
co.id = "box";
co.header.frame_id = "panda_link0";
co.operation = co.ADD;
#if MOVEIT_HAS_OBJECT_POSE
auto& pose{ co.pose };
#else
co.primitive_poses.emplace_back();
auto& pose{ co.primitive_poses[0] };
#endif
pose = []() {
geometry_msgs::msg::Pose p;
p.position.x = 0.3;
p.position.y = 0.0;
p.position.z = 0.64 / 2;
p.orientation.w = 1.0;
return p;
}();
co.primitives.push_back([]() {
shape_msgs::msg::SolidPrimitive sp;
sp.type = sp.BOX;
sp.dimensions = { 0.2, 0.05, 0.64 };
return sp;
}());
return co;
}());
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
t.add(std::move(initial_alternatives));
// fallbacks to reach target_state
auto fallbacks = std::make_unique<Fallbacks>("move to other side");
auto add_to_fallbacks{ [&](auto& solver, auto& name) {
auto move_to = std::make_unique<stages::MoveTo>(name, solver);
move_to->setGroup("panda_arm");
move_to->setGoal(target_state);
fallbacks->add(std::move(move_to));
} };
add_to_fallbacks(cartesian, "Cartesian path");
add_to_fallbacks(ptp, "PTP path");
add_to_fallbacks(rrtconnect, "RRT path");
t.add(std::move(fallbacks));
try {
std::cout << t << std::endl;
t.plan();
} catch (const InitStageException& e) {
std::cout << e << std::endl;
}
// keep alive for interactive inspection in rviz
spinning_thread.join();
return 0;
}

View File

@ -46,6 +46,7 @@ int main(int argc, char** argv) {
auto initial = std::make_unique<stages::FixedState>();
initial->setState(scene);
initial->properties().set("ignore_collisions", true);
auto ik = std::make_unique<stages::ComputeIK>();
ik->insert(std::move(initial));

View File

@ -491,7 +491,7 @@ bool PickPlaceTask::plan() {
int max_solutions = 10;
rosparam_shortcuts::get(node_, "max_solutions", max_solutions);
return task_->plan(max_solutions);
return static_cast<bool>(task_->plan(max_solutions));
}
bool PickPlaceTask::execute() {