Merge branch 'master' into wip-python-api

This commit is contained in:
Robert Haschke 2022-01-07 14:40:47 +01:00
commit 86093be94a
64 changed files with 2674 additions and 1210 deletions

View File

@ -16,7 +16,7 @@ jobs:
env:
- IMAGE: melodic-source
NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_CXX_FLAGS="--coverage"
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
- IMAGE: master-source
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
@ -32,18 +32,21 @@ jobs:
DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
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:
CATKIN_LINT: true
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
UNDERLAY: /root/ws_moveit/install
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: /home/runner/work
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
BASEDIR: ${{ github.workspace }}/.work
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
# perform full clang-tidy check only on manual trigger (workflow_dispatch), PRs do check changed files, otherwise nothing
CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }}
CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
runs-on: ubuntu-latest
@ -53,7 +56,7 @@ jobs:
submodules: recursive
- name: Cache ccache
uses: pat-s/always-upload-cache@v2.1.3
uses: pat-s/always-upload-cache@v2.1.5
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
@ -61,25 +64,26 @@ jobs:
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
ccache-${{ env.CACHE_PREFIX }}
- name: industrial_ci
- id: ici
name: Run industrial_ci
uses: ros-industrial/industrial_ci@master
env: ${{ matrix.env || env }}
env: ${{ matrix.env }}
- name: Upload test artifacts (on failure)
uses: actions/upload-artifact@v2
if: failure()
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
with:
name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
- name: Collect coverage information
uses: rhaschke/lcov-action@master
if: ${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') }}
- name: Generate codecov report
uses: rhaschke/lcov-action@main
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with:
workdir: ${{ env.BASEDIR }}
dir: target_ws
ignore: '"/usr/*" "/opt/*" "/root/ws_moveit/*" "*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
docker: $DOCKER_IMAGE
workdir: ${{ env.BASEDIR }}/target_ws
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
- name: Upload codecov report
uses: codecov/codecov-action@v1
if: ${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') }}
uses: codecov/codecov-action@v2
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with:
files: ${{ env.BASEDIR }}/coverage.info
files: ${{ env.BASEDIR }}/target_ws/coverage.info

View File

@ -23,3 +23,9 @@ jobs:
with:
distro: noetic
- uses: pre-commit/action@v2.0.0
id: precommit
- name: Upload pre-commit changes
if: failure() && steps.precommit.outcome == 'failure'
uses: rhaschke/upload-git-patch-action@main
with:
name: pre-commit

View File

@ -15,7 +15,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v3.4.0
rev: v4.0.1
hooks:
- id: check-added-large-files
- id: check-case-conflict
@ -29,7 +29,7 @@ repos:
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 20.8b1
rev: 21.11b1
hooks:
- id: black
args: ["--line-length", "100"]

View File

@ -7,6 +7,7 @@ find_package(catkin REQUIRED COMPONENTS
actionlib
moveit_core
moveit_ros_move_group
moveit_task_constructor_core
moveit_task_constructor_msgs
pluginlib
std_msgs
@ -20,16 +21,6 @@ catkin_package(
std_msgs
)
# check for MOVEIT_MASTER
include(CheckIncludeFileCXX)
set(CMAKE_REQUIRED_INCLUDES ${moveit_core_INCLUDE_DIRS})
set(CMAKE_REQUIRED_FLAGS "-std=c++11")
CHECK_INCLUDE_FILE_CXX(moveit/collision_detection/collision_env.h MOVEIT_MASTER)
if(NOT MOVEIT_MASTER)
set(MOVEIT_MASTER 0)
endif()
add_definitions(-DMOVEIT_MASTER=${MOVEIT_MASTER})
include_directories(
${catkin_INCLUDE_DIRS}
)

View File

@ -17,6 +17,7 @@
<depend>actionlib</depend>
<depend>pluginlib</depend>
<depend>std_msgs</depend>
<depend>moveit_task_constructor_core</depend>
<depend>moveit_task_constructor_msgs</depend>
<export>

View File

@ -36,12 +36,14 @@
#include "execute_task_solution_capability.h"
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h>
#if MOVEIT_MASTER
#if MOVEIT_HAS_MESSAGE_CHECKS
#include <moveit/utils/message_checks.h>
#endif
@ -168,7 +170,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this, sub_traj,
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
#if MOVEIT_MASTER
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
@ -179,7 +181,7 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
return true;
};
#if MOVEIT_MASTER
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&

View File

@ -4,7 +4,7 @@ project(moveit_task_constructor_core)
find_package(Boost REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roslint
eigen_conversions
tf2_eigen
geometry_msgs
moveit_core
moveit_ros_planning
@ -29,21 +29,13 @@ catkin_package(
geometry_msgs
moveit_core
moveit_task_constructor_msgs
rviz_marker_tools
visualization_msgs
CFG_EXTRAS pybind11.cmake
)
set(CMAKE_CXX_STANDARD 14)
# check for MOVEIT_MASTER
include(CheckIncludeFileCXX)
set(CMAKE_REQUIRED_INCLUDES ${moveit_core_INCLUDE_DIRS})
CHECK_INCLUDE_FILE_CXX(moveit/collision_detection/collision_env.h MOVEIT_MASTER)
if(NOT MOVEIT_MASTER)
set(MOVEIT_MASTER 0)
endif()
add_definitions(-DMOVEIT_MASTER=${MOVEIT_MASTER})
add_compile_options(-fvisibility-inlines-hidden)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)

View File

@ -55,7 +55,8 @@ public:
Stage* findChild(const std::string& name) const;
/** Callback function type used by traverse functions
* The callback should return false if traversal should be stopped. */
* Receives currently visited Stage and current depth in hierarchy
* If callback returns false, traversal is stopped. */
using StageCallback = std::function<bool(const Stage&, unsigned int)>;
/// traverse direct children of this container, calling the callback for each of them
bool traverseChildren(const StageCallback& processor) const;
@ -149,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,
@ -157,17 +159,23 @@ public:
*/
class Fallbacks : public ParallelContainerBase
{
mutable Stage* active_child_ = nullptr;
inline void replaceImpl();
public:
Fallbacks(const std::string& name = "fallbacks") : ParallelContainerBase(name) {}
PRIVATE_CLASS(Fallbacks);
Fallbacks(const std::string& name = "fallbacks");
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
private:
// not needed, we directly use corresponding virtual methods of FallbacksPrivate
bool canCompute() const final { return false; }
void compute() final {}
};
class MergerPrivate;

View File

@ -76,7 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend void swap(StagePrivate*& lhs, StagePrivate*& rhs);
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
public:
using container_type = StagePrivate::container_type;
@ -121,15 +121,21 @@ public:
InterfacePtr pendingBackward() const { return pending_backward_; }
InterfacePtr pendingForward() const { return pending_forward_; }
// tags for internal_external_ bimap
struct INTERNAL
{};
struct EXTERNAL
{};
// map InterfaceStates from children to external InterfaceStates of the container
const auto& internalToExternalMap() const { return internal_external_.left; }
const auto& externalToInternalMap() const { return internal_external_.right; }
inline const auto& internalToExternalMap() const { return internal_external_.by<INTERNAL>(); }
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) {
@ -143,20 +149,25 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// Set ENABLED / DISABLED 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);
/// protected writable overloads
inline auto& internalToExternalMap() { return internal_external_.left; }
inline auto& ExternalToInternalMap() { return internal_external_.right; }
inline auto& internalToExternalMap() { return internal_external_.by<INTERNAL>(); }
inline auto& externalToInternalMap() { return internal_external_.by<EXTERNAL>(); }
// set in resolveInterface()
InterfaceFlags required_interface_;
@ -165,8 +176,8 @@ private:
container_type children_;
// map start/end states of children (internal) to corresponding states in our external interfaces
boost::bimap<boost::bimaps::unordered_set_of<const InterfaceState*>,
boost::bimaps::unordered_multiset_of<const InterfaceState*>>
boost::bimap<boost::bimaps::unordered_set_of<boost::bimaps::tagged<const InterfaceState*, INTERNAL>>,
boost::bimaps::unordered_multiset_of<boost::bimaps::tagged<const InterfaceState*, EXTERNAL>>>
internal_external_;
/* TODO: these interfaces don't need to be priority-sorted.
@ -223,12 +234,91 @@ protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
private:
/// callback for new externally received states
/// notify callback for new externally received interface states
template <typename Interface::Direction>
void onNewExternalState(Interface::iterator external, bool updated);
void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
// override to customize behavior on received interface states (default: propagateStateToAllChildren())
virtual void initializeExternalInterfaces();
};
PIMPL_FUNCTIONS(ParallelContainerBase)
/* The Fallbacks container needs to implement different behaviour based on its interface.
* Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
* FallbacksPrivate is the common base class for all of them, defining the common API
* to be used by the Fallbacks container.
* The actual interface-specific class is instantiated in initializeExternalInterfaces()
* resp. Fallbacks::replaceImpl() when the actual interface is known.
* The key difference between the 3 variants is how they advance to the next job. */
class FallbacksPrivate : public ParallelContainerBasePrivate
{
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other);
void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
// virtual methods specific to each variant
virtual void onNewSolution(const SolutionBase& s);
virtual void reset() {}
};
PIMPL_FUNCTIONS(Fallbacks)
/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
which both have the notion of a currently active child stage */
class FallbacksPrivateCommon : public FallbacksPrivate
{
public:
FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
/// Advance to next child
inline void nextChild();
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() = 0;
void reset() override;
bool canCompute() const override;
void compute() override;
container_type::const_iterator current_; // currently active child
};
/// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{
FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override;
};
/// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{
FallbacksPrivatePropagator(FallbacksPrivate&& old);
void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state
bool job_has_solutions_; // flag indicating whether the current job generated solutions
};
/// Fallbacks implementation for CONNECT interface
struct FallbacksPrivateConnect : FallbacksPrivate
{
FallbacksPrivateConnect(FallbacksPrivate&& old);
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
template <Interface::Direction dir>
void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
mutable container_type::const_iterator active_; // child picked for compute()
};
class WrapperBasePrivate : public ParallelContainerBasePrivate
{
friend class WrapperBase;

View File

@ -0,0 +1,59 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2021, Hamburg University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Michael 'v4hn' Goerner
Desc: Provide test for MoveIt features
*/
#pragma once
#include <moveit/version.h>
#define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
major * 1'000'000 + minor * 1'000 + patch)
// use collision env instead of collision robot/world
#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0)
// cartesian interpolator got separated from RobotState at some point
#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0)
// isEmpty got split off into its own header
#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0)
// use object shape poses relative to a single object pose
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)

View File

@ -84,7 +84,7 @@ enum InterfaceFlag
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
using InterfaceFlags = Flags<InterfaceFlag>;
using InterfaceFlags = utils::Flags<InterfaceFlag>;
/** invert interface such that
* - new end can connect to old start

View File

@ -43,6 +43,8 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/cost_queue.h>
#include <ros/console.h>
#include <ostream>
#include <chrono>
@ -59,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
@ -98,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
@ -148,8 +143,13 @@ public:
void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() {
ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'");
auto compute_start_time = std::chrono::steady_clock::now();
compute();
try {
compute();
} catch (const Property::error& e) {
me()->reportPropertyError(e);
}
auto compute_stop_time = std::chrono::steady_clock::now();
total_compute_time_ += compute_stop_time - compute_start_time;
}
@ -158,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_;
@ -197,6 +199,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) {
@ -293,6 +304,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>
@ -303,18 +315,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;
}
};
@ -326,7 +335,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;
@ -335,9 +344,9 @@ private:
template <Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
// get informed when new start or end state becomes available
// notify callback to get informed about newly inserted (or updated) start or end states
template <Interface::Direction other>
void newState(Interface::iterator it, bool updated);
void newState(Interface::iterator it, Interface::UpdateFlags updated);
// ordered list of pending state pairs
ordered<StatePair> pending;

View File

@ -48,7 +48,7 @@ namespace stages {
class FixedState : public Generator
{
public:
FixedState(const std::string& name = "initial state");
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr);
void setState(const planning_scene::PlanningScenePtr& scene);
void reset() override;

View File

@ -99,7 +99,7 @@ protected:
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen);
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
protected:

View File

@ -42,6 +42,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -82,9 +83,11 @@ public:
enum Status
{
ENABLED, // state is actively considered during planning
DISABLED, // state is disabled because a required connected state failed
DISABLED_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.
@ -100,6 +103,7 @@ public:
inline Status status() const { return std::get<0>(*this); }
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
inline unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); }
@ -135,13 +139,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_;
@ -166,6 +178,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*(); }
@ -187,10 +200,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
@ -201,9 +231,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_)
@ -217,6 +248,16 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
/// 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;
class ContainerBasePrivate;
@ -314,6 +355,12 @@ public:
double computeCost(const CostTerm& cost, std::string& comment) const override;
static SubTrajectory failure(const std::string& msg) {
SubTrajectory s;
s.markAsFailure(msg);
return s;
}
private:
// actual trajectory, might be empty
robot_trajectory::RobotTrajectoryConstPtr trajectory_;

View File

@ -47,6 +47,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/utils/moveit_error_code.h>
namespace moveit {
namespace core {
@ -117,11 +118,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::MoveItErrorCodes execute(const SolutionBase& s);
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const;

View File

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

View File

@ -32,15 +32,38 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: Project-agnostic utility classes
/* Authors: Robert Haschke, Michael 'v4hn' Goerner
Desc: Miscellaneous utilities
*/
#pragma once
#include <string>
#include <type_traits>
#include <initializer_list>
#include <Eigen/Geometry>
#include <moveit/macros/class_forward.h>
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
}
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(LinkModel);
MOVEIT_CLASS_FORWARD(JointModelGroup);
MOVEIT_CLASS_FORWARD(RobotState);
} // namespace core
namespace task_constructor {
MOVEIT_CLASS_FORWARD(Property);
MOVEIT_CLASS_FORWARD(SolutionBase);
namespace utils {
/** template class to compose flags from enums in a type-safe fashion */
template <typename Enum>
class Flags
@ -117,4 +140,12 @@ 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);
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
} // namespace utils
} // namespace task_constructor
} // namespace moveit

