From 7d8f8dfc8de7533dedff51119341d44edbc03b92 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 1 Dec 2017 21:19:16 +0100 Subject: [PATCH 01/12] PropertyMap --- .../moveit/task_constructor/properties.h | 102 +++++++++++++ core/include/moveit/task_constructor/stage.h | 6 + .../include/moveit/task_constructor/stage_p.h | 11 +- .../include/moveit/task_constructor/storage.h | 6 +- core/include/moveit/task_constructor/task.h | 7 + core/src/CMakeLists.txt | 3 + core/src/properties.cpp | 140 ++++++++++++++++++ core/src/stage.cpp | 43 +++++- core/src/storage.cpp | 2 +- core/src/task.cpp | 12 ++ core/test/CMakeLists.txt | 5 +- core/test/test_properties.cpp | 22 +++ 12 files changed, 353 insertions(+), 6 deletions(-) create mode 100644 core/include/moveit/task_constructor/properties.h create mode 100644 core/src/properties.cpp create mode 100644 core/test/test_properties.cpp diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h new file mode 100644 index 00000000..3f7a1aa9 --- /dev/null +++ b/core/include/moveit/task_constructor/properties.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { + +class Property; +class PropertyMap; + +/// initializer function, using another name from the passed property map +boost::any fromName(const PropertyMap& other, const std::string& other_name); + +enum PropertyInitializerSource { + PARENT, + INTERFACE, +}; + +class Property { + friend class PropertyMap; + +public: + typedef std::function InitializerFunction; + typedef std::map InitializerMap; + + Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); + + void setValue(const boost::any& value); + + const boost::any& value() const; + const boost::any& defaultValue() const { return default_; } + const std::string& description() const { return description_; } + std::string typeName() const { return type_index_.name(); } + + /// set an initializer function + Property &initFrom(PropertyInitializerSource source, const InitializerFunction& f = fromName); + Property &initFrom(PropertyInitializerSource source, const std::string& other_name); + +private: + std::string description_; + std::type_index type_index_; + boost::any default_; + boost::any value_; + InitializerMap initializers_; +}; + + +class PropertyMap +{ + std::map props_; + +public: + /// declare a property for future use + Property& declare(const std::string& name, const std::type_info& type, + const std::string& description = "", + const boost::any& default_value = boost::any()); + + template + Property& declare(const std::string& name, const std::string& description = "") { + return declare(name, typeid(T), description); + } + template + Property& declare(const std::string& name, const T& default_value, + const std::string& description = "") { + return declare(name, typeid(T), description, default_value); + } + + /// get the property with given name + Property& property(const std::string &name); + const Property& property(const std::string &name) const { + return const_cast(this)->property(name); + } + + /// allow initialization from given source for listed properties + void initFrom(PropertyInitializerSource source, const std::set &properties = {}); + + /// set the value of a property + void set(const std::string& name, const boost::any& value); + + /// get the value of a property + const boost::any& get(const std::string& name) const; + template + const T& get(const std::string& name) const { + const boost::any& value = get(name); + if (value.empty()) + throw std::runtime_error(std::string("undefined property: " + name)); + return boost::any_cast(value); + } + + /// reset properties to nil, if they have initializers + void reset(); + /// init properties from initializers + void initFrom(PropertyInitializerSource source, const PropertyMap& other, + bool checkConsistency = false); +}; + +} // namespace task_constructor +} // namespace moveit diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index e422a37d..8a18ed1f 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -107,6 +107,12 @@ public: /// remove function callback void erase(SolutionCallbackList::const_iterator which); + PropertyMap& properties(); + const PropertyMap& properties() const { + return const_cast(this)->properties(); + } + void setProperty(const std::string& name, const boost::any& value); + protected: /// Stage can only be instantiated through derived classes Stage(StagePrivate *impl); diff --git a/core/include/moveit/task_constructor/stage_p.h b/core/include/moveit/task_constructor/stage_p.h index 2e42d1ee..6fca3728 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -43,7 +43,7 @@ public: inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); } inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); } - /// validate that sendForward() and sendBackward() will succeed + /// validate correct configuration of this stage /// should be only called by containers' init() method void validate() const; @@ -61,6 +61,7 @@ public: protected: Stage* const me_; // associated/owning Stage instance std::string name_; + PropertyMap properties_; InterfacePtr starts_; InterfacePtr ends_; @@ -121,6 +122,8 @@ protected: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); + // initialize properties from parent and/or state + void initProperties(const InterfaceState &state); Interface::const_iterator next_start_state_; Interface::const_iterator next_end_state_; @@ -148,6 +151,10 @@ public: bool canCompute() const override; bool compute() override; + +private: + // initialize properties from parent + void initProperties(); }; PIMPL_FUNCTIONS(Generator) @@ -168,6 +175,8 @@ private: // get informed when new start or end state becomes available void newStartState(const std::list::iterator& it); void newEndState(const std::list::iterator& it); + // initialize properties from parent and/or interface states + void initProperties(const InterfaceState &start, const InterfaceState &end); std::pair it_pairs_; }; diff --git a/core/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 826fd19b..6a4310d8 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -3,6 +3,7 @@ #pragma once #include +#include #include #include @@ -50,6 +51,9 @@ public: inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; } inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; } + PropertyMap& properties() { return properties_; } + const PropertyMap& properties() const { return properties_; } + private: // these methods should be only called by SolutionBase::set[Start|End]State() inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); } @@ -57,7 +61,7 @@ private: private: planning_scene::PlanningSceneConstPtr scene_; - // TODO: add PropertyMap: std::map to allow passing of parameters or attributes + PropertyMap properties_; Solutions incoming_trajectories_; Solutions outgoing_trajectories_; }; diff --git a/core/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index d15a4455..c1ebe1e9 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -72,6 +72,13 @@ public: ContainerBase *stages(); const ContainerBase *stages() const; + /// properties access + PropertyMap& properties(); + const PropertyMap& properties() const { + return const_cast(this)->properties(); + } + void setProperty(const std::string& name, const boost::any& value); + protected: void initModel(); void initScene(); diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 0608c642..db55946f 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -1,4 +1,5 @@ add_library(${PROJECT_NAME} + properties.cpp storage.cpp stage.cpp container.cpp @@ -6,6 +7,7 @@ add_library(${PROJECT_NAME} introspection.cpp ${PROJECT_INCLUDE}/utils.h + ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/storage.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h @@ -14,6 +16,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/task.h ${PROJECT_INCLUDE}/introspection.h ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ diff --git a/core/src/properties.cpp b/core/src/properties.cpp new file mode 100644 index 00000000..1795acdc --- /dev/null +++ b/core/src/properties.cpp @@ -0,0 +1,140 @@ +#include +#include +#include + +namespace moveit { +namespace task_constructor { + +Property::Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value) + : description_(description) + , type_index_(type_index) + , default_(default_value) +{ + if (!default_.empty() && std::type_index(default_.type()) != type_index_) + throw std::runtime_error("type of default value doesn't match declared type"); +} + +namespace { +void typeCheck(const boost::any& value, const std::type_index& type_index) +{ + if (std::type_index(value.type()) != type_index) { + static boost::format fmt("type (%1%) doesn't match property's declared type (%2%)"); + throw std::runtime_error(boost::str(fmt % value.type().name() % type_index.name())); + } +} +} + +void Property::setValue(const boost::any &value) { + typeCheck(value, type_index_); + value_ = value; + // clear all initializers when value was explicitly set + initializers_.clear(); +} + +const boost::any &Property::value() const { + return value_.empty() ? default_ : value_; +} + +Property& Property::initFrom(PropertyInitializerSource source, const Property::InitializerFunction &f) +{ + initializers_[source] = f; + return *this; +} + +Property& Property::initFrom(PropertyInitializerSource source, const std::string &other_name) +{ + initializers_[source] = [other_name](const PropertyMap& other, const std::string&) { + return fromName(other, other_name); + }; + return *this; +} + + +Property& PropertyMap::declare(const std::string &name, const std::type_info &type, + const std::string &description, const boost::any &default_value) +{ + auto it_inserted = props_.insert(std::make_pair(name, Property(std::type_index(type), description, default_value))); + if (!it_inserted.second && std::type_index(type) != it_inserted.first->second.type_index_) + throw std::runtime_error("Property '" + name + "' was already declared with different type."); + return it_inserted.first->second; +} + +Property& PropertyMap::property(const std::string &name) +{ + auto it = props_.find(name); + if (it == props_.end()) + throw std::runtime_error("Unknown property '" + name + "'"); + return it->second; +} + +void PropertyMap::initFrom(PropertyInitializerSource source, const std::set &properties) +{ + for (auto &pair : props_) { + if (properties.empty() || properties.count(pair.first)) + pair.second.initFrom(source); + } +} + +void PropertyMap::set(const std::string &name, const boost::any &value) +{ + auto range = props_.equal_range(name); + if (range.first == range.second) { // name is not yet declared + if (value.empty()) { + ROS_ERROR("trying to define property '%s' with NULL value", name.c_str()); + return; + } + auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any()))); + it->second.setValue(value); + } else { + assert(range.first->first == name); + range.first->second.setValue(value); + } +} + +const boost::any &PropertyMap::get(const std::string &name) const +{ + return property(name).value(); +} + +void PropertyMap::reset() +{ + for (auto& pair : props_) { + Property &p = pair.second; + if (!p.initializers_.empty()) + // if there are initializers, reset property value + // explicitly setting the value, clears initializers + p.value_ = boost::any(); + } +} + +void PropertyMap::initFrom(PropertyInitializerSource source, const PropertyMap &other, + bool checkConsistency) +{ + for (auto& pair : props_) { + Property &p = pair.second; + auto it = p.initializers_.find(source); + if (it == p.initializers_.end()) continue; + const boost::any& value = it->second(other, pair.first); + if (value.empty()) continue; + + typeCheck(value, p.type_index_); +#if 0 // TODO: we cannot generically check for equality of boost::anys + if (checkConsistency && !p.value_.empty() && p.value_ != value) + throw std::runtime_error ("inconsistent values for property '" + pair.first + "'"); +#endif + p.value_ = value; + } +} + + +boost::any fromName(const PropertyMap& other, const std::string& other_name) +{ + try { + return other.get(other_name); + } catch (const std::runtime_error &e) { + return boost::any(); + } +} + +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/stage.cpp b/core/src/stage.cpp index b632f5ab..aaf92772 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -54,9 +54,11 @@ void StagePrivate::validate() const { InitStageException errors; InterfaceFlags f = interfaceFlags(); + // validate that sendForward() will succeed if (!implies(f & WRITES_NEXT_START, bool(nextStarts()))) errors.push_back(*me_, "sends forward, but next stage cannot receive"); + // validate that sendBackward() will succeed if (!implies(f & WRITES_PREV_END, bool(prevEnds()))) errors.push_back(*me_, "sends backward, but previous stage cannot receive"); @@ -124,6 +126,15 @@ void Stage::erase(SolutionCallbackList::const_iterator which) pimpl()->solution_cbs_.erase(which); } +PropertyMap &Stage::properties() +{ + return pimpl()->properties_; +} + +void Stage::setProperty(const std::string& name, const boost::any& value) { + pimpl()->properties_.set(name, value); +} + template const char* direction(const StagePrivate& stage) { InterfaceFlags f = stage.interfaceFlags(); @@ -226,6 +237,13 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ return state; } +void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) +{ + properties_.reset(); + properties_.initFrom(PARENT, parent()->properties()); + properties_.initFrom(INTERFACE, state.properties()); +} + bool PropagatingEitherWayPrivate::canCompute() const { if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) @@ -241,11 +259,15 @@ bool PropagatingEitherWayPrivate::compute() bool result = false; if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) { - if (me->computeForward(fetchStartState())) + const InterfaceState& state = fetchStartState(); + initProperties(state); + if (me->computeForward(state)) result |= true; } if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) { - if (me->computeBackward(fetchEndState())) + const InterfaceState& state = fetchEndState(); + initProperties(state); + if (me->computeBackward(state)) result |= true; } return result; @@ -404,9 +426,16 @@ bool GeneratorPrivate::canCompute() const { } bool GeneratorPrivate::compute() { + initProperties(); return static_cast(me_)->compute(); } +void GeneratorPrivate::initProperties() +{ + properties_.reset(); + properties_.initFrom(PARENT, parent()->properties()); +} + Generator::Generator(const std::string &name) : ComputeBase(new GeneratorPrivate(this, name)) @@ -450,6 +479,15 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) --it_pairs_.second; } +void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) +{ + properties_.reset(); + properties_.initFrom(PARENT, parent()->properties()); + // properties from start/end states need to be consistent to each other + properties_.initFrom(INTERFACE, start.properties()); + properties_.initFrom(INTERFACE, end.properties(), true); +} + bool ConnectingPrivate::canCompute() const{ // TODO: implement this properly return it_pairs_.first != starts_->end() && @@ -460,6 +498,7 @@ bool ConnectingPrivate::compute() { // TODO: implement this properly const InterfaceState& from = *it_pairs_.first; const InterfaceState& to = *(it_pairs_.second++); + initProperties(from, to); return static_cast(me_)->compute(from, to); } diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 2bf3f7d1..4735914c 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -13,7 +13,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps) } InterfaceState::InterfaceState(const InterfaceState &existing) - : scene_(existing.scene()) + : scene_(existing.scene_), properties_(existing.properties_) { } diff --git a/core/src/task.cpp b/core/src/task.cpp index 409b5563..e3ca4aea 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -215,6 +215,18 @@ const ContainerBase* Task::stages() const return const_cast(this)->stages(); } +PropertyMap &Task::properties() +{ + // forward to wrapped() stage + return wrapped()->properties(); +} + +void Task::setProperty(const std::string &name, const boost::any &value) +{ + // forward to wrapped() stage + wrapped()->setProperty(name, value); +} + std::string Task::id() const { return id_; diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 0b18a0cf..e12b1081 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -7,8 +7,11 @@ if (CATKIN_ENABLE_TESTING) catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp) target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} - ${catkin_LIBRARIES} gtest_main) + + catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp) + target_link_libraries(${PROJECT_NAME}-test-properties + ${PROJECT_NAME} gtest_main) endif() add_executable(test_plan_current_state test_plan_current_state.cpp) diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp new file mode 100644 index 00000000..f8843964 --- /dev/null +++ b/core/test/test_properties.cpp @@ -0,0 +1,22 @@ +#include + +#include +#include + +using namespace moveit::task_constructor; + +TEST(Property, serialContainer) { + PropertyMap props; + props.declare("double1"); + props.declare("double2", 1); + + // avoid second declaration with different type + props.declare("double1"); + ASSERT_THROW(props.declare("double1"), std::runtime_error); + + // types not matching? + ASSERT_THROW(props.set("double1", 1), std::runtime_error); + + props.set("double1", 3.14); + ASSERT_EQ(props.get("double1"), 3.14); +} From 587dcaebb28d52f6bca98c168d613791b6f58b58 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Mon, 4 Dec 2017 10:26:34 +0100 Subject: [PATCH 02/12] ur5 example: use properties --- core/demo/plan_pick_ur5.cpp | 23 +++--- .../stages/cartesian_position_motion.h | 12 --- .../stages/generate_grasp_pose.h | 24 +----- .../moveit/task_constructor/stages/gripper.h | 5 -- .../moveit/task_constructor/stages/move.h | 13 +-- core/src/stages/cartesian_position_motion.cpp | 81 +++++++++++-------- core/src/stages/generate_grasp_pose.cpp | 79 +++++++++++------- core/src/stages/gripper.cpp | 44 ++++++---- core/src/stages/move.cpp | 35 ++++---- 9 files changed, 165 insertions(+), 151 deletions(-) diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 959c5ccc..deb7d624 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -39,29 +39,32 @@ int main(int argc, char** argv){ spawnObject(); Task t; + // define global properties used by most stages + t.setProperty("group", std::string("arm")); + t.setProperty("eef", std::string("gripper")); + t.setProperty("planner", std::string("RRTConnectkConfigDefault")); + t.setProperty("link", std::string("s_model_tool0")); t.add(std::make_unique("current state")); { auto move = std::make_unique("open gripper"); - move->setEndEffector("gripper"); + move->properties().initFrom(PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("move to pre-grasp"); - move->setGroup("arm"); - move->setPlannerId("RRTConnectkConfigDefault"); + move->properties().initFrom(PARENT); move->setTimeout(8.0); t.add(std::move(move)); } { auto move = std::make_unique("proceed to grasp pose"); - move->addSolutionCallback(std::ref(t.introspection())); - move->setGroup("arm"); - move->setLink("s_model_tool0"); + // move->addSolutionCallback(std::ref(t.introspection())); + move->properties().initFrom(PARENT); move->setMinMaxDistance(.03, 0.1); move->setCartesianStepSize(0.02); @@ -73,8 +76,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->setEndEffector("gripper"); - //gengrasp->setGroup("arm"); + gengrasp->properties().initFrom(PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspOffset(.03); @@ -85,7 +87,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->setEndEffector("gripper"); + move->properties().initFrom(PARENT); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); @@ -93,8 +95,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("lift object"); - move->setGroup("arm"); - move->setLink("s_model_tool0"); + move->properties().initFrom(PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h index 0cc48165..93474da6 100644 --- a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h +++ b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h @@ -34,22 +34,10 @@ public: void setCartesianStepSize(double distance); protected: - std::string group_; - - std::string link_; - - double min_distance_; - double max_distance_; - enum { MODE_ALONG= 1, MODE_TOWARDS= 2 } mode_; - - geometry_msgs::PointStamped towards_; - geometry_msgs::Vector3Stamped along_; - - double step_size_; }; } } } diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 941198ee..2840d33d 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -32,37 +32,15 @@ public: void setMaxIKSolutions(uint32_t n); - void ignoreCollisions(bool flag); + void setIgnoreCollisions(bool flag); protected: planning_scene::PlanningSceneConstPtr scene_; - std::string eef_; - - std::string group_; - - std::string ik_link_; - - double grasp_offset_ = 0.0; - - uint32_t max_ik_solutions_; - - bool ignore_collisions_ = false; - - std::string gripper_grasp_pose_; - - std::string object_; - - double timeout_ = 0.1; - - double angle_delta_ = 0.1; - /* temp values */ double current_angle_ = 0.0; - double remaining_time_; - bool tried_current_state_as_seed_ = false; std::vector< std::vector > previous_solutions_; diff --git a/core/include/moveit/task_constructor/stages/gripper.h b/core/include/moveit/task_constructor/stages/gripper.h index 5d57e9d4..8a59e1ef 100644 --- a/core/include/moveit/task_constructor/stages/gripper.h +++ b/core/include/moveit/task_constructor/stages/gripper.h @@ -31,11 +31,6 @@ protected: robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir); protected: - std::string eef_; - std::string named_target_; - std::string grasp_object_; - std::string attach_link_; - planning_pipeline::PlanningPipelinePtr planner_; moveit::planning_interface::MoveGroupInterfacePtr mgi_; }; diff --git a/core/include/moveit/task_constructor/stages/move.h b/core/include/moveit/task_constructor/stages/move.h index e7770893..511624cd 100644 --- a/core/include/moveit/task_constructor/stages/move.h +++ b/core/include/moveit/task_constructor/stages/move.h @@ -4,10 +4,6 @@ #include -namespace moveit { -namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)} -} - namespace moveit { namespace task_constructor { namespace stages { class Move : public Connecting { @@ -17,17 +13,12 @@ public: void init(const planning_scene::PlanningSceneConstPtr &scene); bool compute(const InterfaceState &from, const InterfaceState &to); - void setGroup(std::string group); - void setPlannerId(std::string planner); + void setGroup(const std::__cxx11::string &group); + void setPlannerId(const std::__cxx11::string &planner); void setTimeout(double timeout); protected: - std::string group_; - std::string planner_id_; - double timeout_; - planning_pipeline::PlanningPipelinePtr planner_; - moveit::planning_interface::MoveGroupInterfacePtr mgi_; }; } } } diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index 33c3a5e3..17b49ed4 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -13,44 +13,51 @@ namespace moveit { namespace task_constructor { namespace stages { CartesianPositionMotion::CartesianPositionMotion(std::string name) -: PropagatingEitherWay(name), - step_size_(0.005) +: PropagatingEitherWay(name) { + auto& p = properties(); + p.declare("group", "name of planning group"); + p.declare("link", "", "name of link used for IK"); + p.declare("min_distance", "minimum distance to move"); + p.declare("max_distance", "maximum distance to move"); + p.declare("step_size", 0.005); + p.declare("towards", "target point of motion"); + p.declare("along", "vector along which to move"); } void CartesianPositionMotion::setGroup(std::string group){ - group_= group; + setProperty("group", group); } void CartesianPositionMotion::setLink(std::string link){ - link_= link; + setProperty("link", link); } void CartesianPositionMotion::setMinDistance(double distance){ - min_distance_= distance; + setProperty("min_distance", distance); } void CartesianPositionMotion::setMaxDistance(double distance){ - max_distance_= distance; + setProperty("max_distance", distance); } void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){ - setMinDistance(min_distance); - setMaxDistance(max_distance); + setProperty("min_distance", min_distance); + setProperty("max_distance", max_distance); } void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){ mode_= CartesianPositionMotion::MODE_TOWARDS; - towards_= towards; + setProperty("towards", towards); } void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){ mode_= CartesianPositionMotion::MODE_ALONG; - along_= along; + setProperty("along", along); } void CartesianPositionMotion::setCartesianStepSize(double distance){ - step_size_= distance; + setProperty("step_size", distance); } namespace { @@ -68,8 +75,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ planning_scene::PlanningScenePtr result_scene = from.scene()->diff(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); - const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_); - const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_); + const auto& props = properties(); + const std::string& group = props.get("group"); + const std::string& link = props.get("link"); + + const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group); + const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link); const moveit::core::GroupStateValidityCallbackFn is_valid= std::bind( @@ -83,12 +94,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ bool succeeded= false; if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){ - const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards_.header.frame_id); - - const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); + const geometry_msgs::PointStamped& towards = props.get("towards"); + const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards.header.frame_id); + const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link); Eigen::Vector3d target_point; - tf::pointMsgToEigen(towards_.point, target_point); + tf::pointMsgToEigen(towards.point, target_point); target_point= frame*target_point; // retain orientation of link @@ -101,7 +112,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ link_model, target, true, /* global frame */ - step_size_, /* cartesian step size */ + props.get("step_size"), /* cartesian step size */ 1.5, /* jump threshold */ is_valid); @@ -109,12 +120,13 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; - succeeded= achieved_distance >= min_distance_; + succeeded= achieved_distance >= props.get("min_distance"); } else if( mode_ == CartesianPositionMotion::MODE_ALONG ){ - const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); + const geometry_msgs::Vector3Stamped& along = props.get("along"); + const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id); Eigen::Vector3d direction; - tf::vectorMsgToEigen(along_.vector, direction); + tf::vectorMsgToEigen(along.vector, direction); direction= frame.linear()*direction; double achieved_distance= robot_state.computeCartesianPath( @@ -123,14 +135,14 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){ link_model, direction, true, /* global frame */ - max_distance_, /* distance */ - step_size_, /* cartesian step size */ + props.get("max_distance"), + props.get("step_size"), /* cartesian step size */ 1.5, /* jump threshold */ is_valid); std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; - succeeded= achieved_distance >= min_distance_; + succeeded= achieved_distance >= props.get("min_distance"); } else throw std::runtime_error("position motion has neither a goal nor a direction"); @@ -151,8 +163,12 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){ planning_scene::PlanningScenePtr result_scene = to.scene()->diff(); robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst(); - const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_); - const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_); + const auto& props = properties(); + const std::string& group = props.get("group"); + const std::string& link = props.get("link"); + + const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group); + const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link); const moveit::core::GroupStateValidityCallbackFn is_valid= std::bind( @@ -167,14 +183,15 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){ switch(mode_){ case(CartesianPositionMotion::MODE_TOWARDS): { - const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link_); + const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link); direction= link_pose.linear()*Eigen::Vector3d(-1,0,0); } break; case(CartesianPositionMotion::MODE_ALONG): { - const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along_.header.frame_id); - tf::vectorMsgToEigen(along_.vector, direction); + const geometry_msgs::Vector3Stamped& along = props.get("along"); + const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id); + tf::vectorMsgToEigen(along.vector, direction); direction= frame.linear()*direction; } break; @@ -190,14 +207,14 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){ link_model, direction, true, /* global frame */ - max_distance_, /* distance */ - step_size_, /* cartesian step size */ + props.get("max_distance"), + props.get("step_size"), /* cartesian step size */ 1.5, /* jump threshold */ is_valid); std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl; - bool succeeded= achieved_distance >= min_distance_; + bool succeeded= achieved_distance >= props.get("min_distance"); if(succeeded){ robot_trajectory::RobotTrajectoryPtr traj= std::make_shared(robot_state.getRobotModel(), jmg); diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 685fd22d..e0cbd68e 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -21,6 +21,17 @@ namespace moveit { namespace task_constructor { namespace stages { GenerateGraspPose::GenerateGraspPose(std::string name) : Generator(name) { + auto& p = properties(); + p.declare("group", "name of planning group"); + p.declare("eef", "name of end-effector group"); + p.declare("link", "", "name of link used for IK"); + p.declare("object"); + p.declare("grasp_pose"); + p.declare("timeout", 0.1); + p.declare("max_ik_solutions", 1); + p.declare("grasp_offset", 0.0); + p.declare("angle_delta", 0.1, "angular steps (rad)"); + p.declare("ignore_collisions", false); } void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -30,40 +41,44 @@ void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) } void GenerateGraspPose::setGroup(std::string group){ - group_= group; + setProperty("group", group); } void GenerateGraspPose::setLink(std::string ik_link){ - ik_link_= ik_link; + setProperty("link", ik_link); } void GenerateGraspPose::setEndEffector(std::string eef){ - eef_= eef; + setProperty("eef", eef); } void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ - gripper_grasp_pose_= pose_name; + setProperty("grasp_pose", pose_name); } void GenerateGraspPose::setObject(std::string object){ - object_= object; + setProperty("object", object); } void GenerateGraspPose::setGraspOffset(double offset){ - grasp_offset_= offset; + setProperty("grasp_offset", offset); } void GenerateGraspPose::setTimeout(double timeout){ - timeout_= timeout; - remaining_time_= timeout; + setProperty("timeout", timeout); } void GenerateGraspPose::setAngleDelta(double delta){ - angle_delta_= delta; + setProperty("angle_delta", delta); } void GenerateGraspPose::setMaxIKSolutions(uint32_t n){ - max_ik_solutions_= n; + setProperty("max_ik_solutions", n); +} + +void GenerateGraspPose::setIgnoreCollisions(bool flag) +{ + setProperty("ignore_collisions", flag); } bool GenerateGraspPose::canCompute() const{ @@ -98,48 +113,58 @@ namespace { } bool GenerateGraspPose::compute(){ - assert(scene_->getRobotModel()->hasEndEffector(eef_) && "The specified end effector is not defined in the srdf"); + const auto& props = properties(); + double remaining_time = props.get("timeout"); + + const std::string& eef = props.get("eef"); + const std::string& group = props.get("group"); + + assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf"); planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst(); - const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef_); + const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef); - const moveit::core::JointModelGroup* jmg_active= group_.empty() + const moveit::core::JointModelGroup* jmg_active= group.empty() ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) - : grasp_state.getJointModelGroup(group_); + : grasp_state.getJointModelGroup(group); - const std::string ik_link= ik_link_.empty() ? jmg_eef->getEndEffectorParentGroup().second : ik_link_; + std::string link = props.get("link"); + if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second; - if(!gripper_grasp_pose_.empty()){ - grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_); + const std::string& grasp_pose_name = props.get("grasp_pose"); + if(!grasp_pose_name.empty()){ + grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name); } const moveit::core::GroupStateValidityCallbackFn is_valid= std::bind( &isValid, scene_, - ignore_collisions_, + props.get("ignore_collisions"), &previous_solutions_, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); geometry_msgs::Pose object_pose, grasp_pose; - const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_); + const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get("object")); if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); tf::poseEigenToMsg(object_pose_eigen, object_pose); + double grasp_offset = props.get("grasp_offset"); + uint32_t max_ik_solutions = props.get("max_ik_solutions"); while( canCompute() ){ - if( remaining_time_ <= 0.0 || (max_ik_solutions_ != 0 && previous_solutions_.size() >= max_ik_solutions_)){ + if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){ std::cout << "computed angle " << current_angle_ << " with " << previous_solutions_.size() << " cached ik solutions" - << " and " << remaining_time_ << "s left" << std::endl; - current_angle_+= angle_delta_; - remaining_time_= timeout_; + << " and " << remaining_time << "s left" << std::endl; + current_angle_+= props.get("angle_delta"); + remaining_time = props.get("timeout"); tried_current_state_as_seed_= false; previous_solutions_.clear(); continue; @@ -147,8 +172,8 @@ bool GenerateGraspPose::compute(){ grasp_pose= object_pose; - grasp_pose.position.x-= grasp_offset_*cos(current_angle_); - grasp_pose.position.y-= grasp_offset_*sin(current_angle_); + grasp_pose.position.x-= grasp_offset*cos(current_angle_); + grasp_pose.position.y-= grasp_offset*sin(current_angle_); grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); if(tried_current_state_as_seed_) @@ -156,8 +181,8 @@ bool GenerateGraspPose::compute(){ tried_current_state_as_seed_= true; auto now= std::chrono::steady_clock::now(); - bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid); - remaining_time_-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); + bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, link, 1, remaining_time, is_valid); + remaining_time-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); if(succeeded) { previous_solutions_.emplace_back(); diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 7903b8c6..331e2485 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -15,7 +15,13 @@ namespace moveit { namespace task_constructor { namespace stages { Gripper::Gripper(std::string name) : PropagatingEitherWay(name) -{} +{ + auto& p = properties(); + p.declare("eef", "name of end-effector group"); + p.declare("link", "", "name of link the eef is attached to"); + p.declare("named_target", "", "named target in eef group"); + p.declare("grasp_object", "", "name of grasp object"); +} void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) { @@ -24,25 +30,25 @@ void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) } void Gripper::setEndEffector(std::string eef){ - eef_= eef; + setProperty("eef", eef); } void Gripper::setAttachLink(std::string link){ - attach_link_= link; + setProperty("link", link); } void Gripper::setFrom(std::string named_target){ restrictDirection(BACKWARD); - named_target_= named_target; + setProperty("named_target", named_target); } void Gripper::setTo(std::string named_target){ restrictDirection(FORWARD); - named_target_= named_target; + setProperty("named_target", named_target); } void Gripper::graspObject(std::string grasp_object){ - grasp_object_= grasp_object; + setProperty("grasp_object", grasp_object); } bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene, @@ -50,24 +56,30 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene scene = state.scene()->diff(); assert(scene->getRobotModel()); + const auto& props = properties(); + const std::string& eef = props.get("eef"); + std::string link = props.get("link"); + const std::string& named_target = props.get("named_target"); + const std::string& grasp_object = props.get("grasp_object"); + if(!mgi_){ - assert(scene->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf"); - const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef_); + assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf"); + const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef); const std::string group_name= jmg->getName(); mgi_= std::make_shared(group_name); - if( attach_link_.empty() ){ - attach_link_= jmg->getEndEffectorParentGroup().second; + if( link.empty() ){ + link= jmg->getEndEffectorParentGroup().second; } } - mgi_->setNamedTarget(named_target_); + mgi_->setNamedTarget(named_target); ::planning_interface::MotionPlanRequest req; mgi_->constructMotionPlanRequest(req); - if( !grasp_object_.empty() ){ - scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_, mgi_->getLinkNames(), true); + if( !grasp_object.empty() ){ + scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true); } ::planning_interface::MotionPlanResponse res; @@ -80,10 +92,10 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene scene->setCurrentState(trajectory->getLastWayPoint()); // attach object - if( !grasp_object_.empty() ){ + if( !grasp_object.empty() ){ moveit_msgs::AttachedCollisionObject obj; - obj.link_name= attach_link_; - obj.object.id= grasp_object_; + obj.link_name= link; + obj.object.id= grasp_object; scene->processAttachedCollisionObjectMsg(obj); } diff --git a/core/src/stages/move.cpp b/core/src/stages/move.cpp index 1284c288..12186720 100644 --- a/core/src/stages/move.cpp +++ b/core/src/stages/move.cpp @@ -14,9 +14,13 @@ namespace moveit { namespace task_constructor { namespace stages { Move::Move(std::string name) - : Connecting(name), - timeout_(5.0) -{} + : Connecting(name) +{ + auto& p = properties(); + p.declare("timeout", 5.0, "planning timeout"); + p.declare("group", "name of planning group"); + p.declare("planner", "", "planner id"); +} void Move::init(const planning_scene::PlanningSceneConstPtr &scene) { @@ -24,27 +28,30 @@ void Move::init(const planning_scene::PlanningSceneConstPtr &scene) planner_ = Task::createPlanner(scene->getRobotModel()); } -void Move::setGroup(std::string group){ - group_= group; - mgi_= std::make_shared(group_); +void Move::setGroup(const std::string& group){ + setProperty("group", group); } -void Move::setPlannerId(std::string planner){ - planner_id_= planner; +void Move::setPlannerId(const std::string& planner){ + setProperty("planner", planner); } void Move::setTimeout(double timeout){ - timeout_= timeout; + setProperty("timeout", timeout); } bool Move::compute(const InterfaceState &from, const InterfaceState &to) { - mgi_->setJointValueTarget(to.scene()->getCurrentState()); - if( !planner_id_.empty() ) - mgi_->setPlannerId(planner_id_); - mgi_->setPlanningTime(timeout_); + const auto& props = properties(); + moveit::planning_interface::MoveGroupInterface mgi(props.get("group")); + mgi.setJointValueTarget(to.scene()->getCurrentState()); + + const std::string planner_id = props.get("planner"); + if( !planner_id.empty() ) + mgi.setPlannerId(planner_id); + mgi.setPlanningTime(props.get("timeout")); ::planning_interface::MotionPlanRequest req; - mgi_->constructMotionPlanRequest(req); + mgi.constructMotionPlanRequest(req); ros::Duration(4.0).sleep(); ::planning_interface::MotionPlanResponse res; From f2b688d1f00e19df737354840704e7069e4d1b7e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 12 Dec 2017 15:02:17 +0100 Subject: [PATCH 03/12] generalize GenerateGraspPose replacing scalar graspOffset and hard-coded Euler angles with arbitrary graspFrame --- core/demo/CMakeLists.txt | 3 + core/demo/plan_pick_pa10.cpp | 114 ++++++++++++++++++ core/demo/plan_pick_trixi.cpp | 3 +- core/demo/plan_pick_ur5.cpp | 2 +- .../stages/generate_grasp_pose.h | 13 +- core/src/stages/generate_grasp_pose.cpp | 65 +++++----- core/test/test_plan_generate_grasp_pose.cpp | 2 +- 7 files changed, 167 insertions(+), 35 deletions(-) create mode 100644 core/demo/plan_pick_pa10.cpp diff --git a/core/demo/CMakeLists.txt b/core/demo/CMakeLists.txt index d82dd601..f973c09b 100644 --- a/core/demo/CMakeLists.txt +++ b/core/demo/CMakeLists.txt @@ -3,3 +3,6 @@ target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages) add_executable(plan_pick_trixi plan_pick_trixi.cpp) target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages) + +add_executable(plan_pick_pa10 plan_pick_pa10.cpp) +target_link_libraries(plan_pick_pa10 ${PROJECT_NAME}_stages) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp new file mode 100644 index 00000000..16897048 --- /dev/null +++ b/core/demo/plan_pick_pa10.cpp @@ -0,0 +1,114 @@ +#include + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace moveit::task_constructor; + +void spawnObject(){ + moveit::planning_interface::PlanningSceneInterface psi; + + moveit_msgs::CollisionObject o; + o.id= "object"; + o.header.frame_id= "world"; + o.primitive_poses.resize(1); + o.primitive_poses[0].position.x= 0.3; + o.primitive_poses[0].position.y= 0.23; + o.primitive_poses[0].position.z= 0.12; + o.primitive_poses[0].orientation.w= 1.0; + o.primitives.resize(1); + o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER; + o.primitives[0].dimensions.resize(2); + o.primitives[0].dimensions[0]= 0.23; + o.primitives[0].dimensions[1]= 0.03; + psi.applyCollisionObject(o); +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "plan_pick"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + spawnObject(); + + Task t; + // define global properties used by most stages + t.setProperty("group", std::string("left_arm")); + t.setProperty("eef", std::string("la_tool_mount")); + t.setProperty("planner", std::string("RRTConnectkConfigDefault")); + t.setProperty("link", std::string("la_tool_mount")); + + t.add(std::make_unique("current state")); + + { + auto move = std::make_unique("open gripper"); + move->properties().initFrom(PARENT); + move->setTo("open"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("move to pre-grasp"); + move->properties().initFrom(PARENT); + move->setTimeout(8.0); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("proceed to grasp pose"); + move->properties().initFrom(PARENT); + move->setMinMaxDistance(.05, 0.1); + move->setCartesianStepSize(0.02); + + geometry_msgs::PointStamped target; + target.header.frame_id= "object"; + move->towards(target); + t.add(std::move(move)); + } + + { + auto gengrasp = std::make_unique("generate grasp pose"); + gengrasp->properties().initFrom(PARENT); + gengrasp->setGripperGraspPose("open"); + gengrasp->setObject("object"); + gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)* + Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), + "lh_tool_frame"); + gengrasp->setAngleDelta(-.2); + gengrasp->setMaxIKSolutions(1); + t.add(std::move(gengrasp)); + } + + { + auto move = std::make_unique("grasp"); + move->properties().initFrom(PARENT); + move->setTo("closed"); + move->graspObject("object"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("lift object"); + move->properties().initFrom(PARENT); + move->setMinMaxDistance(0.03, 0.05); + move->setCartesianStepSize(0.01); + + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id= "world"; + direction.vector.z= 1.0; + move->along(direction); + t.add(std::move(move)); + } + + t.plan(); + t.publishAllSolutions(); + + return 0; +} diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index 7c09b158..b385799c 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -77,10 +77,9 @@ int main(int argc, char** argv){ auto gengrasp= std::make_unique("generate grasp pose"); gengrasp->setEndEffector("left_gripper"); //gengrasp->setGroup("arm"); - gengrasp->setLink("l_gripper_tool_frame"); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspOffset(.00); + gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame"); gengrasp->setAngleDelta(.2); t.add(std::move(gengrasp)); } diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index deb7d624..e4f514f7 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -79,7 +79,7 @@ int main(int argc, char** argv){ gengrasp->properties().initFrom(PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); - gengrasp->setGraspOffset(.03); + gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); gengrasp->setMaxIKSolutions(8); t.add(std::move(gengrasp)); diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 2840d33d..e47102d5 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -3,6 +3,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { namespace stages { @@ -18,14 +19,10 @@ public: void setGroup(std::string group_name); - void setLink(std::string ik_link); - void setGripperGraspPose(std::string pose_name); void setObject(std::string object); - void setGraspOffset(double grasp_offset); - void setTimeout(double timeout); void setAngleDelta(double delta); @@ -34,6 +31,14 @@ public: void setIgnoreCollisions(bool flag); + void setGraspFrame(const geometry_msgs::TransformStamped &transform); + void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = ""); + template + void setGraspFrame(const T& t, const std::string& link = "") { + Eigen::Affine3d transform; transform = t; + setGraspFrame(transform, link); + } + protected: planning_scene::PlanningSceneConstPtr scene_; diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index e0cbd68e..75acadd9 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -24,12 +24,11 @@ GenerateGraspPose::GenerateGraspPose(std::string name) auto& p = properties(); p.declare("group", "name of planning group"); p.declare("eef", "name of end-effector group"); - p.declare("link", "", "name of link used for IK"); p.declare("object"); - p.declare("grasp_pose"); + p.declare("eef_grasp_pose"); p.declare("timeout", 0.1); p.declare("max_ik_solutions", 1); - p.declare("grasp_offset", 0.0); + p.declare("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping"); p.declare("angle_delta", 0.1, "angular steps (rad)"); p.declare("ignore_collisions", false); } @@ -44,24 +43,27 @@ void GenerateGraspPose::setGroup(std::string group){ setProperty("group", group); } -void GenerateGraspPose::setLink(std::string ik_link){ - setProperty("link", ik_link); -} - void GenerateGraspPose::setEndEffector(std::string eef){ setProperty("eef", eef); } void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ - setProperty("grasp_pose", pose_name); + setProperty("eef_grasp_pose", pose_name); } void GenerateGraspPose::setObject(std::string object){ setProperty("object", object); } -void GenerateGraspPose::setGraspOffset(double offset){ - setProperty("grasp_offset", offset); +void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){ + setProperty("grasp_frame", frame); +} +void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link) +{ + geometry_msgs::TransformStamped frame; + frame.header.frame_id = link; + tf::transformEigenToMsg(transform, frame.transform); + setGraspFrame(frame); } void GenerateGraspPose::setTimeout(double timeout){ @@ -130,12 +132,28 @@ bool GenerateGraspPose::compute(){ ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) : grasp_state.getJointModelGroup(group); - std::string link = props.get("link"); - if (link.empty()) link = jmg_eef->getEndEffectorParentGroup().second; + geometry_msgs::TransformStamped grasp_frame = props.get("grasp_frame"); + const std::string &link_name = jmg_eef->getEndEffectorParentGroup().second; + if (grasp_frame.header.frame_id.empty()) + grasp_frame.header.frame_id = link_name; + Eigen::Affine3d grasp_pose; + tf::transformMsgToEigen(grasp_frame.transform, grasp_pose); - const std::string& grasp_pose_name = props.get("grasp_pose"); - if(!grasp_pose_name.empty()){ - grasp_state.setToDefaultValues(jmg_eef, grasp_pose_name); + if (grasp_frame.header.frame_id != link_name) { + // convert grasp_pose to transform relative to link (instead of frame_id) + const Eigen::Affine3d link_pose = scene_->getFrameTransform(link_name); + if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + throw std::runtime_error("requested link does not exist or could not be retrieved"); + const Eigen::Affine3d frame_pose = scene_->getFrameTransform(grasp_frame.header.frame_id); + if(frame_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + throw std::runtime_error("requested frame does not exist or could not be retrieved"); + grasp_pose = link_pose.inverse() * frame_pose * grasp_pose; + } + grasp_pose = grasp_pose.inverse(); // invert once + + const std::string& eef_grasp_pose = props.get("eef_grasp_pose"); + if(!eef_grasp_pose.empty()){ + grasp_state.setToDefaultValues(jmg_eef, eef_grasp_pose); } const moveit::core::GroupStateValidityCallbackFn is_valid= @@ -148,14 +166,10 @@ bool GenerateGraspPose::compute(){ std::placeholders::_2, std::placeholders::_3); - geometry_msgs::Pose object_pose, grasp_pose; - const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(props.get("object")); - if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get("object")); + if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); - tf::poseEigenToMsg(object_pose_eigen, object_pose); - - double grasp_offset = props.get("grasp_offset"); uint32_t max_ik_solutions = props.get("max_ik_solutions"); while( canCompute() ){ if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){ @@ -170,18 +184,15 @@ bool GenerateGraspPose::compute(){ continue; } - grasp_pose= object_pose; - - grasp_pose.position.x-= grasp_offset*cos(current_angle_); - grasp_pose.position.y-= grasp_offset*sin(current_angle_); - grasp_pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_); + // rotate object pose about z-axis + Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_pose; if(tried_current_state_as_seed_) grasp_state.setToRandomPositions(jmg_active); tried_current_state_as_seed_= true; auto now= std::chrono::steady_clock::now(); - bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, link, 1, remaining_time, is_valid); + bool succeeded= grasp_state.setFromIK(jmg_active, goal_pose, link_name, 1, remaining_time, is_valid); remaining_time-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); if(succeeded) { diff --git a/core/test/test_plan_generate_grasp_pose.cpp b/core/test/test_plan_generate_grasp_pose.cpp index d3040bcf..069a7f60 100644 --- a/core/test/test_plan_generate_grasp_pose.cpp +++ b/core/test/test_plan_generate_grasp_pose.cpp @@ -43,7 +43,7 @@ int main(int argc, char** argv){ st->setObject("object"); st->setTimeout(0.5); st->setAngleDelta(0.1); - st->setGraspOffset(0.03); + st->setGraspFrame(Eigen::Translation3d(.03,0,0)); t.add(std::move(st)); From 32a0de6bf3d2cab55b4071f81129fb0ba140d50d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 15:57:58 +0100 Subject: [PATCH 04/12] countDefined(): count number of defined properties --- .../moveit/task_constructor/properties.h | 41 ++++++++++++++++ core/src/properties.cpp | 47 +++++++++++++++++++ 2 files changed, 88 insertions(+) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 3f7a1aa9..07f11e2d 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -1,3 +1,41 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * 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. + *********************************************************************/ + +/* Author: Robert Haschke + Desc: PropertyMap stores variables of stages +*/ + #pragma once #include @@ -91,6 +129,9 @@ public: return boost::any_cast(value); } + /// count number of defined properties from given list + size_t countDefined(const std::vector& list) const; + /// reset properties to nil, if they have initializers void reset(); /// init properties from initializers diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 1795acdc..c4ad4d2c 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -1,3 +1,41 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * 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. + *********************************************************************/ + +/* Author: Robert Haschke + Desc: PropertyMap stores variables of stages +*/ + #include #include #include @@ -96,6 +134,15 @@ const boost::any &PropertyMap::get(const std::string &name) const return property(name).value(); } +size_t PropertyMap::countDefined(const std::vector &list) const +{ + size_t count = 0u; + for (const std::string& name : list) { + if (!get(name).empty()) ++count; + } + return count; +} + void PropertyMap::reset() { for (auto& pair : props_) { From 0d6dbee2151ab0c7874968a1fea0eb7e6e8d4640 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 10:57:38 +0100 Subject: [PATCH 05/12] initFrom() -> configureInitFrom() + performInitFrom() Use different function names for different semantics. --- core/demo/plan_pick_pa10.cpp | 12 ++++---- core/demo/plan_pick_ur5.cpp | 12 ++++---- .../moveit/task_constructor/properties.h | 28 +++++++++++++------ core/src/properties.cpp | 12 ++++---- core/src/stage.cpp | 12 ++++---- 5 files changed, 44 insertions(+), 32 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 16897048..f728b0ee 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -49,21 +49,21 @@ int main(int argc, char** argv){ { auto move = std::make_unique("open gripper"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("move to pre-grasp"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTimeout(8.0); t.add(std::move(move)); } { auto move = std::make_unique("proceed to grasp pose"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setMinMaxDistance(.05, 0.1); move->setCartesianStepSize(0.02); @@ -75,7 +75,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->properties().initFrom(PARENT); + gengrasp->properties().configureInitFrom(PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)* @@ -88,7 +88,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); @@ -96,7 +96,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("lift object"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index e4f514f7..bf76aa3e 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -49,14 +49,14 @@ int main(int argc, char** argv){ { auto move = std::make_unique("open gripper"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("move to pre-grasp"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTimeout(8.0); t.add(std::move(move)); } @@ -64,7 +64,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("proceed to grasp pose"); // move->addSolutionCallback(std::ref(t.introspection())); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setMinMaxDistance(.03, 0.1); move->setCartesianStepSize(0.02); @@ -76,7 +76,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->properties().initFrom(PARENT); + gengrasp->properties().configureInitFrom(PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); @@ -87,7 +87,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); @@ -95,7 +95,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("lift object"); - move->properties().initFrom(PARENT); + move->properties().configureInitFrom(PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 07f11e2d..79f0640e 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -58,6 +58,11 @@ enum PropertyInitializerSource { INTERFACE, }; +/** Property is a wrapper for a boost::any value, also providing a default value and a description. + * + * Properties can be configured to be initialized from another PropertyMap. + * Potential sources are the properties of the parent of a stage, or the properties passed in the interface. + */ class Property { friend class PropertyMap; @@ -74,9 +79,10 @@ public: const std::string& description() const { return description_; } std::string typeName() const { return type_index_.name(); } - /// set an initializer function - Property &initFrom(PropertyInitializerSource source, const InitializerFunction& f = fromName); - Property &initFrom(PropertyInitializerSource source, const std::string& other_name); + /// configure initialization from source using an arbitrary function + Property &configureInitFrom(PropertyInitializerSource source, const InitializerFunction& f = fromName); + /// configure initialization from source using other property name + Property &configureInitFrom(PropertyInitializerSource source, const std::string& other_name); private: std::string description_; @@ -87,6 +93,11 @@ private: }; +/** PropertyMap is map of (name, Property) pairs. + * + * Conveniency methods are provided to setup property initialization for several + * properties at once - always inheriting from the identically named external property. + */ class PropertyMap { std::map props_; @@ -113,8 +124,8 @@ public: return const_cast(this)->property(name); } - /// allow initialization from given source for listed properties - void initFrom(PropertyInitializerSource source, const std::set &properties = {}); + /// allow initialization from given source for listed properties - always using the same name + void configureInitFrom(PropertyInitializerSource source, const std::set &properties = {}); /// set the value of a property void set(const std::string& name, const boost::any& value); @@ -134,9 +145,10 @@ public: /// reset properties to nil, if they have initializers void reset(); - /// init properties from initializers - void initFrom(PropertyInitializerSource source, const PropertyMap& other, - bool checkConsistency = false); + + /// perform initialization of properties using configured initializers + void performInitFrom(PropertyInitializerSource source, const PropertyMap& other, + bool checkConsistency = false); }; } // namespace task_constructor diff --git a/core/src/properties.cpp b/core/src/properties.cpp index c4ad4d2c..318185a6 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -73,13 +73,13 @@ const boost::any &Property::value() const { return value_.empty() ? default_ : value_; } -Property& Property::initFrom(PropertyInitializerSource source, const Property::InitializerFunction &f) +Property& Property::configureInitFrom(PropertyInitializerSource source, const Property::InitializerFunction &f) { initializers_[source] = f; return *this; } -Property& Property::initFrom(PropertyInitializerSource source, const std::string &other_name) +Property& Property::configureInitFrom(PropertyInitializerSource source, const std::string &other_name) { initializers_[source] = [other_name](const PropertyMap& other, const std::string&) { return fromName(other, other_name); @@ -105,11 +105,11 @@ Property& PropertyMap::property(const std::string &name) return it->second; } -void PropertyMap::initFrom(PropertyInitializerSource source, const std::set &properties) +void PropertyMap::configureInitFrom(PropertyInitializerSource source, const std::set &properties) { for (auto &pair : props_) { if (properties.empty() || properties.count(pair.first)) - pair.second.initFrom(source); + pair.second.configureInitFrom(source); } } @@ -154,8 +154,8 @@ void PropertyMap::reset() } } -void PropertyMap::initFrom(PropertyInitializerSource source, const PropertyMap &other, - bool checkConsistency) +void PropertyMap::performInitFrom(PropertyInitializerSource source, const PropertyMap &other, + bool checkConsistency) { for (auto& pair : props_) { Property &p = pair.second; diff --git a/core/src/stage.cpp b/core/src/stage.cpp index aaf92772..6f2a9952 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -240,8 +240,8 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) { properties_.reset(); - properties_.initFrom(PARENT, parent()->properties()); - properties_.initFrom(INTERFACE, state.properties()); + properties_.performInitFrom(PARENT, parent()->properties()); + properties_.performInitFrom(INTERFACE, state.properties()); } bool PropagatingEitherWayPrivate::canCompute() const @@ -433,7 +433,7 @@ bool GeneratorPrivate::compute() { void GeneratorPrivate::initProperties() { properties_.reset(); - properties_.initFrom(PARENT, parent()->properties()); + properties_.performInitFrom(PARENT, parent()->properties()); } @@ -482,10 +482,10 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) { properties_.reset(); - properties_.initFrom(PARENT, parent()->properties()); + properties_.performInitFrom(PARENT, parent()->properties()); // properties from start/end states need to be consistent to each other - properties_.initFrom(INTERFACE, start.properties()); - properties_.initFrom(INTERFACE, end.properties(), true); + properties_.performInitFrom(INTERFACE, start.properties()); + properties_.performInitFrom(INTERFACE, end.properties(), true); } bool ConnectingPrivate::canCompute() const{ From d62b85572ae9b8448df2e2347e54929533b47e78 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 12:21:57 +0100 Subject: [PATCH 06/12] remove property name from InitializerFunction's signature --- core/include/moveit/task_constructor/properties.h | 10 +++++----- core/src/properties.cpp | 11 +++++------ 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 79f0640e..1599515b 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -50,7 +50,7 @@ namespace task_constructor { class Property; class PropertyMap; -/// initializer function, using another name from the passed property map +/// initializer function, using given name from the passed property map boost::any fromName(const PropertyMap& other, const std::string& other_name); enum PropertyInitializerSource { @@ -67,7 +67,7 @@ class Property { friend class PropertyMap; public: - typedef std::function InitializerFunction; + typedef std::function InitializerFunction; typedef std::map InitializerMap; Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); @@ -80,9 +80,9 @@ public: std::string typeName() const { return type_index_.name(); } /// configure initialization from source using an arbitrary function - Property &configureInitFrom(PropertyInitializerSource source, const InitializerFunction& f = fromName); - /// configure initialization from source using other property name - Property &configureInitFrom(PropertyInitializerSource source, const std::string& other_name); + Property &configureInitFrom(PropertyInitializerSource source, const InitializerFunction& f); + /// configure initialization from source using given other property name + Property &configureInitFrom(PropertyInitializerSource source, const std::string& name); private: std::string description_; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 318185a6..04996f86 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -79,11 +80,9 @@ Property& Property::configureInitFrom(PropertyInitializerSource source, const Pr return *this; } -Property& Property::configureInitFrom(PropertyInitializerSource source, const std::string &other_name) +Property &Property::configureInitFrom(PropertyInitializerSource source, const std::string &name) { - initializers_[source] = [other_name](const PropertyMap& other, const std::string&) { - return fromName(other, other_name); - }; + initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); }; return *this; } @@ -109,7 +108,7 @@ void PropertyMap::configureInitFrom(PropertyInitializerSource source, const std: { for (auto &pair : props_) { if (properties.empty() || properties.count(pair.first)) - pair.second.configureInitFrom(source); + pair.second.configureInitFrom(source, std::bind(&fromName, std::placeholders::_1, pair.first)); } } @@ -161,7 +160,7 @@ void PropertyMap::performInitFrom(PropertyInitializerSource source, const Proper Property &p = pair.second; auto it = p.initializers_.find(source); if (it == p.initializers_.end()) continue; - const boost::any& value = it->second(other, pair.first); + const boost::any& value = it->second(other); if (value.empty()) continue; typeCheck(value, p.type_index_); From 59fe1e78605b300b4cc860c40c72383c67ca9cde Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 11:33:35 +0100 Subject: [PATCH 07/12] separate setValue() and setCurrentValue() setValue() also updates the default value. reset() reset to the default value. setCurrentValue() only updates the current value, keeping current default. Thus setCurrentValue() can be reverted (to default) using reset(). --- .../moveit/task_constructor/properties.h | 34 ++++++++--- core/src/properties.cpp | 59 +++++++++++-------- 2 files changed, 60 insertions(+), 33 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 1599515b..643f0f88 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -60,8 +60,12 @@ enum PropertyInitializerSource { /** Property is a wrapper for a boost::any value, also providing a default value and a description. * - * Properties can be configured to be initialized from another PropertyMap. + * Properties can be configured to be initialized from another PropertyMap - if still undefined. * Potential sources are the properties of the parent of a stage, or the properties passed in the interface. + * + * Setting the value via setValue() updates both, the current value and the default value. + * Using reset() the default value can be restored. + * Using setCurrentValue() only updates the current value, allowing for later reset to the original default. */ class Property { friend class PropertyMap; @@ -72,11 +76,23 @@ public: Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); + /// set current value and default value void setValue(const boost::any& value); + void setCurrentValue(const boost::any& value); - const boost::any& value() const; + /// reset to default value (which can be empty) + void reset(); + + inline bool defined() const { return !value_.empty(); } + + /// get current value + inline const boost::any& value() const { return value_; } + /// get default value const boost::any& defaultValue() const { return default_; } + + /// get description text const std::string& description() const { return description_; } + /// get typename std::string typeName() const { return type_index_.name(); } /// configure initialization from source using an arbitrary function @@ -84,6 +100,9 @@ public: /// configure initialization from source using given other property name Property &configureInitFrom(PropertyInitializerSource source, const std::string& name); + /// set current value using configured initializers + void performInitFrom(PropertyInitializerSource source, const PropertyMap& other); + private: std::string description_; std::type_index type_index_; @@ -127,8 +146,10 @@ public: /// allow initialization from given source for listed properties - always using the same name void configureInitFrom(PropertyInitializerSource source, const std::set &properties = {}); - /// set the value of a property + /// set (and, if neccessary, declare) the value of a property void set(const std::string& name, const boost::any& value); + /// temporarily set the value of a property + void setCurrent(const std::string& name, const boost::any& value); /// get the value of a property const boost::any& get(const std::string& name) const; @@ -143,12 +164,11 @@ public: /// count number of defined properties from given list size_t countDefined(const std::vector& list) const; - /// reset properties to nil, if they have initializers + /// reset all properties to their defaults void reset(); - /// perform initialization of properties using configured initializers - void performInitFrom(PropertyInitializerSource source, const PropertyMap& other, - bool checkConsistency = false); + /// perform initialization of still undefined properties using configured initializers + void performInitFrom(PropertyInitializerSource source, const PropertyMap& other, bool enforce = false); }; } // namespace task_constructor diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 04996f86..e69fdeac 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -48,6 +48,7 @@ Property::Property(const std::type_index& type_index, const std::string& descrip : description_(description) , type_index_(type_index) , default_(default_value) + , value_(default_value) { if (!default_.empty() && std::type_index(default_.type()) != type_index_) throw std::runtime_error("type of default value doesn't match declared type"); @@ -64,16 +65,24 @@ void typeCheck(const boost::any& value, const std::type_index& type_index) } void Property::setValue(const boost::any &value) { - typeCheck(value, type_index_); - value_ = value; - // clear all initializers when value was explicitly set - initializers_.clear(); + setCurrentValue(value); + default_ = value_; } -const boost::any &Property::value() const { - return value_.empty() ? default_ : value_; +void Property::setCurrentValue(const boost::any &value) +{ + if (!value.empty()) + typeCheck(value, type_index_); + value_ = value; } +void Property::reset() +{ + value_ = default_; +} + + + Property& Property::configureInitFrom(PropertyInitializerSource source, const Property::InitializerFunction &f) { initializers_[source] = f; @@ -86,6 +95,14 @@ Property &Property::configureInitFrom(PropertyInitializerSource source, const st return *this; } +void Property::performInitFrom(PropertyInitializerSource source, const PropertyMap &other) +{ + auto it = initializers_.find(source); + if (it == initializers_.end()) return; + + setCurrentValue(it->second(other)); +} + Property& PropertyMap::declare(const std::string &name, const std::type_info &type, const std::string &description, const boost::any &default_value) @@ -128,6 +145,11 @@ void PropertyMap::set(const std::string &name, const boost::any &value) } } +void PropertyMap::setCurrent(const std::string &name, const boost::any &value) +{ + property(name).setCurrentValue(value); +} + const boost::any &PropertyMap::get(const std::string &name) const { return property(name).value(); @@ -144,31 +166,16 @@ size_t PropertyMap::countDefined(const std::vector &list) const void PropertyMap::reset() { - for (auto& pair : props_) { - Property &p = pair.second; - if (!p.initializers_.empty()) - // if there are initializers, reset property value - // explicitly setting the value, clears initializers - p.value_ = boost::any(); - } + for (auto& pair : props_) + pair.second.reset(); } -void PropertyMap::performInitFrom(PropertyInitializerSource source, const PropertyMap &other, - bool checkConsistency) +void PropertyMap::performInitFrom(PropertyInitializerSource source, const PropertyMap &other, bool enforce) { for (auto& pair : props_) { Property &p = pair.second; - auto it = p.initializers_.find(source); - if (it == p.initializers_.end()) continue; - const boost::any& value = it->second(other); - if (value.empty()) continue; - - typeCheck(value, p.type_index_); -#if 0 // TODO: we cannot generically check for equality of boost::anys - if (checkConsistency && !p.value_.empty() && p.value_ != value) - throw std::runtime_error ("inconsistent values for property '" + pair.first + "'"); -#endif - p.value_ = value; + if (enforce || !p.defined()) + p.performInitFrom(source, other); } } From f6253c46a43590ee767ec966827f540070b8ffad Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 15:06:56 +0100 Subject: [PATCH 08/12] generalize initialization source from enum to int --- .../include/moveit/task_constructor/properties.h | 16 +++++++++------- core/src/properties.cpp | 10 +++++----- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 643f0f88..4cb30c39 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -61,7 +61,8 @@ enum PropertyInitializerSource { /** Property is a wrapper for a boost::any value, also providing a default value and a description. * * Properties can be configured to be initialized from another PropertyMap - if still undefined. - * Potential sources are the properties of the parent of a stage, or the properties passed in the interface. + * A source id allows to distinguish different initialization methods (e.g. using a different reference + * name) as well as to define a priority order between sources. * * Setting the value via setValue() updates both, the current value and the default value. * Using reset() the default value can be restored. @@ -71,8 +72,9 @@ class Property { friend class PropertyMap; public: + typedef int SourceId; typedef std::function InitializerFunction; - typedef std::map InitializerMap; + typedef std::map InitializerMap; Property(const std::type_index& type_index, const std::string& description, const boost::any& default_value); @@ -96,12 +98,12 @@ public: std::string typeName() const { return type_index_.name(); } /// configure initialization from source using an arbitrary function - Property &configureInitFrom(PropertyInitializerSource source, const InitializerFunction& f); + Property &configureInitFrom(SourceId source, const InitializerFunction& f); /// configure initialization from source using given other property name - Property &configureInitFrom(PropertyInitializerSource source, const std::string& name); + Property &configureInitFrom(SourceId source, const std::string& name); /// set current value using configured initializers - void performInitFrom(PropertyInitializerSource source, const PropertyMap& other); + void performInitFrom(SourceId source, const PropertyMap& other); private: std::string description_; @@ -144,7 +146,7 @@ public: } /// allow initialization from given source for listed properties - always using the same name - void configureInitFrom(PropertyInitializerSource source, const std::set &properties = {}); + void configureInitFrom(Property::SourceId source, const std::set &properties = {}); /// set (and, if neccessary, declare) the value of a property void set(const std::string& name, const boost::any& value); @@ -168,7 +170,7 @@ public: void reset(); /// perform initialization of still undefined properties using configured initializers - void performInitFrom(PropertyInitializerSource source, const PropertyMap& other, bool enforce = false); + void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = false); }; } // namespace task_constructor diff --git a/core/src/properties.cpp b/core/src/properties.cpp index e69fdeac..d26ab5b8 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -83,19 +83,19 @@ void Property::reset() -Property& Property::configureInitFrom(PropertyInitializerSource source, const Property::InitializerFunction &f) +Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) { initializers_[source] = f; return *this; } -Property &Property::configureInitFrom(PropertyInitializerSource source, const std::string &name) +Property &Property::configureInitFrom(SourceId source, const std::string &name) { initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); }; return *this; } -void Property::performInitFrom(PropertyInitializerSource source, const PropertyMap &other) +void Property::performInitFrom(SourceId source, const PropertyMap &other) { auto it = initializers_.find(source); if (it == initializers_.end()) return; @@ -121,7 +121,7 @@ Property& PropertyMap::property(const std::string &name) return it->second; } -void PropertyMap::configureInitFrom(PropertyInitializerSource source, const std::set &properties) +void PropertyMap::configureInitFrom(Property::SourceId source, const std::set &properties) { for (auto &pair : props_) { if (properties.empty() || properties.count(pair.first)) @@ -170,7 +170,7 @@ void PropertyMap::reset() pair.second.reset(); } -void PropertyMap::performInitFrom(PropertyInitializerSource source, const PropertyMap &other, bool enforce) +void PropertyMap::performInitFrom(Property::SourceId source, const PropertyMap &other, bool enforce) { for (auto& pair : props_) { Property &p = pair.second; From 4db7e8eb886c461d2a355189facc7a24f2a8173c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 15:07:40 +0100 Subject: [PATCH 09/12] definition of PropertyInitializerSource moved to Stage --- core/demo/plan_pick_pa10.cpp | 12 ++++++------ core/demo/plan_pick_ur5.cpp | 12 ++++++------ core/include/moveit/task_constructor/properties.h | 5 ----- core/include/moveit/task_constructor/stage.h | 11 +++++++++++ core/src/stage.cpp | 12 ++++++------ 5 files changed, 29 insertions(+), 23 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index f728b0ee..0e343559 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -49,21 +49,21 @@ int main(int argc, char** argv){ { auto move = std::make_unique("open gripper"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("move to pre-grasp"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTimeout(8.0); t.add(std::move(move)); } { auto move = std::make_unique("proceed to grasp pose"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setMinMaxDistance(.05, 0.1); move->setCartesianStepSize(0.02); @@ -75,7 +75,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->properties().configureInitFrom(PARENT); + gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(0,0,.05)* @@ -88,7 +88,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); @@ -96,7 +96,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("lift object"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index bf76aa3e..e961cf7c 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -49,14 +49,14 @@ int main(int argc, char** argv){ { auto move = std::make_unique("open gripper"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTo("open"); t.add(std::move(move)); } { auto move = std::make_unique("move to pre-grasp"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTimeout(8.0); t.add(std::move(move)); } @@ -64,7 +64,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("proceed to grasp pose"); // move->addSolutionCallback(std::ref(t.introspection())); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setMinMaxDistance(.03, 0.1); move->setCartesianStepSize(0.02); @@ -76,7 +76,7 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->properties().configureInitFrom(PARENT); + gengrasp->properties().configureInitFrom(Stage::PARENT); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); @@ -87,7 +87,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); @@ -95,7 +95,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("lift object"); - move->properties().configureInitFrom(PARENT); + move->properties().configureInitFrom(Stage::PARENT); move->setMinMaxDistance(0.03, 0.05); move->setCartesianStepSize(0.01); diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 4cb30c39..3be6fb80 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -53,11 +53,6 @@ class PropertyMap; /// initializer function, using given name from the passed property map boost::any fromName(const PropertyMap& other, const std::string& other_name); -enum PropertyInitializerSource { - PARENT, - INTERFACE, -}; - /** Property is a wrapper for a boost::any value, also providing a default value and a description. * * Properties can be configured to be initialized from another PropertyMap - if still undefined. diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 8a18ed1f..b43ad309 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -80,6 +80,17 @@ class Stage { public: PRIVATE_CLASS(Stage) typedef std::unique_ptr pointer; + /** Names for property initialization sources + * + * - INTERFACE allows to pass properties from one stage to the next (in a SerialContainer). + * - PARENT allows to inherit properties from the parent. + * + * INTERFACE takes precedence over PARENT. + */ + enum PropertyInitializerSource { + PARENT, + INTERFACE, + }; virtual ~Stage(); /// auto-convert Stage to StagePrivate* when needed diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6f2a9952..718f6cd9 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -240,8 +240,8 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) { properties_.reset(); - properties_.performInitFrom(PARENT, parent()->properties()); - properties_.performInitFrom(INTERFACE, state.properties()); + properties_.performInitFrom(Stage::PARENT, parent()->properties()); + properties_.performInitFrom(Stage::INTERFACE, state.properties()); } bool PropagatingEitherWayPrivate::canCompute() const @@ -433,7 +433,7 @@ bool GeneratorPrivate::compute() { void GeneratorPrivate::initProperties() { properties_.reset(); - properties_.performInitFrom(PARENT, parent()->properties()); + properties_.performInitFrom(Stage::PARENT, parent()->properties()); } @@ -482,10 +482,10 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) { properties_.reset(); - properties_.performInitFrom(PARENT, parent()->properties()); + properties_.performInitFrom(Stage::PARENT, parent()->properties()); // properties from start/end states need to be consistent to each other - properties_.performInitFrom(INTERFACE, start.properties()); - properties_.performInitFrom(INTERFACE, end.properties(), true); + properties_.performInitFrom(Stage::INTERFACE, start.properties()); + properties_.performInitFrom(Stage::INTERFACE, end.properties(), true); } bool ConnectingPrivate::canCompute() const{ From c9e3be08d0e399d499c52fbc405066a4b5c68dc0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 15:28:25 +0100 Subject: [PATCH 10/12] fix initialization order of properties First from INTERFACE, second from PARENT. INTERFACE initialization only makes sense for Propagating stages. Connecting stages should ensure that interfaces define identical properties which is not possible with boost::any. --- .../include/moveit/task_constructor/properties.h | 8 ++++++++ core/include/moveit/task_constructor/stage.h | 2 ++ core/src/container.cpp | 8 +++++++- core/src/stage.cpp | 16 ++++++++++++---- core/src/stages/cartesian_position_motion.cpp | 2 +- core/src/stages/gripper.cpp | 10 +++++----- 6 files changed, 35 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 3be6fb80..8bc849c9 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -150,6 +150,8 @@ public: /// get the value of a property const boost::any& get(const std::string& name) const; + + /// Get typed value of property. Throws runtime_error if undefined or bad_any_cast on type mismatch. template const T& get(const std::string& name) const { const boost::any& value = get(name); @@ -157,6 +159,12 @@ public: throw std::runtime_error(std::string("undefined property: " + name)); return boost::any_cast(value); } + /// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch. + template + const T& get(const std::string& name, const T& fallback) const { + const boost::any& value = get(name); + return (value.empty()) ? fallback : boost::any_cast(value); + } /// count number of defined properties from given list size_t countDefined(const std::vector& list) const; diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index b43ad309..2074e884 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -73,6 +73,7 @@ private: std::ostream& operator<<(std::ostream &os, const InitStageException& e); +class ContainerBase; class StagePrivate; class Stage { friend std::ostream& operator<<(std::ostream &os, const Stage& stage); @@ -102,6 +103,7 @@ public: /// initialize stage once before planning virtual void init(const planning_scene::PlanningSceneConstPtr& scene); + const ContainerBase* parent() const; const std::string& name() const; void setName(const std::string& name); virtual size_t numSolutions() const = 0; diff --git a/core/src/container.cpp b/core/src/container.cpp index fbcb12b4..31c79d89 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -127,10 +127,16 @@ void ContainerBase::reset() void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene) { InitStageException errors; - auto& children = pimpl()->children(); + auto impl = pimpl(); + auto& children = impl->children(); Stage::init(scene); + // containers don't need to reset and init their properties on each execution + impl->properties_.reset(); + if (impl->parent()) + impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); + // we need to have some children to do the actual work if (children.empty()) { errors.push_back(*this, "no children"); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 718f6cd9..e9c34c0d 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -105,6 +105,10 @@ void Stage::init(const planning_scene::PlanningSceneConstPtr &scene) { } +const ContainerBase *Stage::parent() const { + return pimpl_->parent_; +} + const std::string& Stage::name() const { return pimpl_->name_; } @@ -239,9 +243,12 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) { + // reset properties to their defaults properties_.reset(); - properties_.performInitFrom(Stage::PARENT, parent()->properties()); + // first init from INTERFACE properties_.performInitFrom(Stage::INTERFACE, state.properties()); + // then init from PARENT + properties_.performInitFrom(Stage::PARENT, parent()->properties()); } bool PropagatingEitherWayPrivate::canCompute() const @@ -432,7 +439,9 @@ bool GeneratorPrivate::compute() { void GeneratorPrivate::initProperties() { + // reset properties to their defaults properties_.reset(); + // then init from PARENT properties_.performInitFrom(Stage::PARENT, parent()->properties()); } @@ -481,11 +490,10 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) void ConnectingPrivate::initProperties(const InterfaceState &start, const InterfaceState &end) { + // reset properties to their defaults properties_.reset(); + // then init from PARENT properties_.performInitFrom(Stage::PARENT, parent()->properties()); - // properties from start/end states need to be consistent to each other - properties_.performInitFrom(Stage::INTERFACE, start.properties()); - properties_.performInitFrom(Stage::INTERFACE, end.properties(), true); } bool ConnectingPrivate::canCompute() const{ diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index 17b49ed4..9fd7ae9a 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -17,7 +17,7 @@ CartesianPositionMotion::CartesianPositionMotion(std::string name) { auto& p = properties(); p.declare("group", "name of planning group"); - p.declare("link", "", "name of link used for IK"); + p.declare("link", "name of link used for IK"); p.declare("min_distance", "minimum distance to move"); p.declare("max_distance", "maximum distance to move"); p.declare("step_size", 0.005); diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp index 331e2485..f9a37d90 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -18,9 +18,9 @@ Gripper::Gripper(std::string name) { auto& p = properties(); p.declare("eef", "name of end-effector group"); - p.declare("link", "", "name of link the eef is attached to"); - p.declare("named_target", "", "named target in eef group"); - p.declare("grasp_object", "", "name of grasp object"); + p.declare("link", "name of link the eef is attached to"); + p.declare("named_target", "named target in eef group"); + p.declare("grasp_object", "name of grasp object"); } void Gripper::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -58,9 +58,9 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene const auto& props = properties(); const std::string& eef = props.get("eef"); - std::string link = props.get("link"); + std::string link = props.get("link", ""); const std::string& named_target = props.get("named_target"); - const std::string& grasp_object = props.get("grasp_object"); + const std::string& grasp_object = props.get("grasp_object", ""); if(!mgi_){ assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf"); From 3ab899a9a8bf676463982b858b2738f1fab8bc14 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 3 Feb 2018 17:29:59 +0100 Subject: [PATCH 11/12] cleanup error handling throw std::logic_error on type errors throw std::runtime_error on undeclared property don't expose generic PropertyMap::declare() --- .../moveit/task_constructor/properties.h | 7 ++++--- core/src/properties.cpp | 17 +++++++---------- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 8bc849c9..8555fd32 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -118,16 +118,17 @@ class PropertyMap { std::map props_; -public: - /// declare a property for future use + /// implementation of declare methods Property& declare(const std::string& name, const std::type_info& type, const std::string& description = "", const boost::any& default_value = boost::any()); - +public: + /// declare a property for future use template Property& declare(const std::string& name, const std::string& description = "") { return declare(name, typeid(T), description); } + /// declare a property with default value template Property& declare(const std::string& name, const T& default_value, const std::string& description = "") { diff --git a/core/src/properties.cpp b/core/src/properties.cpp index d26ab5b8..7e714d58 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -38,7 +38,6 @@ #include #include -#include #include namespace moveit { @@ -50,8 +49,8 @@ Property::Property(const std::type_index& type_index, const std::string& descrip , default_(default_value) , value_(default_value) { - if (!default_.empty() && std::type_index(default_.type()) != type_index_) - throw std::runtime_error("type of default value doesn't match declared type"); + // default value's type should match declared type by construction + assert(default_.empty() || std::type_index(default_.type()) == type_index_); } namespace { @@ -59,7 +58,7 @@ void typeCheck(const boost::any& value, const std::type_index& type_index) { if (std::type_index(value.type()) != type_index) { static boost::format fmt("type (%1%) doesn't match property's declared type (%2%)"); - throw std::runtime_error(boost::str(fmt % value.type().name() % type_index.name())); + throw std::logic_error(boost::str(fmt % value.type().name() % type_index.name())); } } } @@ -109,7 +108,7 @@ Property& PropertyMap::declare(const std::string &name, const std::type_info &ty { auto it_inserted = props_.insert(std::make_pair(name, Property(std::type_index(type), description, default_value))); if (!it_inserted.second && std::type_index(type) != it_inserted.first->second.type_index_) - throw std::runtime_error("Property '" + name + "' was already declared with different type."); + throw std::logic_error("Property '" + name + "' was already declared with different type."); return it_inserted.first->second; } @@ -117,7 +116,7 @@ Property& PropertyMap::property(const std::string &name) { auto it = props_.find(name); if (it == props_.end()) - throw std::runtime_error("Unknown property '" + name + "'"); + throw std::runtime_error("Undeclared property '" + name + "'"); return it->second; } @@ -133,10 +132,8 @@ void PropertyMap::set(const std::string &name, const boost::any &value) { auto range = props_.equal_range(name); if (range.first == range.second) { // name is not yet declared - if (value.empty()) { - ROS_ERROR("trying to define property '%s' with NULL value", name.c_str()); - return; - } + if (value.empty()) + throw std::logic_error("trying to set undeclared property '" + name + "' with NULL value"); auto it = props_.insert(range.first, std::make_pair(name, Property(value.type(), "", boost::any()))); it->second.setValue(value); } else { From 41ec46964a6f9809d327257ea9ba72821ca6c7a4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 4 Feb 2018 08:55:21 +0100 Subject: [PATCH 12/12] more unit tests --- core/test/test_properties.cpp | 115 ++++++++++++++++++++++++++++++++-- 1 file changed, 110 insertions(+), 5 deletions(-) diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index f8843964..9f885a2e 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -5,18 +5,123 @@ using namespace moveit::task_constructor; -TEST(Property, serialContainer) { +TEST(Property, standard) { + PropertyMap props; + props.declare("double1", 1, "first"); + props.declare("double2", 2); + props.declare("double4"); + + EXPECT_EQ(props.get("double1"), 1.0); + EXPECT_EQ(props.get("double2"), 2.0); + EXPECT_THROW(props.get("double3"), std::runtime_error); + + EXPECT_THROW(props.get("double4"), std::runtime_error); + EXPECT_FALSE(props.property("double4").defined()); + EXPECT_EQ(props.get("double4", 0.0), 0.0); + + props.set("double3", 3.0); + EXPECT_EQ(props.get("double3"), 3.0); +} + +TEST(Property, redeclare) { PropertyMap props; props.declare("double1"); - props.declare("double2", 1); // avoid second declaration with different type props.declare("double1"); - ASSERT_THROW(props.declare("double1"), std::runtime_error); + EXPECT_THROW(props.declare("double1"), std::logic_error); // types not matching? - ASSERT_THROW(props.set("double1", 1), std::runtime_error); + EXPECT_THROW(props.set("double1", 1), std::logic_error); props.set("double1", 3.14); - ASSERT_EQ(props.get("double1"), 3.14); + EXPECT_EQ(props.get("double1"), 3.14); +} + +TEST(Property, reset) { + PropertyMap props; + props.declare("double1"); + + // setCurrent() only assigns temporary values + props.setCurrent("double1", 1.0); + ASSERT_EQ(props.get("double1"), 1.0); + EXPECT_TRUE(props.property("double1").defaultValue().empty()); + + // they can be reset to their defaults + props.reset(); + EXPECT_FALSE(props.property("double1").defined()); + + // set() also updates the default + props.set("double1", 1.0); + EXPECT_EQ(props.get("double1"), 1.0); + EXPECT_EQ(boost::any_cast(props.property("double1").defaultValue()), 1.0); + + props.setCurrent("double1", 2.0); + EXPECT_EQ(props.get("double1"), 2.0); + EXPECT_EQ(boost::any_cast(props.property("double1").defaultValue()), 1.0); + + // back to default + props.reset(); + EXPECT_EQ(props.get("double1"), 1.0); +} + +class InitFromTest : public ::testing::Test { +protected: + void SetUp() { + master.declare("double1", 1); + master.declare("double2", 2); + master.declare("double4", 4); + + slave.declare("double1"); + slave.declare("double2"); + slave.declare("double3"); + } + PropertyMap master; + PropertyMap slave; +}; + +TEST_F(InitFromTest, standard) { + slave.configureInitFrom(0); // init all matching vars + ASSERT_FALSE(slave.property("double1").defined()); + + slave.performInitFrom(0, master); + EXPECT_EQ(slave.get("double1"), 1.0); + EXPECT_EQ(slave.get("double2"), 2.0); + EXPECT_FALSE(slave.property("double3").defined()); + EXPECT_THROW(slave.property("double4"), std::runtime_error); +} + +TEST_F(InitFromTest, limited) { + slave.configureInitFrom(0, {"double1"}); // limit init to listed props + slave.performInitFrom(0, master); + EXPECT_EQ(slave.get("double1"), 1.0); + EXPECT_FALSE(slave.property("double2").defined()); + EXPECT_FALSE(slave.property("double3").defined()); + EXPECT_THROW(slave.property("double4"), std::runtime_error); +} + +TEST_F(InitFromTest, sourceId) { + slave.configureInitFrom(0); // init all matching vars + slave.performInitFrom(1, master); // init with wrong sourceId -> no effect + EXPECT_FALSE(slave.property("double1").defined()); + EXPECT_FALSE(slave.property("double2").defined()); + EXPECT_FALSE(slave.property("double3").defined()); + EXPECT_THROW(slave.property("double4"), std::runtime_error); +} + +TEST_F(InitFromTest, otherName) { + slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2 + slave.performInitFrom(0, master); + EXPECT_EQ(slave.get("double1"), 2.0); + EXPECT_FALSE(slave.property("double2").defined()); + EXPECT_FALSE(slave.property("double3").defined()); + EXPECT_THROW(slave.property("double4"), std::runtime_error); +} + +TEST_F(InitFromTest, function) { + slave.property("double3").configureInitFrom(0, [](const PropertyMap& other) -> boost::any { + return other.get("double1") + other.get("double2"); + }); + slave.performInitFrom(0, master); + EXPECT_EQ(slave.get("double3"), 3.0); }