mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'master' into wip-python-api
This commit is contained in:
commit
86093be94a
38
.github/workflows/ci.yaml
vendored
38
.github/workflows/ci.yaml
vendored
@ -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
|
||||
|
||||
6
.github/workflows/format.yaml
vendored
6
.github/workflows/format.yaml
vendored
@ -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
|
||||
|
||||
@ -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"]
|
||||
|
||||
@ -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}
|
||||
)
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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) &&
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
59
core/include/moveit/task_constructor/moveit_compat.h
Normal file
59
core/include/moveit/task_constructor/moveit_compat.h
Normal 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)
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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_;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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>
|
||||
|
||||
@ -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__":
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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");
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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(
|
||||
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
@ -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)));
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
|
||||
@ -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" });
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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
113
core/src/utils.cpp
Normal 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
|
||||
@ -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)
|
||||
|
||||
@ -18,6 +18,6 @@ RobotModelPtr getModel() {
|
||||
}
|
||||
|
||||
moveit::core::RobotModelPtr loadModel() {
|
||||
robot_model_loader::RobotModelLoader loader;
|
||||
static robot_model_loader::RobotModelLoader loader;
|
||||
return loader.getModel();
|
||||
}
|
||||
|
||||
7
core/test/move_relative.test
Normal file
7
core/test/move_relative.test
Normal 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
7
core/test/move_to.test
Normal 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
128
core/test/stage_mockups.cpp
Normal 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
152
core/test/stage_mockups.h
Normal 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
|
||||
@ -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);
|
||||
}
|
||||
|
||||
@ -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 });
|
||||
|
||||
@ -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
176
core/test/test_fallback.cpp
Normal 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));
|
||||
}
|
||||
@ -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));
|
||||
}
|
||||
}
|
||||
|
||||
129
core/test/test_move_relative.cpp
Normal file
129
core/test/test_move_relative.cpp
Normal 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
170
core/test/test_move_to.cpp
Normal 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
195
core/test/test_pruning.cpp
Normal 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);
|
||||
}
|
||||
@ -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());
|
||||
}
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
@ -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}
|
||||
)
|
||||
|
||||
@ -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>
|
||||
|
||||
142
demo/src/fallbacks_move_to.cpp
Normal file
142
demo/src/fallbacks_move_to.cpp
Normal 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;
|
||||
}
|
||||
@ -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() {
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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>
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user