View File

@ -18,7 +18,7 @@
<build_depend>roslint</build_depend>
<exec_depend>roscpp</exec_depend>
<depend>eigen_conversions</depend>
<depend>tf2_eigen</depend>
<depend>geometry_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>

View File

@ -61,8 +61,7 @@ class Test(unittest.TestCase):
task.enableIntrospection()
task.init()
if task.plan():
task.publish(task.solutions[0])
self.assertFalse(task.plan())
if __name__ == "__main__":

View File

@ -5,6 +5,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/introspection.h
${PROJECT_INCLUDE}/marker_tools.h
${PROJECT_INCLUDE}/merge.h
${PROJECT_INCLUDE}/moveit_compat.h
${PROJECT_INCLUDE}/properties.h
${PROJECT_INCLUDE}/stage.h
${PROJECT_INCLUDE}/stage_p.h
@ -27,6 +28,7 @@ add_library(${PROJECT_NAME}
stage.cpp
storage.cpp
task.cpp
utils.cpp
solvers/planner_interface.cpp
solvers/cartesian_path.cpp

View File

@ -53,12 +53,65 @@ using namespace std::placeholders;
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,7 +159,76 @@ 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) {
ROS_DEBUG_STREAM_NAMED("Pruning", "'" << child.name() << "' generated a failure");
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
@ -114,87 +236,37 @@ void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::PRUNED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_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))
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::DISABLED_FAILED);
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to))
setStatus<Interface::FORWARD>(to, InterfaceState::Status::DISABLED_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::DISABLED) {
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()) {
auto external{ internalToExternalMap().find(s) };
if (external != internalToExternalMap().end()) {
// only escalate if there is no enabled alternative child
auto internals{ externalToInternalMap().equal_range(external->second) };
auto is_enabled = [](auto&& state_pair) { return state_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->second, status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with DISABLED_FAILED and all other states down the tree only with DISABLED.
// This allows us to re-enable the FAILED side, while not (yet) consider the DISABLED states again,
// when new states arrive in a Connecting stage.
// All DISABLED states are only re-enabled if the FAILED state actually gets connected.
if (status == InterfaceState::DISABLED_FAILED)
status = InterfaceState::DISABLED; // 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);
@ -202,6 +274,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);
@ -365,31 +445,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
@ -422,28 +477,13 @@ 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) {
ROS_DEBUG_STREAM_NAMED("SerialContainer", "'" << this->name() << "' received solution of child stage '"
<< current.creator()->name() << "'");
// 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();
@ -481,14 +521,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)
@ -553,9 +591,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);
}
@ -566,6 +604,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);
@ -578,9 +617,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);
}
@ -636,15 +675,8 @@ bool SerialContainer::canCompute() const {
void SerialContainer::compute() {
for (const auto& stage : pimpl()->children()) {
try {
if (!stage->pimpl()->canCompute())
continue;
ROS_DEBUG("Computing stage '%s'", stage->name().c_str());
if (stage->pimpl()->canCompute())
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
}
}
@ -665,8 +697,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);
@ -677,17 +709,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,
@ -722,9 +758,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) {}
@ -775,11 +811,7 @@ bool WrapperBase::canCompute() const {
}
void WrapperBase::compute() {
try {
wrapped()->pimpl()->runCompute();
} catch (const Property::error& e) {
wrapped()->reportPropertyError(e);
}
wrapped()->pimpl()->runCompute();
}
bool Alternatives::canCompute() const {
@ -791,11 +823,7 @@ bool Alternatives::canCompute() const {
void Alternatives::compute() {
for (const auto& stage : pimpl()->children()) {
try {
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
stage->pimpl()->runCompute();
}
}
@ -803,48 +831,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;
try {
active_child_->pimpl()->runCompute();
} catch (const Property::error& e) {
active_child_->reportPropertyError(e);
}
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())
ROS_DEBUG_STREAM_NAMED("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) {
@ -888,11 +1108,7 @@ bool Merger::canCompute() const {
void Merger::compute() {
for (const auto& stage : pimpl()->children()) {
try {
stage->pimpl()->runCompute();
} catch (const Property::error& e) {
stage->reportPropertyError(e);
}
stage->pimpl()->runCompute();
}
}

View File

@ -36,11 +36,11 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <Eigen/Geometry>
@ -196,14 +196,14 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
#if MOVEIT_MASTER
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else
#if MOVEIT_MASTER
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);

View File

@ -37,9 +37,11 @@
*/
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#if MOVEIT_MASTER
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
#include <moveit/robot_state/cartesian_interpolator.h>
#endif
@ -89,7 +91,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
};
std::vector<moveit::core::RobotStatePtr> trajectory;
#if MOVEIT_MASTER
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")),
@ -100,15 +102,14 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
is_valid);
#endif
if (!trajectory.empty()) {
result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);
assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
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"));
}
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return achieved_fraction >= props.get<double>("min_fraction");
}

View File

@ -40,6 +40,8 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
@ -53,9 +55,9 @@ void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_mod
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit_msgs::Constraints& /*path_constraints*/) {
const auto& props = properties();
// Get maximum joint distance
@ -95,12 +97,38 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
return true;
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/,
const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/,
const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& /*result*/,
const moveit_msgs::Constraints& /*path_constraints*/) {
throw std::runtime_error("Not yet implemented");
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
auto to{ from->diff() };
kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() };
constraints.add(path_constraints, from->getTransforms());
auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg,
const double* joint_values) -> bool {
robot_state->setJointGroupPositions(jmg, joint_values);
robot_state->update();
return to->isStateValid(*robot_state, constraints, jmg->getName());
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
return false;
}
to->getCurrentStateNonConst().update();
timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
if (timeout <= 0.0)
return false;
return plan(from, to, jmg, timeout, result, path_constraints);
}
} // namespace solvers
} // namespace task_constructor

View File

@ -42,7 +42,7 @@
#include <moveit/planning_pipeline/planning_pipeline.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/kinematic_constraints/utils.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -181,7 +181,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
geometry_msgs::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
tf::poseEigenToMsg(target_eigen, target.pose);
target.pose = tf2::toMsg(target_eigen);
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(

View File

@ -38,6 +38,8 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
@ -104,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())
@ -314,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 */) {
@ -481,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;
@ -686,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 {
@ -707,54 +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().status() == InterfaceState::DISABLED)
// remove all pending pairs involving this state
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
else
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 DISABLED_FAILED
if (oit->priority().status() == InterfaceState::DISABLED_FAILED)
oit->owner()->updatePriority(&*oit, InterfaceState::Priority(oit->priority(), InterfaceState::ENABLED));
pending.insert(make_pair<other>(it, oit));
// TODO: bool updated -> uint_8 updated (bitfield of PRIORITY | STATUS)
template <Interface::Direction dir>
void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) {
auto parent_pimpl = parent()->pimpl();
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()) // suppress 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 }) {
bool fw = (d == Interface::FORWARD);
if (fw)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << (fw ? "" : "") << 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?
@ -771,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;
}
@ -811,14 +886,23 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const std::string& from_object_name = from_object_pair.first;
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_pair.first);
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name);
return false;
}
#if MOVEIT_HAS_OBJECT_POSE
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name);
return false; // transforms do not match
}
#endif
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name);
return false; // shapes not matching
}
@ -826,7 +910,7 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_pair.first);
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name);
return false; // transforms do not match
}
}
@ -857,16 +941,24 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
<< to_object->getAttachedLinkName());
return false; // links not matching
}
if (from_object->getFixedTransforms().size() != to_object->getFixedTransforms().size()) {
if (from_object->getShapes().size() != to_object->getShapes().size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
return false; // shapes not matching
}
for (auto from_it = from_object->getFixedTransforms().cbegin(),
from_end = from_object->getFixedTransforms().cend(), to_it = to_object->getFixedTransforms().cbegin();
from_it != from_end; ++from_it, ++to_it)
#if MOVEIT_HAS_OBJECT_POSE
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
auto from_end = from_object->getShapePosesInLinkFrame().cend();
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
#else
auto from_it = from_object->getFixedTransforms().cbegin();
auto from_end = from_object->getFixedTransforms().cend();
auto to_it = to_object->getFixedTransforms().cbegin();
#endif
for (; from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object->getName());
ROS_DEBUG_STREAM_NAMED("Connecting",
name() << ": different pose of attached object shape: " << from_object->getName());
return false; // transforms do not match
}
}

View File

