mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge d2918f130d
This commit is contained in:
commit
11b81856d5
2
.github/workflows/ci.yaml
vendored
2
.github/workflows/ci.yaml
vendored
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
add_compile_options(-fvisibility-inlines-hidden)
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
@ -609,7 +640,7 @@ void SerialContainerPrivate::validateConnectivity() const {
|
||||
ContainerBasePrivate::validateConnectivity();
|
||||
|
||||
InterfaceFlags mine = interfaceFlags();
|
||||
// check that input / output interface of first / last child matches this' resp. interface
|
||||
// check that input/output interface of first/last child matches this' resp. interface
|
||||
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
|
||||
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
|
||||
|
||||
@ -621,7 +652,7 @@ void SerialContainerPrivate::validateConnectivity() const {
|
||||
const StagePrivate* const cur_impl = **cur;
|
||||
InterfaceFlags required = cur_impl->interfaceFlags();
|
||||
|
||||
// get iterators to prev / next stage in sequence
|
||||
// get iterators to prev/next stage in sequence
|
||||
auto prev = cur;
|
||||
--prev;
|
||||
auto next = cur;
|
||||
@ -649,9 +680,8 @@ bool SerialContainer::canCompute() const {
|
||||
|
||||
void SerialContainer::compute() {
|
||||
for (const auto& stage : pimpl()->children()) {
|
||||
if (!stage->pimpl()->canCompute())
|
||||
continue;
|
||||
stage->pimpl()->runCompute();
|
||||
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,
|
||||
@ -721,7 +755,7 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child,
|
||||
void ParallelContainerBasePrivate::validateConnectivity() const {
|
||||
InterfaceFlags my_interface = interfaceFlags();
|
||||
|
||||
// check that input / output interfaces of all children are handled by my interface
|
||||
// check that input/output interfaces of all children are handled by my interface
|
||||
for (const auto& child : children())
|
||||
validateInterfaces(*child->pimpl(), my_interface);
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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,9 +103,9 @@ 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"),
|
||||
props.get<double>("max_acceleration_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");
|
||||
}
|
||||
|
||||
@ -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,10 +94,9 @@ 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"),
|
||||
props.get<double>("max_acceleration_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;
|
||||
}
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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?
|
||||
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);
|
||||
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));
|
||||
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<dir>();
|
||||
bool have_enabled_opposites = false;
|
||||
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++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);
|
||||
}
|
||||
// std::cerr << name_ << ": ";
|
||||
// printPendingPairs(std::cerr);
|
||||
// std::cerr << std::endl;
|
||||
#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;
|
||||
}
|
||||
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");
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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 {
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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 });
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
128
core/test/test_move_relative.cpp
Normal file
128
core/test/test_move_relative.cpp
Normal 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();
|
||||
}
|
||||
@ -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());
|
||||
}
|
||||
|
||||
@ -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 17)
|
||||
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)
|
||||
|
||||
29
demo/launch/fallbacks_move_to.launch.py
Normal file
29
demo/launch/fallbacks_move_to.launch.py
Normal 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])
|
||||
141
demo/src/fallbacks_move_to.cpp
Normal file
141
demo/src/fallbacks_move_to.cpp
Normal 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;
|
||||
}
|
||||
@ -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));
|
||||
|
||||
@ -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() {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user