mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
ModifyPlanningScene stage
- attach / detach objects to robot - enable / disable collision pairs - works in either direction (FORWARD + BACKWARD)
This commit is contained in:
parent
3a4dc6755b
commit
381edf2d22
@ -0,0 +1,173 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/stage.h>
|
||||||
|
#include <deque>
|
||||||
|
#include <map>
|
||||||
|
|
||||||
|
namespace moveit { namespace planning_interface {
|
||||||
|
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
||||||
|
} }
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
|
class ModifyPlanningScene : public PropagatingEitherWay {
|
||||||
|
public:
|
||||||
|
typedef std::vector<std::string> Names;
|
||||||
|
ModifyPlanningScene(std::string name);
|
||||||
|
|
||||||
|
bool computeForward(const InterfaceState& from) override;
|
||||||
|
bool computeBackward(const InterfaceState& to) override;
|
||||||
|
|
||||||
|
/// methods to attach objects to a robot link
|
||||||
|
template <class C>
|
||||||
|
void attachObjects(const C& objects, const std::string& attach_link);
|
||||||
|
template <class C>
|
||||||
|
void detachObjects(const C& objects, const std::string& attach_link);
|
||||||
|
/// attachObjects() and detachObjects() forward to this method, setting attach = true resp. false
|
||||||
|
void attachObjects(const Names& objects, const std::string& attach_link, bool attach);
|
||||||
|
|
||||||
|
/// method to enable / disable collision between object pairs
|
||||||
|
template <class T1, class T2>
|
||||||
|
void enableCollisions(const T1& first, const T2& second, bool enable_collision);
|
||||||
|
/// enable / disable all collisions for given object
|
||||||
|
void enableCollisions(const std::string& object, bool enable_collision);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// list of objects to attach (true) / detach (false) to a given link
|
||||||
|
std::map<std::string, std::pair<Names, bool> > attach_objects_;
|
||||||
|
|
||||||
|
// list of objects to mutually
|
||||||
|
struct CollisionMatrixPairs {
|
||||||
|
Names first;
|
||||||
|
Names second;
|
||||||
|
bool enable;
|
||||||
|
};
|
||||||
|
std::list<CollisionMatrixPairs> collision_matrix_edits_;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// apply stored modifications to scene
|
||||||
|
void attachObjects(planning_scene::PlanningScene &scene, const std::pair<std::string, std::pair<Names, bool> >& pair, bool invert);
|
||||||
|
void setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert);
|
||||||
|
planning_scene::PlanningScenePtr apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert);
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class
|
||||||
|
namespace detail {
|
||||||
|
|
||||||
|
// generic implementation
|
||||||
|
template <class T> struct AttachHelper {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const T& objects, const std::string& link, bool attach) {
|
||||||
|
static_assert(std::is_base_of<std::string, typename T::value_type>::value, "T must be a container of std::strings");
|
||||||
|
stage->attachObjects(ModifyPlanningScene::Names(objects.cbegin(), objects.cend()), link, attach);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// specialization for Names
|
||||||
|
template<>
|
||||||
|
struct AttachHelper<ModifyPlanningScene::Names> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& objects, const std::string& link, bool attach) {
|
||||||
|
stage->attachObjects(objects, link, attach);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// specialization for std::string
|
||||||
|
template<>
|
||||||
|
struct AttachHelper<std::string> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const std::string& object, const std::string& link, bool attach) {
|
||||||
|
stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
// specialization for string literal
|
||||||
|
template<int N>
|
||||||
|
struct AttachHelper<char[N]> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const char object[N], const std::string& link, bool attach) {
|
||||||
|
stage->attachObjects(ModifyPlanningScene::Names({object}), link, attach);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace detail
|
||||||
|
|
||||||
|
// implementation of methods forwards to AttachHelper::impl()
|
||||||
|
template <class T>
|
||||||
|
void ModifyPlanningScene::attachObjects(const T& first, const std::string& attach_link) {
|
||||||
|
detail::AttachHelper<T>::impl(this, first, attach_link, true);
|
||||||
|
}
|
||||||
|
template <class T>
|
||||||
|
void ModifyPlanningScene::detachObjects(const T& first, const std::string& attach_link) {
|
||||||
|
detail::AttachHelper<T>::impl(this, first, attach_link, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// specialization for Names, implemented in .cpp
|
||||||
|
template <>
|
||||||
|
void ModifyPlanningScene::enableCollisions<ModifyPlanningScene::Names, ModifyPlanningScene::Names>
|
||||||
|
(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision);
|
||||||
|
|
||||||
|
// As C++ doesn't allow for partial specialization of functions / methods, we need to use a helper class
|
||||||
|
namespace detail {
|
||||||
|
|
||||||
|
// generic implementation
|
||||||
|
template <class T1, class T2> struct CollisionHelper {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const T1& first, const T2& second, bool enable) {
|
||||||
|
static_assert(std::is_base_of<std::string, typename T1::value_type>::value, "T1 must be a container of std::strings");
|
||||||
|
static_assert(std::is_base_of<std::string, typename T2::value_type>::value, "T2 must be a container of std::strings");
|
||||||
|
stage->enableCollisions(ModifyPlanningScene::Names(first.cbegin(), first.cend()),
|
||||||
|
ModifyPlanningScene::Names(second.cbegin(), second.cend()), enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// specialization for Names
|
||||||
|
template<>
|
||||||
|
struct CollisionHelper<ModifyPlanningScene::Names, ModifyPlanningScene::Names> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable) {
|
||||||
|
stage->enableCollisions(first, second, enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// specialization for pair of std::string
|
||||||
|
template<>
|
||||||
|
struct CollisionHelper<std::string, std::string> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const std::string& first, const std::string& second, bool enable) {
|
||||||
|
stage->enableCollisions(ModifyPlanningScene::Names({first}), ModifyPlanningScene::Names({second}), enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
// specialization for pair of string literals
|
||||||
|
template<int N1, int N2>
|
||||||
|
struct CollisionHelper<char[N1], char[N2]> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const char first[N1], const char second[N2], bool enable) {
|
||||||
|
stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}),
|
||||||
|
ModifyPlanningScene::Names({std::string(second)}), enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
// specialization for std::string, container
|
||||||
|
template<class T2>
|
||||||
|
struct CollisionHelper<std::string, T2> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const std::string& first, const T2& second, bool enable) {
|
||||||
|
stage->enableCollisions(ModifyPlanningScene::Names({first}), second, enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
// specialization for string literal, container
|
||||||
|
template<int N1, class T2>
|
||||||
|
struct CollisionHelper<char[N1], T2> {
|
||||||
|
static void impl(ModifyPlanningScene* stage, const char first[N1], const T2& second, bool enable) {
|
||||||
|
stage->enableCollisions(ModifyPlanningScene::Names({std::string(first)}), second, enable);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace detail
|
||||||
|
|
||||||
|
// implementation of method forwards to CollisionHelper::impl()
|
||||||
|
template <class T1, class T2>
|
||||||
|
inline void ModifyPlanningScene::enableCollisions(const T1& first, const T2& second, bool enable_collision) {
|
||||||
|
detail::CollisionHelper<T1, T2>::impl(this, first, second, enable_collision);
|
||||||
|
}
|
||||||
|
|
||||||
|
// single-object variant forwards to empty Names in second arg
|
||||||
|
inline void ModifyPlanningScene::enableCollisions(const std::string &object, bool enable_collision) {
|
||||||
|
enableCollisions(Names({object}), Names(), enable_collision);
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -76,6 +76,7 @@ public:
|
|||||||
~Task();
|
~Task();
|
||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
|
const moveit::core::RobotModelPtr getRobotModel() const;
|
||||||
|
|
||||||
void add(Stage::pointer &&stage);
|
void add(Stage::pointer &&stage);
|
||||||
void clear() override;
|
void clear() override;
|
||||||
|
|||||||
@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
${PROJECT_INCLUDE}/stages/current_state.h
|
${PROJECT_INCLUDE}/stages/current_state.h
|
||||||
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
|
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
|
||||||
${PROJECT_INCLUDE}/stages/gripper.h
|
${PROJECT_INCLUDE}/stages/gripper.h
|
||||||
|
${PROJECT_INCLUDE}/stages/modify_planning_scene.h
|
||||||
${PROJECT_INCLUDE}/stages/move.h
|
${PROJECT_INCLUDE}/stages/move.h
|
||||||
|
|
||||||
cartesian_position_motion.cpp
|
cartesian_position_motion.cpp
|
||||||
@ -11,6 +12,7 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
compute_ik.cpp
|
compute_ik.cpp
|
||||||
generate_grasp_pose.cpp
|
generate_grasp_pose.cpp
|
||||||
gripper.cpp
|
gripper.cpp
|
||||||
|
modify_planning_scene.cpp
|
||||||
move.cpp
|
move.cpp
|
||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
|
|||||||
77
core/src/stages/modify_planning_scene.cpp
Normal file
77
core/src/stages/modify_planning_scene.cpp
Normal file
@ -0,0 +1,77 @@
|
|||||||
|
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
|
ModifyPlanningScene::ModifyPlanningScene(std::string name)
|
||||||
|
: PropagatingEitherWay(name)
|
||||||
|
{}
|
||||||
|
|
||||||
|
void ModifyPlanningScene::attachObjects(const Names& objects, const std::string& attach_link, bool attach)
|
||||||
|
{
|
||||||
|
auto it_inserted = attach_objects_.insert(std::make_pair(attach_link, std::make_pair(Names(), attach)));
|
||||||
|
Names &o = it_inserted.first->second.first;
|
||||||
|
o.insert(o.end(), objects.begin(), objects.end());
|
||||||
|
}
|
||||||
|
|
||||||
|
template <>
|
||||||
|
void ModifyPlanningScene::enableCollisions<ModifyPlanningScene::Names>
|
||||||
|
(const ModifyPlanningScene::Names& first, const ModifyPlanningScene::Names& second, bool enable_collision) {
|
||||||
|
collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision}));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
||||||
|
planning_scene::PlanningScenePtr to = apply(from.scene(), false);
|
||||||
|
sendForward(from, InterfaceState(to), robot_trajectory::RobotTrajectoryPtr());
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
||||||
|
{
|
||||||
|
planning_scene::PlanningScenePtr from = apply(to.scene(), true);
|
||||||
|
sendBackward(InterfaceState(from), to, robot_trajectory::RobotTrajectoryPtr());
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene, const std::pair<std::string, std::pair<Names, bool> >& pair, bool invert)
|
||||||
|
{
|
||||||
|
moveit_msgs::AttachedCollisionObject obj;
|
||||||
|
obj.link_name = pair.first;
|
||||||
|
bool attach = pair.second.second;
|
||||||
|
if (invert) attach = !attach;
|
||||||
|
obj.object.operation = attach ? (int8_t) moveit_msgs::CollisionObject::ADD
|
||||||
|
: (int8_t) moveit_msgs::CollisionObject::REMOVE;
|
||||||
|
for (const std::string& name : pair.second.first) {
|
||||||
|
obj.object.id = name;
|
||||||
|
scene.processAttachedCollisionObjectMsg(obj);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModifyPlanningScene::setCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert)
|
||||||
|
{
|
||||||
|
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
|
||||||
|
bool enable = invert ? !pairs.enable : pairs.enable;
|
||||||
|
if (pairs.second.empty()) {
|
||||||
|
for (const auto &name : pairs.first)
|
||||||
|
acm.setEntry(name, enable);
|
||||||
|
} else
|
||||||
|
acm.setEntry(pairs.first, pairs.second, enable);
|
||||||
|
}
|
||||||
|
|
||||||
|
// invert indicates, whether to detach instead of attach (and vice versa)
|
||||||
|
// as well as to disable instead of enable collision (and vice versa)
|
||||||
|
planning_scene::PlanningScenePtr ModifyPlanningScene::apply(const planning_scene::PlanningSceneConstPtr &scene, bool invert)
|
||||||
|
{
|
||||||
|
planning_scene::PlanningScenePtr result = scene->diff();
|
||||||
|
// attach/detach objects
|
||||||
|
for (const auto &pair : attach_objects_)
|
||||||
|
attachObjects(*result, pair, invert);
|
||||||
|
|
||||||
|
// enable/disable collisions
|
||||||
|
for (const auto &pairs : collision_matrix_edits_)
|
||||||
|
setCollisions(*result, pairs, invert);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
} } }
|
||||||
@ -68,6 +68,10 @@ void Task::initModel () {
|
|||||||
if( !rml_->getModel() )
|
if( !rml_->getModel() )
|
||||||
throw Exception("Task failed to construct RobotModel");
|
throw Exception("Task failed to construct RobotModel");
|
||||||
}
|
}
|
||||||
|
const moveit::core::RobotModelPtr Task::getRobotModel() const {
|
||||||
|
return rml_ ? rml_->getModel() : moveit::core::RobotModelPtr();
|
||||||
|
}
|
||||||
|
|
||||||
void Task::initScene() {
|
void Task::initScene() {
|
||||||
assert(rml_);
|
assert(rml_);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user