@ -37,13 +37,14 @@
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <chrono>
#include <functional>
#include <iterator>
@ -71,14 +72,14 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
void ComputeIK::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string& frame) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = frame;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setTargetPose(pose_msg);
}
@ -89,22 +90,21 @@ namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
bool isTargetPoseColliding(const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d pose,
const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr) {
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
const robot_model::LinkModel* link,
collision_detection::CollisionResult* collision_result = nullptr) {
// consider all rigidly connected parent links as well
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
// place link at given pose
// place links at given pose
robot_state.updateStateWithLinkAt(parent, pose);
robot_state.updateCollisionBodyTransforms();
// disable collision checking for parent links (except links fixed to root)
auto& acm = scene->getAllowedCollisionMatrixNonConst();
auto acm = scene->getAllowedCollisionMatrix();
std::vector<const std::string*> pending_links; // parent link names that might be rigidly connected to root
while (parent) {
pending_links.push_back(&parent->getName());
@ -231,10 +231,10 @@ void ComputeIK::compute() {
properties().performInitFrom(INTERFACE, s.start()->properties());
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = s.start()->scene()->diff();
const planning_scene::PlanningSceneConstPtr& scene{ s.start()->scene() };
const bool ignore_collisions = props.get<bool>("ignore_collisions");
const auto& robot_model = sandbox_scene->getRobotModel();
const auto& robot_model = scene->getRobotModel();
const moveit::core::JointModelGroup* eef_jmg = nullptr;
const moveit::core::JointModelGroup* jmg = nullptr;
std::string msg;
@ -256,18 +256,18 @@ void ComputeIK::compute() {
// extract target_pose
geometry_msgs::PoseStamped target_pose_msg = props.get<geometry_msgs::PoseStamped>("target_pose");
if (target_pose_msg.header.frame_id.empty()) // if not provided, assume planning frame
target_pose_msg.header.frame_id = sandbox_scene->getPlanningFrame();
target_pose_msg.header.frame_id = scene->getPlanningFrame();
Eigen::Isometry3d target_pose;
tf::poseMsgToEigen(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != sandbox_scene->getPlanningFrame()) {
if (!sandbox_scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
tf2::fromMsg(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK",
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
return;
}
// transform target_pose w.r.t. planning frame
target_pose = sandbox_scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
target_pose = scene->getFrameTransform(target_pose_msg.header.frame_id) * target_pose;
}
// determine IK link from ik_frame
@ -286,57 +286,52 @@ void ComputeIK::compute() {
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) {
link = robot_model->getLinkModel(ik_pose_msg.header.frame_id);
} else {
const robot_state::AttachedBody* attached =
sandbox_scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id);
if (!attached) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id);
return;
}
const EigenSTL::vector_Isometry3d& tf = attached->getFixedTransforms();
if (tf.empty()) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes.");
return;
}
// prepend link
link = attached->getAttachedLink();
ik_pose = tf[0] * ik_pose;
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'");
return;
}
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
// transform target pose such that ik frame will reach there if link does
target_pose = target_pose * ik_pose.inverse();
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
}
// validate placed link for collisions
collision_detection::CollisionResult collisions;
bool colliding = !ignore_collisions && isTargetPoseColliding(sandbox_scene, target_pose, link, &collisions);
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst();
// markers used for failures
std::deque<visualization_msgs::Marker> failure_markers;
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
std::deque<visualization_msgs::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::Marker> eef_markers;
// visualize placed end-effector
auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {
auto appender = [&eef_markers](visualization_msgs::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, ", "));
spawn(InterfaceState(sandbox_scene), std::move(solution));
auto colliding_scene{ scene->diff() };
colliding_scene->setCurrentState(sandbox_state);
spawn(InterfaceState(colliding_scene), std::move(solution));
return;
} else
generateVisualMarkers(sandbox_state, appender, links_to_visualize);
@ -349,12 +344,12 @@ void ComputeIK::compute() {
compare_state.setToDefaultValues(jmg, compare_pose_name);
compare_state.copyJointGroupPositions(jmg, compare_pose);
} else
sandbox_scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
scene->getCurrentState().copyJointGroupPositions(jmg, compare_pose);
double min_solution_distance = props.get<double>("min_solution_distance");
IKSolutions ik_solutions;
auto is_valid = [sandbox_scene, ignore_collisions, min_solution_distance,
auto is_valid = [scene, ignore_collisions, min_solution_distance,
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
const double* joint_positions) {
for (const auto& sol : ik_solutions) {
@ -365,7 +360,7 @@ void ComputeIK::compute() {
ik_solutions.emplace_back();
state->copyJointGroupPositions(jmg, ik_solutions.back());
return ignore_collisions || !sandbox_scene->isStateColliding(*state, jmg->getName());
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName());
};
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
@ -388,13 +383,10 @@ void ComputeIK::compute() {
// for all new solutions (successes and failures)
for (size_t i = previous; i != ik_solutions.size(); ++i) {
// create a new scene for each solution as they will have different robot states
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
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
@ -403,12 +395,16 @@ void ComputeIK::compute() {
solution.markAsFailure();
// set scene's robot state
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
robot_state.update();
robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
solution_state.setJointGroupPositions(jmg, ik_solutions[i].data());
solution_state.update();
InterfaceState state(scene);
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));
}
@ -425,9 +421,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::ColorRGBA tint_color;
tint_color.r = 1.0;
tint_color.g = 0.0;
tint_color.b = 0.0;
tint_color.a = 0.5;
for (auto& marker : eef_markers)
marker.color = tint_color;
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(InterfaceState(scene), std::move(solution));
}

View File

@ -37,14 +37,15 @@
*/
#include <moveit/task_constructor/stages/fix_collision_objects.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
#include <rviz_marker_tools/marker_creation.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
namespace vm = visualization_msgs;
@ -116,7 +117,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
bool failure = false;
while (!failure) {
res.clear();
#if MOVEIT_MASTER
#if MOVEIT_HAS_COLLISION_ENV
scene.getCollisionEnv()->checkRobotCollision(req, res, scene.getCurrentState(),
scene.getAllowedCollisionMatrix());
#else
@ -137,9 +138,8 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
// marker indicating correction
const cd::Contact& c = info.second.front();
rviz_marker_tools::setColor(m.color, failure ? rviz_marker_tools::RED : rviz_marker_tools::GREEN);
tf::poseEigenToMsg(Eigen::Translation3d(c.pos) *
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction),
m.pose);
m.pose = tf2::toMsg(Eigen::Translation3d(c.pos) *
Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), correction));
rviz_marker_tools::makeArrow(m, depth, true);
result.markers().push_back(m);
if (failure)
@ -147,7 +147,7 @@ SubTrajectory FixCollisionObjects::fixCollisions(planning_scene::PlanningScene&
// fix collision by shifting object along correction direction
if (!dir.empty()) // if explicitly given, use this correction direction
tf::vectorMsgToEigen(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
tf2::fromMsg(boost::any_cast<geometry_msgs::Vector3>(dir), correction);
const std::string& name = c.body_type_1 == cd::BodyTypes::WORLD_OBJECT ? c.body_name_1 : c.body_name_2;
scene.getWorldNonConst()->moveObject(name, Eigen::Isometry3d(Eigen::Translation3d(correction)));

View File

@ -43,7 +43,8 @@ namespace moveit {
namespace task_constructor {
namespace stages {
FixedState::FixedState(const std::string& name) : Generator(name) {
FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene)
: Generator(name), scene_(scene) {
setCostTerm(std::make_unique<cost::Constant>(0.0));
}

View File

@ -40,9 +40,10 @@
#include <rviz_marker_tools/marker_creation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -58,6 +59,37 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(nam
p.declare<boost::any>("grasp", "grasp posture");
}
static void applyPreGrasp(moveit::core::RobotState& state, const moveit::core::JointModelGroup* jmg,
const Property& diff_property) {
try {
// try named joint pose
const std::string& diff_state_name{ boost::any_cast<std::string>(diff_property.value()) };
if (!state.setToDefaultValues(jmg, diff_state_name)) {
throw moveit::Exception{ "unknown state '" + diff_state_name + "'" };
}
return;
} catch (const boost::bad_any_cast&) {
}
try {
// try RobotState
const moveit_msgs::RobotState& robot_state_msg = boost::any_cast<moveit_msgs::RobotState>(diff_property.value());
if (!robot_state_msg.is_diff)
throw moveit::Exception{ "RobotState message must be a diff" };
const auto& accepted = jmg->getJointModelNames();
for (const auto& joint_name_list :
{ robot_state_msg.joint_state.name, robot_state_msg.multi_dof_joint_state.joint_names })
for (const auto& name : joint_name_list)
if (std::find(accepted.cbegin(), accepted.cend(), name) == accepted.cend())
throw moveit::Exception("joint '" + name + "' is not part of group '" + jmg->getName() + "'");
robotStateMsgToRobotState(robot_state_msg, state);
return;
} catch (const boost::bad_any_cast&) {
}
throw moveit::Exception{ "no named pose or RobotState message" };
}
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
InitStageException errors;
try {
@ -76,15 +108,18 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) {
props.get<std::string>("object");
// check availability of eef
const std::string& eef = props.get<std::string>("eef");
if (!robot_model->hasEndEffector(eef))
if (!robot_model->hasEndEffector(eef)) {
errors.push_back(*this, "unknown end effector: " + eef);
else {
// check availability of eef pose
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
const std::string& name = props.get<std::string>("pregrasp");
std::map<std::string, double> m;
if (!jmg->getVariableDefaultPositions(name, m))
errors.push_back(*this, "unknown end effector pose: " + name);
throw errors;
}
// check availability of eef pose
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
moveit::core::RobotState test_state{ robot_model };
try {
applyPreGrasp(test_state, jmg, props.property("pregrasp"));
} catch (const moveit::Exception& e) {
errors.push_back(*this, std::string{ "invalid pregrasp: " } + e.what());
}
if (errors)
@ -98,14 +133,7 @@ void GenerateGraspPose::onNewSolution(const SolutionBase& s) {
const std::string& object = props.get<std::string>("object");
if (!scene->knowsFrameTransform(object)) {
const std::string msg = "object '" + object + "' not in scene";
if (storeFailures()) {
InterfaceState state(scene);
SubTrajectory solution;
solution.markAsFailure();
solution.setComment(msg);
spawn(std::move(state), std::move(solution));
} else
ROS_WARN_STREAM_NAMED("GenerateGraspPose", msg);
spawn(InterfaceState{ scene }, SubTrajectory::failure(msg));
return;
}
@ -123,7 +151,12 @@ void GenerateGraspPose::compute() {
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));
try {
applyPreGrasp(robot_state, jmg, props.property("pregrasp"));
} catch (const moveit::Exception& e) {
spawn(InterfaceState{ scene }, SubTrajectory::failure(std::string{ "invalid pregrasp: " } + e.what()));
return;
}
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object");
@ -135,7 +168,7 @@ void GenerateGraspPose::compute() {
current_angle += props.get<double>("angle_delta");
InterfaceState state(scene);
tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
target_pose_msg.pose = tf2::toMsg(target_pose);
state.properties().set("target_pose", target_pose_msg);
props.exposeTo(state.properties(), { "pregrasp", "grasp" });

View File

@ -37,13 +37,15 @@
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <rviz_marker_tools/marker_creation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/attached_body.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -53,6 +55,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam
auto& p = properties();
p.declare<std::string>("object");
p.declare<geometry_msgs::PoseStamped>("ik_frame");
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
}
void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
@ -63,7 +66,7 @@ void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
std::string msg;
if (!scene->getCurrentState().hasAttachedBody(object))
msg = "'" + object + "' is not an attached object";
if (scene->getCurrentState().getAttachedBody(object)->getFixedTransforms().empty())
if (scene->getCurrentState().getAttachedBody(object)->getShapes().empty())
msg = "'" + object + "' has no associated shapes";
if (!msg.empty()) {
if (storeFailures()) {
@ -95,20 +98,20 @@ void GeneratePlacePose::compute() {
const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose");
Eigen::Isometry3d target_pose;
tf::poseMsgToEigen(pose_msg.pose, target_pose);
tf2::fromMsg(pose_msg.pose, target_pose);
// target pose w.r.t. planning frame
scene->getTransforms().transformPose(pose_msg.header.frame_id, target_pose, target_pose);
const geometry_msgs::PoseStamped& ik_frame_msg = props.get<geometry_msgs::PoseStamped>("ik_frame");
Eigen::Isometry3d ik_frame;
tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame);
tf2::fromMsg(ik_frame_msg.pose, ik_frame);
ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame;
Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame;
// 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) {
@ -121,7 +124,7 @@ void GeneratePlacePose::compute() {
// target ik_frame's pose w.r.t. planning frame
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = scene->getPlanningFrame();
tf::poseEigenToMsg(object * object_to_ik, target_pose_msg.pose);
target_pose_msg.pose = tf2::toMsg(object * object_to_ik);
InterfaceState state(scene);
forwardProperties(*s.end(), state); // forward properties from inner solutions
@ -136,20 +139,21 @@ void GeneratePlacePose::compute() {
}
};
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
if (object->getShapes().size() == 1) {
switch (object->getShapes()[0]->type) {
case shapes::CYLINDER:
spawner(target_pose, 2);
spawner(target_pose, z_flips);
return;
case shapes::BOX: { // consider 180/90 degree rotations about z axis
const double* dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size;
spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
spawner(target_pose, z_flips, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2);
return;
}
case shapes::SPHERE: // keep original orientation and rotate about world's z
target_pose.linear() = orig_object_pose.linear();
spawner(target_pose, 1);
spawner(target_pose, z_flips);
return;
default:
break;

View File

@ -41,7 +41,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -70,7 +70,7 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf
void MoveRelative::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
@ -139,8 +139,8 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
// position half-way between pos_link and pos_reached
tf::pointEigenToMsg(0.5 * (pos_link + pos_reached), m.pose.position);
tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) });
m.pose.orientation = tf2::toMsg(quat_cylinder);
markers.push_back(m);
}
} else {
@ -154,8 +154,8 @@ static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Inter
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, linear_norm - distance);
rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
// position half-way between pos_reached and pos_target
tf::pointEigenToMsg(0.5 * (pos_reached + pos_target), m.pose.position);
tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_reached + pos_target) });
m.pose.orientation = tf2::toMsg(quat_cylinder);
markers.push_back(m);
}
}
@ -193,24 +193,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else {
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg;
const moveit::core::LinkModel* link;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
solution.markAsFailure("missing ik_frame");
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
return false;
}
}
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
return false;
bool use_rotation_distance = false; // measure achieved distance as rotation?
Eigen::Vector3d linear; // linear translation
@ -224,8 +211,8 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
try { // try to extract Twist
const geometry_msgs::TwistStamped& target = boost::any_cast<geometry_msgs::TwistStamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.twist.linear, linear);
tf::vectorMsgToEigen(target.twist.angular, angular);
tf2::fromMsg(target.twist.linear, linear);
tf2::fromMsg(target.twist.angular, angular);
linear_norm = linear.norm();
angular_norm = angular.norm();
@ -256,9 +243,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 */
@ -267,7 +254,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
try { // try to extract Vector
const geometry_msgs::Vector3Stamped& target = boost::any_cast<geometry_msgs::Vector3Stamped>(direction);
const Eigen::Isometry3d& frame_pose = scene->getFrameTransform(target.header.frame_id);
tf::vectorMsgToEigen(target.vector, linear);
tf2::fromMsg(target.vector, linear);
// use max distance?
if (max_distance > 0.0) {
@ -282,7 +269,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());
@ -291,9 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
COMPUTE:
// transform target pose such that ik frame will reach there if link does
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();
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);
@ -302,13 +287,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
double distance = 0.0;
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
distance = rotation.angle();
} else
distance = (reached_pose.translation() - link_pose.translation()).norm();
}
if (use_rotation_distance) {
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
distance = rotation.angle();
} else
distance = (reached_pose.translation() - link_pose.translation()).norm();
// min_distance reached?
if (min_distance > 0.0) {

View File

@ -36,13 +36,16 @@
Desc: Move to joint-state or Cartesian goal pose
*/
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.h>
#include <moveit/planning_scene/planning_scene.h>
#include <rviz_marker_tools/marker_creation.h>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/conversions.h>
namespace moveit {
namespace task_constructor {
@ -70,7 +73,7 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan
void MoveTo::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
@ -142,33 +145,29 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
}
bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen) {
Eigen::Isometry3d& target) {
try {
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf::poseMsgToEigen(target.pose, target_eigen);
const geometry_msgs::PoseStamped& msg = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf2::fromMsg(msg.pose, target);
// transform target into global frame
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
target_eigen = frame * target_eigen;
target = scene->getFrameTransform(msg.header.frame_id) * target;
} catch (const boost::bad_any_cast&) {
return false;
}
return true;
}
bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
Eigen::Vector3d target_point;
tf::pointMsgToEigen(target.point, target_point);
tf2::fromMsg(target.point, target_point);
// transform target into global frame
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
target_point = frame * target_point;
target_point = scene->getFrameTransform(target.header.frame_id) * target_point;
// retain link orientation
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
target_eigen = ik_pose;
target_eigen.translation() = target_point;
} catch (const boost::bad_any_cast&) {
return false;
@ -204,47 +203,37 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
// plan to joint-space target
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else { // Cartesian goal
const moveit::core::LinkModel* link;
Eigen::Isometry3d target_eigen;
// Cartesian targets require an IK reference frame
// Where to go?
Eigen::Isometry3d target;
// What frame+offset in the robot should go there?
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
solution.markAsFailure("missing ik_frame");
return false;
}
ik_pose_msg.header.frame_id = link->getName();
ik_pose_msg.pose.orientation.w = 1.0;
} else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
return false;
}
}
if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) {
const moveit::core::LinkModel* link;
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
return false;
if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) {
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
return false;
}
auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) {
geometry_msgs::PoseStamped msg;
msg.header.frame_id = scene->getPlanningFrame();
msg.pose = tf2::toMsg(pose);
rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name);
} };
// visualize plan with frame at target pose and frame at link
geometry_msgs::PoseStamped target;
target.header.frame_id = scene->getPlanningFrame();
tf::poseEigenToMsg(target_eigen, target.pose);
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
add_frame(target, "target frame");
add_frame(ik_pose_world, "ik frame");
// transform target pose such that ik frame will reach there if link does
Eigen::Isometry3d ik_pose;
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse();
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
// plan to Cartesian target
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
}
// store result

View File

@ -44,7 +44,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
namespace moveit {
namespace task_constructor {
@ -145,7 +145,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
void SimpleGraspBase::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
tf::poseEigenToMsg(pose, pose_msg.pose);
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}

View File

@ -82,6 +82,20 @@ bool InterfaceState::Priority::operator<(const InterfaceState::Priority& other)
return cost() < other.cost();
}
void InterfaceState::updatePriority(const InterfaceState::Priority& priority) {
// Never overwrite ARMED with PRUNED: PRUNED => !ARMED
assert(priority.status() != InterfaceState::Status::PRUNED || priority_.status() != InterfaceState::Status::ARMED);
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
@ -106,14 +120,16 @@ void Interface::add(InterfaceState& state) {
it->priority_ = InterfaceState::Priority(1, state.incomingTrajectories().front()->cost());
else if (!state.outgoingTrajectories().empty())
it->priority_ = InterfaceState::Priority(1, state.outgoingTrajectories().front()->cost());
else // otherwise, assume priority was well defined before
assert(it->priority_.enabled() && it->priority_.depth() >= 1u);
else { // otherwise, assume priority was well defined before
assert(it->priority_.enabled());
assert(it->priority_.depth() >= 1u);
}
// move list node into interface's state list (sorted by priority)
moveFrom(it, container);
// and finally call notify callback
if (notify_)
notify_(it, false);
notify_(it, UpdateFlags());
}
Interface::container_type Interface::remove(iterator it) {
@ -124,15 +140,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) {
@ -142,15 +166,16 @@ 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:", // DISABLED - yellow
"\033[31mf:", // DISABLED_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;
}

View File

@ -74,30 +74,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 {
@ -122,7 +106,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;
}
@ -246,30 +230,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::MoveItErrorCodes Task::execute(const SolutionBase& s) {
moveit::core::MoveItErrorCode Task::execute(const SolutionBase& s) {
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> ac("execute_task_solution");
ac.waitForServer();

113
core/src/utils.cpp Normal file
View File

@ -0,0 +1,113 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Hamburg University
* Copyright (c) 2017, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke */
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/storage.h>
namespace moveit {
namespace task_constructor {
namespace utils {
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame) {
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
return state.getRigidlyConnectedParentLinkModel(frame);
#else
const moveit::core::LinkModel* link{ nullptr };
if (state.hasAttachedBody(frame)) {
link = state.getAttachedBody(frame)->getAttachedLink();
} else if (state.getRobotModel()->hasLinkModel(frame))
link = state.getLinkModel(frame);
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
#endif
}
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) {
auto get_tip = [&jmg]() -> const moveit::core::LinkModel* {
// determine IK frame from group
std::vector<const moveit::core::LinkModel*> tips;
jmg->getEndEffectorTips(tips);
if (tips.size() != 1) {
return nullptr;
}
return tips[0];
};
if (property.value().empty()) { // property undefined
robot_link = get_tip();
if (!robot_link) {
solution.markAsFailure("missing ik_frame");
return false;
}
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link);
} else {
auto ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(property.value());
if (ik_pose_msg.header.frame_id.empty()) {
if (!(robot_link = get_tip())) {
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
return false;
}
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame;
} else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) {
robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id);
tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame);
tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame;
} else {
std::stringstream ss;
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
solution.markAsFailure(ss.str());
return false;
}
}
return true;
}
} // namespace utils
} // namespace task_constructor
} // namespace moveit

View File

@ -6,29 +6,47 @@
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_library(gtest_utils gtest_value_printers.cpp models.cpp)
add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp)
target_link_libraries(gtest_utils ${PROJECT_NAME})
catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp)
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
macro(mtc_add_test TYPE)
# Split ARGN into .test file and other sources
set(TEST_FILE ${ARGN})
set(SOURCES ${ARGN})
list(FILTER TEST_FILE INCLUDE REGEX "\.test$")
list(FILTER SOURCES EXCLUDE REGEX "\.test$")
# Determine TARGET name from first source file
list(GET SOURCES 0 TEST_NAME)
string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME})
string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME})
# Configure build target
if(TEST_FILE) # Add rostest if .test file was specified
_add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES})
else()
_catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES})
endif()
target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
endmacro()
macro(mtc_add_gtest)
mtc_add_test("gtest" ${ARGN})
endmacro()
macro(mtc_add_gmock)
mtc_add_test("gmock" ${ARGN})
endmacro()
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
mtc_add_gtest(test_stage.cpp)
mtc_add_gtest(test_container.cpp)
mtc_add_gtest(test_serial.cpp)
mtc_add_gtest(test_pruning.cpp)
mtc_add_gtest(test_properties.cpp)
mtc_add_gtest(test_cost_terms.cpp)
catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp)
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
mtc_add_gmock(test_fallback.cpp)
mtc_add_gmock(test_cost_queue.cpp)
mtc_add_gmock(test_interface_state.cpp)
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
catkin_add_gmock(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp)
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_utils gtest_main)
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)
target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
mtc_add_gtest(test_move_to.cpp move_to.test)
mtc_add_gtest(test_move_relative.cpp move_relative.test)
# building these integration tests works without moveit config packages
add_executable(pick_ur5 pick_ur5.cpp)

View File

@ -18,6 +18,6 @@ RobotModelPtr getModel() {
}
moveit::core::RobotModelPtr loadModel() {
robot_model_loader::RobotModelLoader loader;
static robot_model_loader::RobotModelLoader loader;
return loader.getModel();
}

View File

@ -0,0 +1,7 @@
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core" type="moveit_task_constructor_core-test-move-relative" test-name="move_relative"/>
</launch>

7
core/test/move_to.test Normal file
View File

@ -0,0 +1,7 @@
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core" type="moveit_task_constructor_core-test-move-to" test-name="move_to"/>
</launch>

128
core/test/stage_mockups.cpp Normal file
View File

@ -0,0 +1,128 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include <gtest/gtest.h>
namespace moveit {
namespace task_constructor {
unsigned int GeneratorMockup::id_ = 0;
unsigned int MonitoringGeneratorMockup::id_ = 0;
unsigned int ConnectMockup::id_ = 0;
unsigned int PropagatorMockup::id_ = 0;
unsigned int ForwardMockup::id_ = 0;
unsigned int BackwardMockup::id_ = 0;
void resetMockupIds() {
GeneratorMockup::id_ = 0;
MonitoringGeneratorMockup::id_ = 0;
ConnectMockup::id_ = 0;
PropagatorMockup::id_ = 0;
ForwardMockup::id_ = 0;
BackwardMockup::id_ = 0;
}
PredefinedCosts::PredefinedCosts(std::list<double>&& costs, bool finite)
: costs_{ std::move(costs) }, finite_{ finite } {}
bool PredefinedCosts::exhausted() const {
return costs_.empty();
}
double PredefinedCosts::cost() const {
// keep the tests going, but this should not happen
EXPECT_GT(costs_.size(), 0u);
if (costs_.empty())
return 0.0;
double c{ costs_.front() };
if (finite_ || costs_.size() > 1) {
costs_.pop_front();
}
return c;
}
GeneratorMockup::GeneratorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: Generator{ "GEN" + std::to_string(++id_) }
, costs_{ std::move(costs) }
, solutions_per_compute_{ solutions_per_compute } {}
void GeneratorMockup::init(const moveit::core::RobotModelConstPtr& robot_model) {
ps_.reset(new planning_scene::PlanningScene(robot_model));
Generator::init(robot_model);
}
bool GeneratorMockup::canCompute() const {
// check if runs are being used and if there are still runs left (costs are then never exhausted) or if costs
// are being used and they are not exhausted yet
return !costs_.exhausted();
}
void GeneratorMockup::compute() {
++runs_;
for (std::size_t i = 0; canCompute() && i < solutions_per_compute_; ++i)
spawn(InterfaceState(ps_), costs_.cost());
}
MonitoringGeneratorMockup::MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs)
: MonitoringGenerator{ "MON" + std::to_string(++id_), monitored }, costs_{ std::move(costs) } {}
void MonitoringGeneratorMockup::onNewSolution(const SolutionBase& s) {
++runs_;
spawn(InterfaceState{ s.end()->scene()->diff() }, SubTrajectory{});
}
ConnectMockup::ConnectMockup(PredefinedCosts&& costs)
: Connecting{ "CON" + std::to_string(++id_) }, costs_{ std::move(costs) } {}
void ConnectMockup::compute(const InterfaceState& from, const InterfaceState& to) {
++runs_;
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(costs_.cost());
connect(from, to, solution);
}
PropagatorMockup::PropagatorMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatingEitherWay{ "PRO" + std::to_string(++id_) }
, costs_{ std::move(costs) }
, solutions_per_compute_{ solutions_per_compute } {}
void PropagatorMockup::computeForward(const InterfaceState& from) {
++runs_;
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
SubTrajectory solution{ robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost() };
sendForward(from, InterfaceState{ from.scene()->diff() }, std::move(solution));
}
}
void PropagatorMockup::computeBackward(const InterfaceState& to) {
++runs_;
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
}
}
ForwardMockup::ForwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
restrictDirection(FORWARD);
setName("FWD" + std::to_string(++id_));
}
BackwardMockup::BackwardMockup(PredefinedCosts&& costs, std::size_t solutions_per_compute)
: PropagatorMockup{ std::move(costs), solutions_per_compute } {
restrictDirection(BACKWARD);
setName("BWD" + std::to_string(++id_));
}
} // namespace task_constructor
} // namespace moveit

152
core/test/stage_mockups.h Normal file
View File

@ -0,0 +1,152 @@
#pragma once
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/planning_scene/planning_scene.h>
#include "models.h"
#include <gtest/gtest.h>
#include <gmock/gmock.h>
namespace moveit {
namespace task_constructor {
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
struct PredefinedCosts : CostTerm
{
mutable std::list<double> costs_; // list of costs to assign
mutable bool finite_;
PredefinedCosts(std::list<double>&& costs, bool finite = true);
static PredefinedCosts constant(double c) { return PredefinedCosts{ std::list<double>{ c }, false }; }
static PredefinedCosts single(double c) { return PredefinedCosts{ std::list<double>{ c }, true }; }
bool exhausted() const;
double cost() const;
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
};
constexpr double INF{ std::numeric_limits<double>::infinity() };
struct GeneratorMockup : public Generator
{
planning_scene::PlanningScenePtr ps_;
PredefinedCosts costs_;
size_t runs_{ 0 };
std::size_t solutions_per_compute_;
static unsigned int id_;
// default to one solution to avoid infinity loops
GeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ std::list<double>{ 0.0 }, true },
std::size_t solutions_per_compute = 1);
GeneratorMockup(std::initializer_list<double> costs)
: GeneratorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
};
struct MonitoringGeneratorMockup : public MonitoringGenerator
{
PredefinedCosts costs_;
size_t runs_{ 0 };
static unsigned int id_;
MonitoringGeneratorMockup(Stage* monitored, PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
MonitoringGeneratorMockup(Stage* monitored, std::initializer_list<double> costs)
: MonitoringGeneratorMockup{ monitored, PredefinedCosts{ std::list<double>{ costs }, true } } {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& s) override;
};
struct ConnectMockup : public Connecting
{
PredefinedCosts costs_;
size_t runs_{ 0 };
static unsigned int id_;
ConnectMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0));
ConnectMockup(std::initializer_list<double> costs)
: ConnectMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
using Connecting::compatible; // make this accessible for testing
void compute(const InterfaceState& from, const InterfaceState& to) override;
};
struct PropagatorMockup : public PropagatingEitherWay
{
PredefinedCosts costs_;
size_t runs_{ 0 };
std::size_t solutions_per_compute_;
static unsigned int id_;
PropagatorMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
PropagatorMockup(std::initializer_list<double> costs)
: PropagatorMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
void computeForward(const InterfaceState& from) override;
void computeBackward(const InterfaceState& to) override;
};
struct ForwardMockup : public PropagatorMockup
{
static unsigned int id_;
ForwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
ForwardMockup(std::initializer_list<double> costs)
: ForwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
};
struct BackwardMockup : public PropagatorMockup
{
static unsigned int id_;
BackwardMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0), std::size_t solutions_per_compute = 1);
BackwardMockup(std::initializer_list<double> costs)
: BackwardMockup{ PredefinedCosts{ std::list<double>{ costs }, true } } {}
};
// reset ids of all Mockup types (used to generate unique stage names)
void resetMockupIds();
// provide a basic test fixture that prepares a Task
struct TaskTestBase : public testing::Test
{
Task t;
TaskTestBase() {
resetMockupIds();
t.setRobotModel(getModel());
}
template <typename C, typename S>
auto add(C& container, S* stage) -> S* {
container.add(Stage::pointer(stage));
return stage;
}
};
#define EXPECT_COSTS(value, matcher) \
{ \
std ::vector<double> costs; \
std::transform(value.begin(), value.end(), std::back_inserter(costs), \
[](const SolutionBaseConstPtr& s) { return s->cost(); }); \
EXPECT_THAT(costs, matcher); \
}
} // namespace task_constructor
} // namespace moveit

View File

@ -4,8 +4,10 @@
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include "models.h"
#include "gtest_value_printers.h"
#include <gtest/gtest.h>
#include <initializer_list>
#include <chrono>
@ -13,101 +15,6 @@
using namespace moveit::task_constructor;
static unsigned int MOCK_ID = 0;
class GeneratorMockup : public Generator
{
moveit::core::RobotModelConstPtr robot;
int runs = 0;
public:
GeneratorMockup(int runs = 0) : Generator("generator " + std::to_string(++MOCK_ID)), runs(runs) {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override { robot = robot_model; }
bool canCompute() const override { return runs > 0; }
void compute() override {
if (runs > 0) {
--runs;
spawn(InterfaceState(std::make_shared<planning_scene::PlanningScene>(robot)), SubTrajectory());
}
}
};
class MonitoringGeneratorMockup : public MonitoringGenerator
{
public:
MonitoringGeneratorMockup(Stage* monitored)
: MonitoringGenerator("monitoring generator " + std::to_string(++MOCK_ID), monitored) {}
bool canCompute() const override { return false; }
void compute() override {}
void onNewSolution(const SolutionBase& s) override {
spawn(InterfaceState(s.end()->scene()->diff()), SubTrajectory());
}
};
class PropagatorMockup : public PropagatingEitherWay
{
int fw_runs = 0;
int bw_runs = 0;
public:
PropagatorMockup(int fw = 0, int bw = 0)
: PropagatingEitherWay("propagate " + std::to_string(++MOCK_ID)), fw_runs(fw), bw_runs(bw) {}
void computeForward(const InterfaceState& from) override {
if (fw_runs > 0) {
--fw_runs;
sendForward(from, InterfaceState(from.scene()->diff()), SubTrajectory());
}
}
void computeBackward(const InterfaceState& to) override {
if (bw_runs > 0) {
--bw_runs;
sendBackward(InterfaceState(to.scene()->diff()), to, SubTrajectory());
}
}
};
class ForwardMockup : public PropagatorMockup
{
public:
ForwardMockup(int runs = 0) : PropagatorMockup(runs, 0) {
restrictDirection(FORWARD);
setName("forward " + std::to_string(MOCK_ID));
}
};
class BackwardMockup : public PropagatorMockup
{
public:
BackwardMockup(int runs = 0) : PropagatorMockup(0, runs) {
restrictDirection(BACKWARD);
setName("backward " + std::to_string(MOCK_ID));
}
};
// ForwardMockup that takes a while for its computation
class TimedForwardMockup : public ForwardMockup
{
std::chrono::milliseconds duration_;
public:
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup(1000), duration_(duration) {}
void computeForward(const InterfaceState& from) override {
std::this_thread::sleep_for(duration_);
ForwardMockup::computeForward(from);
}
};
class ConnectMockup : public Connecting
{
int runs = 0;
public:
ConnectMockup(int runs = 0) : Connecting("connect " + std::to_string(++MOCK_ID)), runs(runs) {}
void compute(const InterfaceState& from, const InterfaceState& to) override {
if (runs > 0)
--runs;
connect(from, to, std::make_shared<SubTrajectory>());
}
};
enum StageType
{
GEN,
@ -116,23 +23,23 @@ enum StageType
ANY,
CONN
};
void append(ContainerBase& c, const std::initializer_list<StageType>& types, int runs = 0) {
void append(ContainerBase& c, const std::initializer_list<StageType>& types) {
for (StageType t : types) {
switch (t) {
case GEN:
c.add(std::make_unique<GeneratorMockup>(runs));
c.add(std::make_unique<GeneratorMockup>());
break;
case FW:
c.add(std::make_unique<ForwardMockup>(runs));
c.add(std::make_unique<ForwardMockup>());
break;
case BW:
c.add(std::make_unique<BackwardMockup>(runs));
c.add(std::make_unique<BackwardMockup>());
break;
case ANY:
c.add(std::make_unique<PropagatorMockup>(runs, runs));
c.add(std::make_unique<PropagatorMockup>());
break;
case CONN:
c.add(std::make_unique<ConnectMockup>(runs));
c.add(std::make_unique<ConnectMockup>());
break;
}
}
@ -196,7 +103,7 @@ protected:
Container container;
InterfacePtr dummy;
InitTest() : ::testing::Test(), dummy(new Interface) {}
InitTest() : ::testing::Test{}, robot_model{ getModel() }, dummy{ new Interface } {}
void pushInterface(bool start = true, bool end = true) {
// pretend, that the container is connected
@ -215,7 +122,7 @@ protected:
}
void initContainer(const std::initializer_list<StageType>& types = {}) {
container.clear();
MOCK_ID = 0;
resetMockupIds();
append(container, types);
}
@ -505,7 +412,7 @@ TEST_F(SerialTest, interface_detection) {
}
template <typename T>
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children, int runs = 0) {
Stage::pointer make(const std::string& name, const std::initializer_list<StageType>& children) {
auto container = new T(name);
append(*container, children);
return Stage::pointer(container);
@ -669,20 +576,16 @@ TEST_F(ParallelTest, init_any) {
}
TEST(Task, move) {
MOCK_ID = 0;
resetMockupIds();
Task t1("foo");
t1.add(std::make_unique<GeneratorMockup>());
t1.add(std::make_unique<GeneratorMockup>());
EXPECT_EQ(t1.stages()->numChildren(), 2u);
MOCK_ID = 0;
resetMockupIds();
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) {
@ -692,13 +595,13 @@ TEST(Task, reuse) {
t.setRobotModel(robot_model);
auto configure = [](Task& t) {
MOCK_ID = 0;
resetMockupIds();
auto ref = new stages::FixedState("fixed");
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
ref->setState(scene);
t.add(Stage::pointer(ref));
t.add(std::make_unique<ConnectMockup>(2));
t.add(std::make_unique<ConnectMockup>());
t.add(std::make_unique<MonitoringGeneratorMockup>(ref));
};
@ -719,13 +622,26 @@ TEST(Task, reuse) {
}
}
// ForwardMockup that takes a while for its computation
class TimedForwardMockup : public ForwardMockup
{
std::chrono::milliseconds duration_;
public:
TimedForwardMockup(std::chrono::milliseconds duration) : ForwardMockup{}, duration_{ duration } {}
void computeForward(const InterfaceState& from) override {
std::this_thread::sleep_for(duration_);
ForwardMockup::computeForward(from);
}
};
TEST(Task, timeout) {
MOCK_ID = 0;
resetMockupIds();
Task t;
t.setRobotModel(getModel());
auto timeout = std::chrono::milliseconds(10);
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::constant(0.0)));
t.add(std::make_unique<TimedForwardMockup>(timeout));
// no timeout, but limited number of solutions
@ -735,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();
@ -749,19 +665,3 @@ TEST(Task, timeout) {
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.solutions().size(), 2u);
}
TEST(Fallback, failing) {
MOCK_ID = 0;
Task t;
t.setRobotModel(getModel());
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>());
fallback->add(std::make_unique<ForwardMockup>());
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}

View File

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

View File

@ -11,88 +11,58 @@
#include <gtest/gtest.h>
#include "stage_mockups.h"
using namespace moveit::task_constructor;
using namespace planning_scene;
const double STAGE_COST{ 7.0 };
constexpr double STAGE_COST{ 7.0 };
constexpr double TERM_COST{ 1.0 };
const double TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator
struct GeneratorCostMockup : public GeneratorMockup
{
PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
public:
GeneratorMockup() : Generator("generator") {
prev.reset(new Interface);
next.reset(new Interface);
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
spawn(std::move(state), STAGE_COST);
}
GeneratorCostMockup() : GeneratorMockup{ PredefinedCosts{ { STAGE_COST }, true } } {}
};
class ConnectMockup : public Connecting
struct ConnectCostMockup : public ConnectMockup
{
using Connecting::Connecting;
void compute(const InterfaceState& from, const InterfaceState& to) override {
auto solution{ std::make_shared<SubTrajectory>() };
solution->setCost(STAGE_COST);
connect(from, to, solution);
}
ConnectCostMockup() : ConnectMockup{ PredefinedCosts::constant(STAGE_COST) } {}
};
class ForwardMockup : public PropagatingForward
struct ForwardCostMockup : public ForwardMockup
{
PlanningScenePtr ps;
ForwardCostMockup() : ForwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
};
public:
using PropagatingForward::PropagatingForward;
struct BackwardCostMockup : public BackwardMockup
{
BackwardCostMockup() : BackwardMockup{ PredefinedCosts::constant(STAGE_COST) } {}
};
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
PropagatingForward::init(robot_model);
}
struct ForwardTrajectoryMockup : public ForwardMockup
{
ForwardTrajectoryMockup(PredefinedCosts&& costs = PredefinedCosts::constant(0.0),
std::size_t solutions_per_compute = 1)
: ForwardMockup{ std::move(costs), solutions_per_compute } {}
void computeForward(const InterfaceState& from) override {
SubTrajectory solution;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
ps->getRobotModel()->getJointModelGroups()[0]) };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
++runs_;
sendForward(from, std::move(to), std::move(solution));
};
};
for (size_t i = 0; i < solutions_per_compute_; ++i) {
SubTrajectory solution;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(from.scene()->getRobotModel(), nullptr) };
planning_scene::PlanningScenePtr ps{ from.scene()->diff() };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST);
InterfaceState to(from);
class BackwardMockup : public PropagatingBackward
{
using PropagatingBackward::PropagatingBackward;
void computeBackward(const InterfaceState& to) override {
SubTrajectory solution;
solution.setCost(STAGE_COST);
InterfaceState from(to);
sendBackward(std::move(from), to, std::move(solution));
};
sendForward(from, std::move(to), std::move(solution));
}
}
};
template <typename T>
@ -105,7 +75,9 @@ public:
InterfaceStatePtr state_start, state_end;
Standalone(const moveit::core::RobotModelConstPtr& robot)
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {
resetMockupIds();
}
// reset and prepare for a compute step
void prepare() {
@ -173,7 +145,7 @@ TEST(CostTerm, SolutionConnected) {
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<ConnectMockup>() };
auto stage{ std::make_unique<ConnectCostMockup>() };
// custom CostTerm to verify SubTrajectory is hooked up to its states & creator
class VerifySolutionCostTerm : public TrajectoryCostTerm
@ -193,38 +165,40 @@ TEST(CostTerm, SolutionConnected) {
const_cast<const SerialContainerPrivate*>(container_.pimpl())->internalToExternalMap().at(s.end()))
<< "SubTrajectory is not connected to its expected end InterfaceState";
EXPECT_EQ(s.creator(), creator_);
return 1.0;
return TERM_COST;
}
};
stage->setCostTerm(std::make_unique<VerifySolutionCostTerm>(container, &*stage));
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "custom CostTerm overwrites stage cost";
EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "custom CostTerm overwrites stage cost";
}
TEST(CostTerm, SetLambdaCostTerm) {
const moveit::core::RobotModelConstPtr robot{ getModel() };
Standalone<SerialContainer> container(robot);
auto stage{ std::make_unique<GeneratorMockup>() };
stage->setCostTerm([](auto&& /*s*/) { return 1.0; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use simple lambda signature";
stage = std::make_unique<GeneratorMockup>();
auto stage{ std::make_unique<GeneratorCostMockup>() };
stage->setCostTerm([](auto&& /*s*/) { return TERM_COST; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), TERM_COST) << "can use simple lambda signature";
stage = std::make_unique<GeneratorCostMockup>();
stage->setCostTerm([](auto&& /*s*/, auto&& /*comment*/) { return 1.0; });
container.computeWithStages({ std::move(stage) });
EXPECT_EQ(container.solutions().front()->cost(), 1.0) << "can use full lambda signature";
stage = std::make_unique<GeneratorMockup>();
stage->setCostTerm([](auto&& /*s*/, auto&& comment) {
comment = "I want the user to see this";
const std::string cost_term_comment{ "I want the user to see this" };
stage = std::make_unique<GeneratorCostMockup>();
stage->setCostTerm([&](auto&& /*s*/, auto&& comment) {
comment = cost_term_comment;
return 1.0;
});
container.computeWithStages({ std::move(stage) });
auto sol = std::dynamic_pointer_cast<const SolutionSequence>(container.solutions().front());
EXPECT_EQ(sol->cost(), 1.0);
EXPECT_EQ(sol->solutions()[0]->comment(), "I want the user to see this") << "can write to comment";
EXPECT_EQ(sol->solutions()[0]->comment(), cost_term_comment) << "can write to comment";
}
TEST(CostTerm, CostOverwrite) {
@ -232,14 +206,14 @@ TEST(CostTerm, CostOverwrite) {
Standalone<SerialContainer> container(robot);
container.computeWithStages({ std::make_unique<GeneratorMockup>() });
container.computeWithStages({ std::make_unique<GeneratorCostMockup>() });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
auto constant_cost{ std::make_shared<cost::Constant>(1.0) };
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
container.computeWithStageCost({ std::make_unique<GeneratorCostMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost->cost) << "custom cost overwrites stage cost";
}
@ -251,16 +225,16 @@ TEST(CostTerm, StageTypes) {
auto constant{ std::make_shared<cost::Constant>(1.0) };
// already tested above
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
// cont.computeWithStageCost(std::make_unique<GeneratorMockupCost>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
container.computeWithStageCost({ std::make_unique<ConnectCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for connect";
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
container.computeWithStageCost({ std::make_unique<ForwardCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for forward propagator";
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
container.computeWithStageCost({ std::make_unique<BackwardCostMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant->cost) << "custom cost works for backward propagator";
}
@ -268,7 +242,7 @@ TEST(CostTerm, PassThroughUsesCost) {
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant);
@ -283,7 +257,7 @@ TEST(CostTerm, PassThroughOverwritesCost) {
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant_inner{ std::make_shared<cost::Constant>(84.0) };
stage->setCostTerm(constant_inner);
@ -300,7 +274,7 @@ TEST(CostTerm, PassThroughCanModifyCost) {
moveit::core::RobotModelPtr robot{ getModel() };
Standalone<stages::PassThrough> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
auto stage{ std::make_unique<BackwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(8.0) };
stage->setCostTerm(constant);
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
@ -315,18 +289,18 @@ TEST(CostTerm, CompositeSolutions) {
Standalone<SerialContainer> container{ getModel() };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto s1{ std::make_unique<ForwardCostMockup>() };
auto s2{ std::make_unique<ForwardCostMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
}
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s1{ std::make_unique<ForwardCostMockup>() };
auto constant{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant);
auto s2{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardCostMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant->cost)
@ -334,8 +308,8 @@ TEST(CostTerm, CompositeSolutions) {
}
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto s1{ std::make_unique<ForwardCostMockup>() };
auto s2{ std::make_unique<ForwardCostMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
auto constant1{ std::make_shared<cost::Constant>(1.0) };
s1->setCostTerm(constant1);
@ -343,7 +317,7 @@ TEST(CostTerm, CompositeSolutions) {
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
auto s3{ std::make_unique<ForwardCostMockup>() };
auto constant2{ std::make_shared<cost::Constant>(9.0) };
s3->setCostTerm(constant2);
@ -356,15 +330,15 @@ TEST(CostTerm, CompositeSolutions) {
TEST(CostTerm, CompositeSolutionsContainerCost) {
Standalone<SerialContainer> container{ getModel() };
auto s1{ std::make_unique<ForwardMockup>() };
auto s1{ std::make_unique<ForwardTrajectoryMockup>() };
auto s1_ptr{ s1.get() };
auto s2{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardTrajectoryMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
auto s3{ std::make_unique<ForwardTrajectoryMockup>() };
container.setCostTerm(std::make_unique<cost::TrajectoryDuration>());
container.computeWithStages({ std::move(c1), std::move(s3) });

176
core/test/test_fallback.cpp Normal file
View File

@ -0,0 +1,176 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/task_p.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include "stage_mockups.h"
#include "models.h"
#include "gtest_value_printers.h"
#include <gtest/gtest.h>
#include <initializer_list>
#include <chrono>
#include <thread>
using namespace moveit::task_constructor;
using FallbacksFixtureGenerator = TaskTestBase;
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)));
fallback->add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(2.0)));
t.add(std::move(fallback));
EXPECT_TRUE(t.plan());
ASSERT_EQ(t.solutions().size(), 1u);
EXPECT_EQ(t.solutions().front()->cost(), 1.0);
}
using FallbacksFixturePropagate = TaskTestBase;
TEST_F(FallbacksFixturePropagate, failingNoSolutions) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({}), 0));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({}), 0));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST_F(FallbacksFixturePropagate, failingWithFailedSolutions) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts::single(0.0)));
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(INF)));
t.add(std::move(fallback));
EXPECT_FALSE(t.plan());
EXPECT_EQ(t.solutions().size(), 0u);
}
TEST_F(FallbacksFixturePropagate, computeFirstSuccessfulStageOnly) {
t.add(std::make_unique<GeneratorMockup>());
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_EQ(t.numSolutions(), 1u);
}
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));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, INF, 110.0, 120.0 })));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 210.0, 220.0, 0, 0 })));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(113, 124, 212, 221));
}
// 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
// use a fallback container to delay computation twice: only the last child succeeds
auto inner = std::make_unique<Fallbacks>("Inner");
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false)));
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(0.0)));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::move(inner));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan(1)); // only return 1st solution
EXPECT_COSTS(t.solutions(), testing::ElementsAre(2)); // expecting less costly solution as result
}
// 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");
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF }, false))); // always fail
inner->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 10.0, INF, 30.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::move(inner));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF })));
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 33));
// check that first solution is not marked as pruned
}
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({ INF, 1.0 }), 2));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::single(2.0)));
t.add(std::move(fallback));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, successfulWithMixedSolutions2) {
t.add(std::make_unique<GeneratorMockup>());
auto fallback = std::make_unique<Fallbacks>("Fallbacks");
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts({ 1.0, INF }), 2));
fallback->add(std::make_unique<ForwardMockup>(PredefinedCosts::single(2.0)));
t.add(std::move(fallback));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(1.0));
}
TEST_F(FallbacksFixturePropagate, activeChildReset) {
t.add(std::make_unique<GeneratorMockup>(PredefinedCosts({ 1.0, INF, 3.0 })));
auto fallbacks = std::make_unique<Fallbacks>("Fallbacks");
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(10.0)));
fallbacks->add(std::make_unique<ForwardMockup>(PredefinedCosts::constant(20.0)));
auto fwd1 = fallbacks->findChild("FWD1");
auto fwd2 = fallbacks->findChild("FWD2");
t.add(std::move(fallbacks));
EXPECT_TRUE(t.plan());
EXPECT_COSTS(t.solutions(), testing::ElementsAre(11, 13));
EXPECT_COSTS(fwd1->solutions(), testing::ElementsAre(10, 10));
EXPECT_COSTS(fwd2->solutions(), testing::IsEmpty());
}
using FallbacksFixtureConnect = TaskTestBase;
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({ 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, 22, 121));
}

View File

@ -19,7 +19,7 @@ TEST(InterfaceStatePriority, compare) {
EXPECT_TRUE(Prio(1, 42) < Prio(0, 0));
EXPECT_TRUE(Prio(0, 0) < Prio(0, 42)); // at same depth, higher cost is larger
auto dstart = InterfaceState::DISABLED_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::DISABLED_FAILED));
i.updatePriority(*i.begin(), Prio(6, 0, InterfaceState::Status::ARMED));
EXPECT_THAT(i.depths(), ::testing::ElementsAreArray({ 3, 6 }));
}
@ -86,9 +86,12 @@ TEST(StatePairs, compare) {
EXPECT_TRUE(pair(Prio(1, 0), Prio(0, 1)) < pair(Prio(1, 1), Prio(0, 1)));
EXPECT_TRUE(pair(Prio(1, 1), Prio(1, 1)) < pair(Prio(1, 0), Prio(0, 0)));
auto good = InterfaceState::ENABLED;
auto bad = InterfaceState::DISABLED_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 = InterfaceState::Status::ENABLED;
auto good_good = pair(Prio(0, 10, good), Prio(0, 0, good));
ASSERT_TRUE(good_good > pair(good, good)); // a bad status should reverse this relation
for (auto bad : { InterfaceState::Status::ARMED, InterfaceState::Status::PRUNED }) {
EXPECT_TRUE(good_good < pair(bad, good));
EXPECT_TRUE(good_good < pair(good, bad));
EXPECT_TRUE(good_good < pair(bad, bad));
}
}

View File

@ -0,0 +1,129 @@
#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/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#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;
const JointModelGroup* group;
PandaMoveRelative() {
t.setRobotModel(loadModel());
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::AttachedCollisionObject createAttachedObject(const std::string& id) {
moveit_msgs::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::SolidPrimitive p;
p.type = p.SPHERE;
p.dimensions.resize(1);
p.dimensions[p.SPHERE_RADIUS] = 0.01;
return p;
}());
geometry_msgs::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::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::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);
ros::init(argc, argv, "move_relative_test");
ros::AsyncSpinner spinner(1);
spinner.start();
return RUN_ALL_TESTS();
}

170
core/test/test_move_to.cpp Normal file
View File

@ -0,0 +1,170 @@
#include "models.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit_msgs/RobotState.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/console.h>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
using namespace moveit::core;
constexpr double TAU{ 2 * M_PI };
// provide a basic test fixture that prepares a Task
struct PandaMoveTo : public testing::Test
{
Task t;
stages::MoveTo* move_to;
PlanningScenePtr scene;
PandaMoveTo() {
t.setRobotModel(loadModel());
scene = std::make_shared<PlanningScene>(t.getRobotModel());
scene->getCurrentStateNonConst().setToDefaultValues();
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
"extended");
t.add(std::make_unique<stages::FixedState>("start", scene));
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
move_to = move.get();
move_to->setGroup("panda_arm");
t.add(std::move(move));
}
};
#define EXPECT_ONE_SOLUTION \
{ \
EXPECT_TRUE(t.plan()); \
EXPECT_EQ(t.solutions().size(), 1u); \
}
TEST_F(PandaMoveTo, namedTarget) {
move_to->setGoal("ready");
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, mapTarget) {
move_to->setGoal(std::map<std::string, double>{ { "panda_joint1", TAU / 8 }, { "panda_joint2", TAU / 8 } });
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, stateTarget) {
move_to->setGoal([] {
moveit_msgs::RobotState state;
state.is_diff = true;
state.joint_state.name = { "panda_joint1", "panda_joint2" };
state.joint_state.position = { TAU / 8, TAU / 8 };
return state;
}());
EXPECT_ONE_SOLUTION;
}
geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) {
state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose);
auto frame_eigen{ state.getFrameTransform(frame) };
geometry_msgs::PoseStamped p;
p.header.frame_id = state.getRobotModel()->getModelFrame();
p.pose = tf2::toMsg(frame_eigen);
return p;
}
TEST_F(PandaMoveTo, pointTarget) {
geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") };
geometry_msgs::PointStamped point;
point.header = pose.header;
point.point = pose.pose.position;
move_to->setGoal(point);
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, poseTarget) {
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8"));
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
const std::string IK_FRAME{ "panda_hand" };
move_to->setIKFrame(IK_FRAME);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
EXPECT_ONE_SOLUTION;
}
moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) {
moveit_msgs::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::SolidPrimitive p;
p.type = p.SPHERE;
p.dimensions.resize(1);
p.dimensions[p.SPHERE_RADIUS] = 0.01;
return p;
}());
#if MOVEIT_HAS_OBJECT_POSE
aco.object.pose.position.z = 0.2;
aco.object.pose.orientation.w = 1.0;
#else
aco.object.primitive_poses.resize(1);
aco.object.primitive_poses[0].position.z = 0.2;
aco.object.primitive_poses[0].orientation.w = 1.0;
#endif
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
// If we don't have this, we also don't have subframe support
aco.object.subframe_names.resize(1, "subframe");
aco.object.subframe_poses.resize(1, [] {
geometry_msgs::Pose p;
p.orientation.w = 1.0;
return p;
}());
#endif
return aco;
}
TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
move_to->setIKFrame(ATTACHED_OBJECT);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT));
EXPECT_ONE_SOLUTION;
}
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
// If we don't have this, we also don't have subframe support
TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" };
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
move_to->setIKFrame(IK_FRAME);
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
EXPECT_ONE_SOLUTION;
}
#endif
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "move_to_test");
ros::AsyncSpinner spinner(1);
spinner.start();
return RUN_ALL_TESTS();
}

195
core/test/test_pruning.cpp Normal file
View File

@ -0,0 +1,195 @@
#include <moveit/task_constructor/task.h>
#include "stage_mockups.h"
#include "models.h"
#include <list>
#include <memory>
#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;
TEST_F(Pruning, PropagatorFailure) {
auto back = add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
add(t, new ForwardMockup({ INF }));
EXPECT_FALSE(t.plan());
ASSERT_EQ(t.solutions().size(), 0u);
// ForwardMockup fails, so the backward stage should never compute
EXPECT_EQ(back->runs_, 0u);
}
TEST_F(Pruning, PruningMultiForward) {
add(t, new BackwardMockup());
add(t, new BackwardMockup());
add(t, new GeneratorMockup());
// spawn two solutions for the only incoming state
add(t, new ForwardMockup(PredefinedCosts{ { 0.0, 0.0 } }, 2));
// fail to extend the second solution
add(t, new ForwardMockup({ 0, INF }));
EXPECT_TRUE(t.plan());
// the second (infeasible) solution in the last stage must not disable
// the earlier partial solution just because they share stage solutions
ASSERT_EQ(t.solutions().size(), 1u);
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 }));
add(t, new ForwardMockup());
auto c2 = add(t, new ConnectMockup());
add(t, new GeneratorMockup({ 1, 2, 3 }));
t.plan();
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_EQ(c1->runs_, 3u);
EXPECT_EQ(c2->runs_, 6u); // expect 6 instead of 9 calls
}
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());
add(t, new GeneratorMockup({ 0, INF, 10, 20 })); // 2nd is a dummy to postpone creation of 3rd
auto c2 = add(t, new ConnectMockup({ INF, 0, 0, 0 })); // 1st attempt is a failure
add(t, new GeneratorMockup());
t.plan();
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_EQ(c1->runs_, 6u); // expect 6 instead of 9 calls
EXPECT_EQ(c2->runs_, 3u);
}
TEST_F(Pruning, PropagateIntoContainer) {
add(t, new BackwardMockup({ INF }));
add(t, new GeneratorMockup({ 0 }));
auto inner = add(t, new SerialContainer());
auto con = add(*inner, new ConnectMockup());
add(*inner, new GeneratorMockup({ 0 }));
EXPECT_FALSE(t.plan());
// the failure in the backward stage (outside the container)
// should prune the expected computation of con inside the container
EXPECT_EQ(con->runs_, 0u);
}
TEST_F(Pruning, PropagateFromContainerPull) {
auto back = add(t, new BackwardMockup());
add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
auto inner = add(t, new SerialContainer());
add(*inner, new ForwardMockup());
add(*inner, new ForwardMockup({ INF }));
EXPECT_FALSE(t.plan());
// the failure inside the container should prune computing of back
EXPECT_EQ(back->runs_, 0u);
}
TEST_F(Pruning, PropagateFromContainerPush) {
auto inner = add(t, new SerialContainer());
add(*inner, new BackwardMockup({ INF }));
add(t, new GeneratorMockup({ 0 }));
auto con = add(t, new ConnectMockup());
add(t, new GeneratorMockup({ 0 }));
EXPECT_FALSE(t.plan());
// the failure inside container should prune computing of con
EXPECT_EQ(con->runs_, 0u);
}
TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
auto back = add(t, new BackwardMockup());
add(t, new GeneratorMockup({ 0 }));
auto inner = add(t, new Alternatives());
add(*inner, new ForwardMockup({ INF }));
auto serial = add(*inner, new SerialContainer());
add(*serial, new ConnectMockup());
add(*serial, new GeneratorMockup({ 0 }));
EXPECT_TRUE(t.plan());
// the failure in one branch of Alternatives must not prune computing back
EXPECT_EQ(back->runs_, 1u);
}

View File

@ -5,6 +5,7 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/utils/robot_model_test_utils.h>
#include "stage_mockups.h"
#include "models.h"
#include <list>
#include <memory>
@ -13,105 +14,11 @@
using namespace moveit::task_constructor;
using namespace planning_scene;
MOVEIT_STRUCT_FORWARD(PredefinedCosts);
struct PredefinedCosts : CostTerm
{
mutable std::list<double> costs_; // list of costs to assign
mutable double cost_ = 0.0; // last assigned cost
bool finite_; // finite number of compute() attempts?
PredefinedCosts(bool finite, std::list<double>&& costs) : costs_(std::move(costs)), finite_(finite) {}
bool exhausted() const { return finite_ && costs_.empty(); }
double cost() const {
if (!costs_.empty()) {
cost_ = costs_.front();
costs_.pop_front();
}
return cost_;
}
double operator()(const SubTrajectory& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const SolutionSequence& /*s*/, std::string& /*comment*/) const override { return cost(); }
double operator()(const WrappedSolution& /*s*/, std::string& /*comment*/) const override { return cost(); }
};
/** Generator creating solutions with given costs */
struct GeneratorMockup : Generator
{
PlanningScenePtr ps_;
PredefinedCosts costs_;
static unsigned int id_;
GeneratorMockup(std::initializer_list<double> costs = { 0.0 })
: Generator("GEN" + std::to_string(++id_)), costs_(true, costs) {}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps_.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return !costs_.exhausted(); }
void compute() override { spawn(InterfaceState(ps_), costs_.cost()); }
};
struct PropagatorMockup : public PropagatingEitherWay
{
PredefinedCosts costs_;
std::size_t solutions_per_compute_;
unsigned int calls_ = 0;
PropagatorMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
: PropagatingEitherWay(), costs_(false, costs), solutions_per_compute_(solutions_per_compute) {}
void computeForward(const InterfaceState& from) override {
++calls_;
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
sendForward(from, InterfaceState(from.scene()->diff()), std::move(solution));
}
}
void computeBackward(const InterfaceState& to) override {
++calls_;
for (std::size_t i = 0; i < solutions_per_compute_; ++i) {
SubTrajectory solution(robot_trajectory::RobotTrajectoryConstPtr(), costs_.cost());
sendBackward(InterfaceState(to.scene()->diff()), to, std::move(solution));
}
}
};
struct ForwardMockup : public PropagatorMockup
{
static unsigned int id_;
ForwardMockup(std::initializer_list<double> costs = { 0.0 }, std::size_t solutions_per_compute = 1)
: PropagatorMockup(costs, solutions_per_compute) {
restrictDirection(FORWARD);
setName("FW" + std::to_string(++id_));
}
};
struct BackwardMockup : public PropagatorMockup
{
static unsigned int id_;
BackwardMockup(std::initializer_list<double> costs = { 0.0 }) : PropagatorMockup(costs) {
restrictDirection(BACKWARD);
setName("BW" + std::to_string(++id_));
}
};
/* Forward propagator, contributing no solutions at all */
struct ForwardDummy : PropagatingForward
{
using PropagatingForward::PropagatingForward;
void computeForward(const InterfaceState& /*from*/) override {}
};
/* Connect creating solutions with given costs */
struct Connect : stages::Connect
{
PlanningScenePtr ps_;
PredefinedCostsPtr costs_;
unsigned int calls_ = 0;
unsigned int calls_{ 0 };
static unsigned int id_;
static GroupPlannerVector getPlanners() {
@ -119,9 +26,9 @@ struct Connect : stages::Connect
return { { "group", planner }, { "eef_group", planner } };
}
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false)
Connect(std::initializer_list<double> costs = { 0.0 }, bool enforce_sequential = false)
: stages::Connect("CON" + std::to_string(++id_), getPlanners()) {
costs_ = std::make_shared<PredefinedCosts>(false, costs);
costs_ = std::make_shared<PredefinedCosts>(costs, false);
setCostTerm(costs_);
if (enforce_sequential)
setProperty("merge_mode", SEQUENTIAL);
@ -132,47 +39,27 @@ struct Connect : stages::Connect
}
};
constexpr double INF = std::numeric_limits<double>::infinity();
unsigned int GeneratorMockup::id_ = 0;
unsigned int ForwardMockup::id_ = 0;
unsigned int BackwardMockup::id_ = 0;
unsigned int Connect::id_ = 0;
struct TestBase : public testing::Test
struct TestBase : public TaskTestBase
{
Task task;
TestBase() {
resetIds();
task.setRobotModel(getModel());
}
void resetIds() {
GeneratorMockup::id_ = 0;
ForwardMockup::id_ = 0;
BackwardMockup::id_ = 0;
Connect::id_ = 0;
}
template <typename C, typename S>
auto add(C& container, S* stage) -> S* {
container.add(Stage::pointer(stage));
return stage;
}
TestBase() { Connect::id_ = 0; }
};
using ConnectConnect = TestBase;
// https://github.com/ros-planning/moveit_task_constructor/issues/182
TEST_F(ConnectConnect, SuccSucc) {
add(task, new GeneratorMockup({ 1, 2, 3 }));
add(task, new Connect());
add(task, new GeneratorMockup({ 10, 20 }));
add(task, new Connect());
add(task, new GeneratorMockup());
add(t, new GeneratorMockup({ 1.0, 2.0, 3.0 }));
add(t, new Connect());
add(t, new GeneratorMockup({ 10.0, 20.0 }));
add(t, new Connect());
add(t, new GeneratorMockup({ 0.0 }));
EXPECT_TRUE(task.plan());
ASSERT_EQ(task.solutions().size(), 3u * 2u);
EXPECT_TRUE(t.plan());
ASSERT_EQ(t.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : task.solutions()) {
for (const auto& s : t.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
@ -180,143 +67,12 @@ TEST_F(ConnectConnect, SuccSucc) {
// https://github.com/ros-planning/moveit_task_constructor/issues/218
TEST_F(ConnectConnect, FailSucc) {
add(task, new GeneratorMockup());
add(task, new Connect({ INF }, true));
add(task, new GeneratorMockup());
add(task, new Connect());
add(task, new GeneratorMockup());
add(task, new ForwardDummy());
add(t, new GeneratorMockup());
add(t, new Connect({ INF }, true));
add(t, new GeneratorMockup());
add(t, new Connect());
add(t, new GeneratorMockup());
add(t, new ForwardMockup(PredefinedCosts::constant(0.0), 0));
EXPECT_FALSE(task.plan());
}
using Pruning = TestBase;
TEST_F(Pruning, PropagatorFailure) {
auto back = add(task, new BackwardMockup());
add(task, new GeneratorMockup({ 0 }));
add(task, new ForwardMockup({ INF }));
EXPECT_FALSE(task.plan());
ASSERT_EQ(task.solutions().size(), 0u);
// ForwardMockup fails, so the backward stage should never compute
EXPECT_EQ(back->calls_, 0u);
}
TEST_F(Pruning, PruningMultiForward) {
add(task, new BackwardMockup());
add(task, new BackwardMockup());
add(task, new GeneratorMockup());
// spawn two solutions for the only incoming state
add(task, new ForwardMockup({ 0, 0 }, 2));
// fail to extend the second solution
add(task, new ForwardMockup({ 0, INF }));
EXPECT_TRUE(task.plan());
// the second (infeasible) solution in the last stage must not disable
// the earlier partial solution just because they share stage solutions
ASSERT_EQ(task.solutions().size(), 1u);
EXPECT_EQ((*task.solutions().begin())->cost(), 0u);
}
TEST_F(Pruning, ConnectConnectForward) {
add(task, new GeneratorMockup());
auto c1 = add(task, new Connect({ INF, 0 })); // 1st attempt is a failue
add(task, new GeneratorMockup({ 0, 10, 20 }));
add(task, new ForwardMockup());
auto c2 = add(task, new Connect());
add(task, new GeneratorMockup({ 1, 2, 3 }));
task.plan();
ASSERT_EQ(task.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : task.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_EQ(c1->calls_, 3u);
EXPECT_EQ(c2->calls_, 6u); // expect 6 instead of 9 calls
}
TEST_F(Pruning, ConnectConnectBackward) {
add(task, new GeneratorMockup({ 1, 2, 3 }));
auto c1 = add(task, new Connect());
add(task, new BackwardMockup());
add(task, new GeneratorMockup({ 0, INF, 10, 20 })); // 2nd is a dummy to postpone creation of 3rd
auto c2 = add(task, new Connect({ INF, 0 })); // 1st attempt is a failure
add(task, new GeneratorMockup());
task.plan();
ASSERT_EQ(task.solutions().size(), 3u * 2u);
std::vector<double> expected_costs = { 11, 12, 13, 21, 22, 23 };
auto expected_cost = expected_costs.begin();
for (const auto& s : task.solutions()) {
EXPECT_EQ(s->cost(), *expected_cost);
++expected_cost;
}
EXPECT_EQ(c1->calls_, 6u); // expect 6 instead of 9 calls
EXPECT_EQ(c2->calls_, 3u);
}
TEST_F(Pruning, PropagateIntoContainer) {
add(task, new BackwardMockup({ INF }));
add(task, new GeneratorMockup({ 0 }));
auto inner = add(task, new SerialContainer());
auto con = add(*inner, new Connect());
add(*inner, new GeneratorMockup({ 0 }));
EXPECT_FALSE(task.plan());
// the failure in the backward stage (outside the container)
// should prune the expected computation of con inside the container
EXPECT_EQ(con->calls_, 0u);
}
TEST_F(Pruning, PropagateFromContainerPull) {
auto back = add(task, new BackwardMockup());
add(task, new BackwardMockup());
add(task, new GeneratorMockup({ 0 }));
auto inner = add(task, new SerialContainer());
add(*inner, new ForwardMockup());
add(*inner, new ForwardMockup({ INF }));
EXPECT_FALSE(task.plan());
// the failure inside the container should prune computing of back
EXPECT_EQ(back->calls_, 0u);
}
TEST_F(Pruning, PropagateFromContainerPush) {
auto inner = add(task, new SerialContainer());
add(*inner, new BackwardMockup({ INF }));
add(task, new GeneratorMockup({ 0 }));
auto con = add(task, new Connect());
add(task, new GeneratorMockup({ 0 }));
EXPECT_FALSE(task.plan());
// the failure inside container should prune computing of con
EXPECT_EQ(con->calls_, 0u);
}
TEST_F(Pruning, PropagateFromParallelContainerMultiplePaths) {
auto back = add(task, new BackwardMockup());
add(task, new GeneratorMockup({ 0 }));
auto inner = add(task, new Alternatives());
add(*inner, new ForwardMockup({ INF }));
auto serial = add(*inner, new SerialContainer());
add(*serial, new Connect());
add(*serial, new GeneratorMockup({ 0 }));
EXPECT_TRUE(task.plan());
// the failure in one branch of Alternatives must not prune computing back
EXPECT_EQ(back->calls_, 1u);
EXPECT_FALSE(t.plan());
}

View File

@ -6,52 +6,36 @@
#include <moveit/planning_scene/planning_scene.h>
#include <geometry_msgs/PoseStamped.h>
#include "stage_mockups.h"
#include <ros/console.h>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
class GeneratorMockup : public Generator
struct StandaloneGeneratorMockup : public GeneratorMockup
{
PlanningScenePtr ps;
InterfacePtr prev;
InterfacePtr next;
public:
GeneratorMockup() : Generator("generator") {
StandaloneGeneratorMockup(std::initializer_list<double>&& costs)
: StandaloneGeneratorMockup{ PredefinedCosts{ std::move(costs), true } } {}
StandaloneGeneratorMockup(PredefinedCosts&& costs = PredefinedCosts{ { 0.0 }, true })
: GeneratorMockup{ std::move(costs) } {
prev.reset(new Interface);
next.reset(new Interface);
pimpl()->setPrevEnds(prev);
pimpl()->setNextStarts(next);
}
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset((new PlanningScene(robot_model)));
Generator::init(robot_model);
}
bool canCompute() const override { return true; }
void compute() override {
InterfaceState state(ps);
state.properties().set("target_pose", geometry_msgs::PoseStamped());
spawn(std::move(state), 0.0);
}
};
class ConnectMockup : public Connecting
{
public:
using Connecting::compatible;
void compute(const InterfaceState& from, const InterfaceState& to) override {}
};
TEST(Stage, registerCallbacks) {
GeneratorMockup g;
StandaloneGeneratorMockup g{ PredefinedCosts::constant(0.0) };
g.init(getModel());
uint called = 0;
auto cb = [&called](const SolutionBase& s) {
auto cb = [&called](const SolutionBase& /* s */) {
++called;
return true;
};

View File

@ -1,7 +1,11 @@
cmake_minimum_required(VERSION 3.1.3)
project(moveit_task_constructor_demo)
add_compile_options(-std=c++14)
if(NOT "${CMAKE_CXX_STANDARD}")
set(CMAKE_CXX_STANDARD 14)
endif()
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
find_package(catkin REQUIRED COMPONENTS
roscpp
@ -16,33 +20,41 @@ catkin_package(
)
include_directories(
include
${catkin_INCLUDE_DIRS}
include
${catkin_INCLUDE_DIRS}
)
add_executable(cartesian src/cartesian.cpp)
target_link_libraries(cartesian ${catkin_LIBRARIES})
add_executable(modular src/modular.cpp)
target_link_libraries(modular ${catkin_LIBRARIES})
add_library(pick_place_task OBJECT src/pick_place_task.cpp)
add_executable(pick_place_demo src/pick_place_demo.cpp $<TARGET_OBJECTS:pick_place_task>)
target_link_libraries(pick_place_demo ${catkin_LIBRARIES})
add_executable(alternative_path_costs src/alternative_path_costs.cpp)
target_link_libraries(alternative_path_costs ${catkin_LIBRARIES})
add_executable(ik_clearance_cost src/ik_clearance_cost.cpp)
target_link_libraries(ik_clearance_cost ${catkin_LIBRARIES})
install(TARGETS cartesian modular pick_place_demo
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
add_library(${PROJECT_NAME}_pick_place_task src/pick_place_task.cpp)
target_link_libraries(${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES})
add_dependencies(${PROJECT_NAME}_pick_place_task ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
install(TARGETS ${PROJECT_NAME}_pick_place_task
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# declare a demo consisting of a single cpp file
function(demo name)
add_executable(${PROJECT_NAME}_${name} src/${name}.cpp)
add_dependencies(${PROJECT_NAME}_${name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}_${name} ${catkin_LIBRARIES})
set_target_properties(${PROJECT_NAME}_${name} PROPERTIES OUTPUT_NAME ${name} PREFIX "")
install(TARGETS ${PROJECT_NAME}_${name}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
endfunction()
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)
install(DIRECTORY launch config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

View File

@ -2,7 +2,7 @@
<package format="2">
<name>moveit_task_constructor_demo</name>
<version>0.0.1</version>
<description>The moveit_task_constructor_demo package</description>
<description>demo tasks illustrating various capabilities of MTC.</description>
<author email="simongold@picknik.ai">simon Goldstein</author>
<author email="henningkayser@picknik.ai">Henning Kayser</author>

View File

@ -0,0 +1,142 @@
#include <ros/ros.h>
#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) {
ros::init(argc, argv, "mtc_tutorial");
ros::AsyncSpinner spinner{ 1 };
spinner.start();
// setup Task
Task t;
t.loadRobotModel();
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 = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner") };
pp->setPlannerId("PTP");
return pp;
}();
const auto rrtconnect = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("ompl") };
pp->setPlannerId("RRTConnectkConfigDefault");
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::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::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::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 {
t.init();
std::cout << t << std::endl;
t.plan();
} catch (const InitStageException& e) {
std::cout << e << std::endl;
}
ros::waitForShutdown();
return 0;
}

View File

@ -429,9 +429,7 @@ bool PickPlaceTask::init() {
*****************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
stage->allowCollisions(
object_name_,
t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false);
stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false);
place->insert(std::move(stage));
}
@ -492,7 +490,7 @@ bool PickPlaceTask::plan() {
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
int max_solutions = pnh_.param<int>("max_solutions", 10);
return task_->plan(max_solutions);
return static_cast<bool>(task_->plan(max_solutions));
}
bool PickPlaceTask::execute() {

View File

@ -6,8 +6,6 @@
if (CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
add_executable(pick_place_test pick_place_test.cpp $<TARGET_OBJECTS:pick_place_task>)
target_link_libraries(pick_place_test ${catkin_LIBRARIES} gtest_main)
add_rostest(pick_place.test)
add_rostest_gtest(pick_place_test pick_place.test pick_place_test.cpp)
target_link_libraries(pick_place_test ${PROJECT_NAME}_pick_place_task ${catkin_LIBRARIES})
endif()

View File

@ -1,8 +1,9 @@
<?xml version="1.0"?>
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/demo.launch">
<include file="$(find moveit_resources_panda_moveit_config)/launch/demo.launch" pass_all_args="true">
<arg name="use_rviz" value="false" />
<arg name="execution_type" value="last point" />
<arg name="execution_type" value="last point" /> <!-- arg name in 0.8 -->
<arg name="fake_execution_type" value="last point" /> <!-- arg name in 0.8.1 -->
<env name="LD_PRELOAD" value="$(optenv PRELOAD)" />
</include>

View File

@ -1,6 +1,6 @@
#include <rviz_marker_tools/marker_creation.h>
#include <urdf_model/link.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
namespace vm = visualization_msgs;
@ -139,21 +139,17 @@ std_msgs::ColorRGBA getColor(Color color, double alpha) {
}
geometry_msgs::Pose composePoses(const geometry_msgs::Pose& first, const Eigen::Isometry3d& second) {
geometry_msgs::Pose result;
Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(first, result_eigen);
tf2::fromMsg(first, result_eigen);
result_eigen = result_eigen * second;
tf::poseEigenToMsg(result_eigen, result);
return result;
return tf2::toMsg(result_eigen);
}
geometry_msgs::Pose composePoses(const Eigen::Isometry3d& first, const geometry_msgs::Pose& second) {
geometry_msgs::Pose result;
Eigen::Isometry3d result_eigen;
tf::poseMsgToEigen(second, result_eigen);
tf2::fromMsg(second, result_eigen);
result_eigen = first * result_eigen;
tf::poseEigenToMsg(result_eigen, result);
return result;
return tf2::toMsg(result_eigen);
}
void prepareMarker(vm::Marker& m, int marker_type) {
@ -285,8 +281,8 @@ vm::Marker& makeArrow(vm::Marker& m, const Eigen::Vector3d& start_point, const E
m.scale.z = head_length;
prepareMarker(m, vm::Marker::ARROW);
m.points.resize(2);
tf::pointEigenToMsg(start_point, m.points[0]);
tf::pointEigenToMsg(end_point, m.points[1]);
m.points[0] = tf2::toMsg(start_point);
m.points[1] = tf2::toMsg(end_point);
return m;
}

View File

@ -65,7 +65,7 @@ QModelIndex LocalTaskModel::index(Node* n) const {
const ContainerBase* parent = n->parent();
// the internal pointer refers to n
int row = 0;
size_t row = 0;
auto find_row = [n, &row](const Stage& child, int /* depth */) -> bool {
if (&child == n)
return false; // found, don't continue traversal
@ -73,7 +73,7 @@ QModelIndex LocalTaskModel::index(Node* n) const {
return true;
};
parent->traverseChildren(find_row);
Q_ASSERT(row < (int)parent->numChildren());
Q_ASSERT(row < parent->numChildren());
return createIndex(row, 0, n);
}

View File

@ -150,7 +150,7 @@ public:
}
template <class Derived>
void addBuiltInClass(const QString& name, const QString& description) {
addBuiltInClass("Built Ins", name, description, []() { return new Derived(); });
addBuiltInClass("Built Ins", name, description, [] { return new Derived(); });
}
/** @brief Instantiate and return a instance of a subclass of Type using our

View File

@ -179,6 +179,7 @@ void TaskDisplay::calculateOffsetPosition() {
void TaskDisplay::update(float wall_dt, float ros_dt) {
requestPanel();
Display::update(wall_dt, ros_dt);
calculateOffsetPosition();
trajectory_visual_->update(wall_dt, ros_dt);
}