From cbb2cd69f77844945730feceebddd95c254e1452 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 17:26:59 +0200 Subject: [PATCH 1/7] property inheritance: both from PARENT and INTERFACE - source_id -> source_flags: bits indicating configured paths - initializers, e.g. fromName(), should throw - ignore undeclared errors during inheritance - on undefined error, reset the value to None - override value only if previously set by lower-priority source MANUAL > CURRENT > PARENT > INTERFACE --- .../moveit/task_constructor/properties.h | 18 +++--- core/include/moveit/task_constructor/stage.h | 8 ++- core/src/properties.cpp | 58 ++++++++++++------- core/src/stage.cpp | 4 +- core/src/stages/compute_ik.cpp | 2 +- core/test/test_properties.cpp | 26 ++++----- 6 files changed, 66 insertions(+), 50 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 6fce3790..250d389b 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -107,7 +107,7 @@ public: /// exception thrown when trying to set a value not matching the declared type class type_error; - typedef int SourceId; + typedef uint SourceFlags; /// function callback used to initialize property value from another PropertyMap typedef std::function InitializerFunction; /// function callback used to signal value setting to external components @@ -138,14 +138,11 @@ public: std::string typeName() const { return type_index_.name(); } /// return true, if property initialized from given SourceId - bool initsFrom(SourceId source) const; + bool initsFrom(SourceFlags source) const; /// configure initialization from source using an arbitrary function - Property &configureInitFrom(SourceId source, const InitializerFunction& f); + Property &configureInitFrom(SourceFlags source, const InitializerFunction& f); /// configure initialization from source using given other property name - Property &configureInitFrom(SourceId source, const std::string& name); - - /// set current value using matching configured initializers - void performInitFrom(SourceId source, const PropertyMap& other); + Property &configureInitFrom(SourceFlags source, const std::string& name); /// define a function callback to be called on each value update /// note, that boost::any doesn't allow for change detection @@ -158,7 +155,8 @@ private: boost::any value_; /// used for external initialization - SourceId source_id_ = 0; + SourceFlags source_flags_ = 0; + SourceFlags initialized_from_; InitializerFunction initializer_; SignalFunction signaller_; SerializeFunction serialize_; @@ -237,7 +235,7 @@ public: const_iterator end() const { return props_.end(); } /// allow initialization from given source for listed properties - always using the same name - void configureInitFrom(Property::SourceId source, const std::set &properties = {}); + void configureInitFrom(Property::SourceFlags source, const std::set &properties = {}); /// set (and, if neccessary, declare) the value of a property template @@ -282,7 +280,7 @@ public: void reset(); /// perform initialization of still undefined properties using configured initializers - void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = false); + void performInitFrom(Property::SourceFlags source, const PropertyMap& other); }; // boost::any needs a specialization to avoid recursion diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index ef56e267..d16a6b26 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -132,9 +132,11 @@ public: * * INTERFACE takes precedence over PARENT. */ - enum PropertyInitializerSource { - PARENT, - INTERFACE, + enum PropertyInitializerSource { // TODO: move to property.cpp + DEFAULT = 0, + MANUAL = 1, + PARENT = 2, + INTERFACE = 4, }; virtual ~Stage(); diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 75f6693e..53233109 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -49,15 +49,18 @@ Property::Property(const std::type_index& type_index, const std::string& descrip , type_index_(type_index) , default_(default_value) , value_() + , initialized_from_(-1) , serialize_(serialize) { // default value's type should match declared type by construction assert(default_.empty() || std::type_index(default_.type()) == type_index_); + reset(); } void Property::setValue(const boost::any &value) { setCurrentValue(value); default_ = value_; + initialized_from_ = 0; } void Property::setCurrentValue(const boost::any &value) @@ -66,13 +69,18 @@ void Property::setCurrentValue(const boost::any &value) throw Property::type_error(value.type().name(), type_index_.name()); value_ = value; + initialized_from_ = 1; // manually initialized TODO: use enums + if (signaller_) signaller_(this); } void Property::reset() { + if (initialized_from_ == 0) // TODO: use enum + return; // keep manually set values boost::any().swap(value_); + initialized_from_ = -1; // set to max value } std::string Property::serialize() const { @@ -80,32 +88,26 @@ std::string Property::serialize() const { return serialize_(value()); } -bool Property::initsFrom(Property::SourceId source) const +bool Property::initsFrom(Property::SourceFlags source) const { - return (source == source_id_ && initializer_); + return source & source_flags_; } -Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f) +Property& Property::configureInitFrom(SourceFlags source, const Property::InitializerFunction &f) { - if (source != source_id_ && initializer_) + if (source != source_flags_ && initializer_) throw error("Property was already configured for initialization from another source id"); - source_id_ = source; + source_flags_ = f ? source : SourceFlags(); initializer_ = f; return *this; } -Property &Property::configureInitFrom(SourceId source, const std::string &name) +Property &Property::configureInitFrom(SourceFlags source, const std::string &name) { return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); }); } -void Property::performInitFrom(SourceId source, const PropertyMap &other) -{ - if (source_id_ != source || !initializer_) return; // source ids not matching - setCurrentValue(initializer_(other)); -} - Property& PropertyMap::declare(const std::string &name, const std::type_index &type_index, const std::string &description, const boost::any &default_value, @@ -143,7 +145,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const st other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); } -void PropertyMap::configureInitFrom(Property::SourceId source, const std::set &properties) +void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) { for (auto &pair : props_) { if (properties.empty() || properties.count(pair.first)) @@ -194,23 +196,37 @@ void PropertyMap::reset() pair.second.reset(); } -void PropertyMap::performInitFrom(Property::SourceId source, const PropertyMap &other, bool enforce) +void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMap &other) { for (auto& pair : props_) { Property &p = pair.second; - if (enforce || !p.defined()) - p.performInitFrom(source, other); + + // don't override value previously set by higher-priority source + // MANUAL > CURRENT > PARENT > INTERFACE + if (p.initialized_from_ < source && p.defined()) + continue; + // is the property configured for initialization from this source? + if (!p.initsFrom(source)) + continue; + + boost::any value; + try { + value = p.initializer_(other); + } catch (const Property::undeclared&) { + // ignore undeclared + continue; + } catch (const Property::undefined&) { + } + + p.setCurrentValue(value); + p.initialized_from_ = source; } } 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(); - } + return other.get(other_name); } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 58e08960..6c320600 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -440,13 +440,13 @@ void PropagatingEitherWayPrivate::compute() if (hasStartState()) { const InterfaceState& state = fetchStartState(); // enforce property initialization from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); + properties_.performInitFrom(Stage::INTERFACE, state.properties()); me->computeForward(state); } if (hasEndState()) { const InterfaceState& state = fetchEndState(); // enforce property initialization from INTERFACE - properties_.performInitFrom(Stage::INTERFACE, state.properties(), true); + properties_.performInitFrom(Stage::INTERFACE, state.properties()); me->computeBackward(state); } } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8f2a4828..87ae14ff 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -212,7 +212,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) // -1 TODO: this should not be necessary in my opinion: Why do you think so? // It is, because the properties on the interface might change from call to call... // enforced initialization from interface ensures that new target_pose is read - properties().performInitFrom(INTERFACE, s.start()->properties(), true); + properties().performInitFrom(INTERFACE, s.start()->properties()); const auto& props = properties(); const bool ignore_collisions = props.get("ignore_collisions"); diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp index 67e9f576..17226ed3 100644 --- a/core/test/test_properties.cpp +++ b/core/test/test_properties.cpp @@ -105,10 +105,10 @@ protected: }; TEST_F(InitFromTest, standard) { - slave.configureInitFrom(0); // init all matching vars + slave.configureInitFrom(1); // init all matching vars ASSERT_FALSE(slave.property("double1").defined()); - slave.performInitFrom(0, master); + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 1.0); EXPECT_EQ(slave.get("double2"), 2.0); EXPECT_FALSE(slave.property("double3").defined()); @@ -116,8 +116,8 @@ TEST_F(InitFromTest, standard) { } TEST_F(InitFromTest, limited) { - slave.configureInitFrom(0, {"double1"}); // limit init to listed props - slave.performInitFrom(0, master); + slave.configureInitFrom(1, {"double1"}); // limit init to listed props + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 1.0); EXPECT_FALSE(slave.property("double2").defined()); EXPECT_FALSE(slave.property("double3").defined()); @@ -125,8 +125,8 @@ TEST_F(InitFromTest, limited) { } TEST_F(InitFromTest, sourceId) { - slave.configureInitFrom(0); // init all matching vars - slave.performInitFrom(1, master); // init with wrong sourceId -> no effect + slave.configureInitFrom(1); // init all matching vars + slave.performInitFrom(2, 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()); @@ -134,14 +134,14 @@ TEST_F(InitFromTest, sourceId) { } TEST_F(InitFromTest, multipleSourceIds) { - slave.configureInitFrom(0); - slave.configureInitFrom(0); // init is allowed second time with same id - EXPECT_THROW(slave.configureInitFrom(1), std::runtime_error); // but not with other id + slave.configureInitFrom(1); + slave.configureInitFrom(1); // init is allowed second time with same id + EXPECT_THROW(slave.configureInitFrom(2), std::runtime_error); // but not with other id } TEST_F(InitFromTest, otherName) { - slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2 - slave.performInitFrom(0, master); + slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2 + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double1"), 2.0); EXPECT_FALSE(slave.property("double2").defined()); EXPECT_FALSE(slave.property("double3").defined()); @@ -149,9 +149,9 @@ TEST_F(InitFromTest, otherName) { } TEST_F(InitFromTest, function) { - slave.property("double3").configureInitFrom(0, [](const PropertyMap& other) -> boost::any { + slave.property("double3").configureInitFrom(1, [](const PropertyMap& other) -> boost::any { return other.get("double1") + other.get("double2"); }); - slave.performInitFrom(0, master); + slave.performInitFrom(1, master); EXPECT_EQ(slave.get("double3"), 3.0); } From beffe27987a2ade3f7c446c6844388f40f05f537 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 17:28:16 +0200 Subject: [PATCH 2/7] improve property debugging --- core/include/moveit/task_constructor/properties.h | 3 ++- core/src/container.cpp | 9 ++++++++- core/src/properties.cpp | 11 +++++++---- core/src/stage.cpp | 3 ++- 4 files changed, 19 insertions(+), 7 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 250d389b..cd5d4b14 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -128,7 +128,8 @@ public: /// get default value const boost::any& defaultValue() const { return default_; } /// serialize current value - std::string serialize() const; + std::string serialize(const boost::any& value) const; + std::string serialize() const { return serialize(value()); } /// get description text const std::string& description() const { return description_; } diff --git a/core/src/container.cpp b/core/src/container.cpp index 0b08f0c8..3f1c23b0 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -254,7 +254,14 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) // recursively init all children and accumulate errors InitStageException errors; for (auto& child : children) { - try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); } + try { child->init(robot_model); } + catch (const Property::error &e) { + std::ostringstream oss; + oss << e.what(); + pimpl()->composePropertyErrorMsg(e.name(), oss); + errors.push_back(*child, oss.str()); + } + catch (InitStageException &e) { errors.append(e); } } if (errors) throw errors; diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 53233109..1b028761 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -39,6 +39,7 @@ #include #include #include +#include namespace moveit { namespace task_constructor { @@ -83,9 +84,9 @@ void Property::reset() initialized_from_ = -1; // set to max value } -std::string Property::serialize() const { - if (!serialize_) return ""; - return serialize_(value()); +std::string Property::serialize(const boost::any& v) const { + if (!serialize_ || v.empty()) return ""; + return serialize_(v); } bool Property::initsFrom(Property::SourceFlags source) const @@ -142,7 +143,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set &prop void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) { const Property& p = property(name); - other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); + other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); } void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set &properties) @@ -218,6 +219,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa } catch (const Property::undefined&) { } + ROS_DEBUG_STREAM_NAMED("Properties", pair.first << ": " << p.initialized_from_ << + " -> " << source << ": " << p.serialize(value)); p.setCurrentValue(value); p.initialized_from_ = source; } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 6c320600..133dd1d0 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -222,6 +222,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model) impl->properties_.reset(); if (impl->parent()) { try { + ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'"); impl->properties_.performInitFrom(PARENT, impl->parent()->properties()); } catch (const Property::error &e) { std::ostringstream oss; @@ -299,7 +300,7 @@ void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std os << "declared, but undefined"; if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent"; - else if (p.initsFrom(Stage::PARENT)) os << ", initializes from interface"; + if (p.initsFrom(Stage::INTERFACE)) os << ", initializes from interface"; } catch (const Property::undeclared &e) { os << "undeclared"; } From 92296e020f712b2f62203733063d60d38d75d8a7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 1 Jun 2018 10:13:18 +0200 Subject: [PATCH 3/7] SimpleGrasp(GraspGenerator) allow any GraspGenerator stage that provides "pregrasp" and "grasp" postures as well as a target_pose for grasping. --- core/demo/plan_pick_pr2.cpp | 15 +++--- core/demo/plan_pick_ur5.cpp | 17 ++++--- .../task_constructor/stages/simple_grasp.h | 38 ++++++++------- core/src/stages/simple_grasp.cpp | 47 ++++++++++++------- 4 files changed, 71 insertions(+), 46 deletions(-) diff --git a/core/demo/plan_pick_pr2.cpp b/core/demo/plan_pick_pr2.cpp index c6f36d1a..0166c2f1 100644 --- a/core/demo/plan_pick_pr2.cpp +++ b/core/demo/plan_pick_pr2.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -55,15 +56,17 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique(); - grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + auto grasp_generator = std::make_unique("generate grasp pose"); grasp_generator->setAngleDelta(.2); - grasp_generator->setPreGraspPose("open"); - grasp_generator->setGraspPose("closed"); - grasp_generator->setMonitoredStage(initial_stage); + + auto grasp = std::make_unique(std::move(grasp_generator)); + grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); + grasp->setPreGraspPose("open"); + grasp->setGraspPose("closed"); + grasp->setMonitoredStage(initial_stage); // pick stage - auto pick = std::make_unique(std::move(grasp_generator)); + auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("left_gripper")); pick->setProperty("object", std::string("object")); geometry_msgs::TwistStamped approach; diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 3cd1919e..42ad3c2b 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -1,6 +1,7 @@ #include #include +#include #include #include #include @@ -55,15 +56,17 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique(); - grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); - grasp_generator->setMaxIKSolutions(8); + auto grasp_generator = std::make_unique("generate grasp pose"); grasp_generator->setAngleDelta(.2); - grasp_generator->setPreGraspPose("open"); - grasp_generator->setGraspPose("closed"); - grasp_generator->setMonitoredStage(initial_stage); - auto pick = std::make_unique(std::move(grasp_generator)); + auto grasp = std::make_unique(std::move(grasp_generator)); + grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); + grasp->setMaxIKSolutions(8); + grasp->setPreGraspPose("open"); + grasp->setGraspPose("closed"); + grasp->setMonitoredStage(initial_stage); + + auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("gripper")); pick->setProperty("object", std::string("object")); geometry_msgs::TwistStamped approach; diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 2e6e9207..95676f06 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -48,34 +48,45 @@ class GenerateGraspPose; /** base class for simple grasping / ungrasping * - * Given a named pre-grasp and grasp posture the stage generates fully-qualified - * pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector. + * Given a pre-grasp and grasp posture the stage generates a trajectory + * connecting these two states by a grasping trajectory. + * The class takes a generator stage that provides the grasp pose and + * optionally the pre-grasp and grasp postures. * * Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class * provides the common functionality for grasping (forward = true) and ungrasping (forward = false). */ class SimpleGraspBase : public SerialContainer { moveit::core::RobotModelConstPtr model_; - GenerateGraspPose* grasp_generator_ = nullptr; + MonitoringGenerator* generator_ = nullptr; + +protected: + void setup(std::unique_ptr&& generator, bool forward); public: - SimpleGraspBase(const std::string& name, bool forward); + SimpleGraspBase(const std::string& name); void init(const moveit::core::RobotModelConstPtr& robot_model) override; void setMonitoredStage(Stage* monitored); void setEndEffector(const std::string& eef) { - properties().set("eef", eef); + properties().set("eef", eef); } void setObject(const std::string& object) { - properties().set("object", object); + properties().set("object", object); } void setPreGraspPose(const std::string& pregrasp) { - properties().set("pregrasp", pregrasp); + properties().set("pregrasp", pregrasp); } void setGraspPose(const std::string& grasp) { - properties().set("grasp", grasp); + properties().set("grasp", grasp); + } + void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { + properties().set("pregrasp", pregrasp); + } + void setGraspPose(const moveit_msgs::RobotState& grasp) { + properties().set("grasp", grasp); } void setIKFrame(const geometry_msgs::PoseStamped &transform) { @@ -91,30 +102,23 @@ public: setIKFrame(Eigen::Affine3d::Identity(), link); } - void setAngleDelta(double angle_delta) { - properties().set("angle_delta", angle_delta); - } - void setMaxIKSolutions(uint32_t max_ik_solutions) { properties().set("max_ik_solutions", max_ik_solutions); } - void setTimeout(double timeout) { - properties().set("timeout", timeout); - } }; /// specialization of SimpleGraspBase to realize grasping class SimpleGrasp : public SimpleGraspBase { public: - SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {} + SimpleGrasp(std::unique_ptr&& generator, const std::string& name = "grasp"); }; /// specialization of SimpleGraspBase to realize ungrasping class SimpleUnGrasp : public SimpleGraspBase { public: - SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {} + SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name = "ungrasp"); }; } } } diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 7bc7ab75..5412e90e 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -36,7 +36,6 @@ #include -#include #include #include #include @@ -49,24 +48,29 @@ namespace moveit { namespace task_constructor { namespace stages { -SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) +SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name) +{ + auto &props = properties(); + props.declare("pregrasp", "pregrasp posture"); + props.declare("grasp", "grasp posture"); +} + +void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - auto gengrasp = std::make_unique(forward ? "generate grasp pose" : "generate release pose"); - grasp_generator_ = gengrasp.get(); - - auto ik = std::make_unique("compute ik", std::move(gengrasp)); - const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" }; + generator_ = generator.get(); + auto ik = new ComputeIK("compute ik", std::move(generator)); + const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object" }; ik->exposePropertiesOfChild(0, grasp_prop_names); - insert(std::move(ik), insertion_position); + insert(std::unique_ptr(ik), insertion_position); exposePropertiesOfChild(insertion_position, grasp_prop_names); exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { - auto allow_touch = std::make_unique(forward ? "allow object collision" : "forbid object collision"); + auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision"); PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); @@ -79,25 +83,25 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef) ->getLinkModelNamesWithCollisionGeometry(), true); }); - insert(std::move(allow_touch), insertion_position); + insert(std::unique_ptr(allow_touch), insertion_position); } { auto pipeline = std::make_shared(); pipeline->setTimeout(8.0); pipeline->setPlannerId("RRTConnectkConfigDefault"); - auto move = std::make_unique(forward ? "close gripper" : "open gripper", pipeline); + auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline); PropertyMap& p = move->properties(); p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ const std::string& eef = parent_map.get("eef"); const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef); return boost::any(jmg->getName()); }); - insert(std::move(move), insertion_position); - exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp"); + insert(std::unique_ptr(move), insertion_position); + exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp"); } { - auto attach = std::make_unique(forward ? "attach object" : "detach object"); + auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object"); PropertyMap& p = attach->properties(); p.declare("eef"); p.declare("object"); @@ -111,7 +115,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) obj.object.id = p.get("object"); scene->processAttachedCollisionObjectMsg(obj); }); - insert(std::move(attach), insertion_position); + insert(std::unique_ptr(attach), insertion_position); } } @@ -123,7 +127,7 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) void SimpleGraspBase::setMonitoredStage(Stage* monitored) { - grasp_generator_->setMonitoredStage(monitored); + generator_->setMonitoredStage(monitored); } void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { @@ -133,4 +137,15 @@ void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& setIKFrame(pose_msg); } +SimpleGrasp::SimpleGrasp(std::unique_ptr&& generator, const std::string& name) + : SimpleGraspBase(name) +{ + setup(std::move(generator), true); +} + +SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name) + : SimpleGraspBase(name) { + setup(std::move(generator), false); +} + } } } From 0dab499533dbb252d7ffc177494a376bc536cb21 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 13:53:00 +0200 Subject: [PATCH 4/7] Stage: generally allow forwarding of interface properties --- core/include/moveit/task_constructor/stage.h | 7 +++++++ .../moveit/task_constructor/stages/compute_ik.h | 4 ---- core/src/stage.cpp | 15 +++++++++++++++ core/src/stages/compute_ik.cpp | 8 +------- 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index ef56e267..7cf0d380 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -159,6 +159,13 @@ public: void setTimeout(double timeout) { setProperty("timeout", timeout); } double timeout() const { return properties().get("timeout"); } + /// forwarding of properties between interface states + void forwardProperties(const InterfaceState& source, InterfaceState& dest); + std::set forwardedProperties() const + { return properties().get>("forwarded_properties"); } + void setForwardedProperties(const std::set& names) + { setProperty("forwarded_properties", names); } + typedef std::function SolutionCallback; typedef std::list SolutionCallbackList; /// add function to be called for every newly found solution or failure diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index d982b55e..39cdf09a 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -104,10 +104,6 @@ public: void setIgnoreCollisions(bool flag) { setProperty("ignore_collisions", flag); } - - void setForwardedProperties(const std::set& names) { - setProperty("forward_properties", names); - } }; } } } diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 58e08960..cfb2da0e 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -105,6 +105,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to, assert(nextStarts()); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(from, to); auto to_it = states_.insert(states_.end(), std::move(to)); @@ -122,6 +123,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to, assert(prevEnds()); if (!storeSolution(solution)) return; // solution dropped + me()->forwardProperties(to, from); auto from_it = states_.insert(states_.end(), std::move(from)); @@ -181,6 +183,8 @@ Stage::Stage(StagePrivate *impl) assert(impl); auto& p = properties(); p.declare("timeout", "timeout per run (s)"); + p.declare>("forwarded_properties", std::set(), + "set of interface properties to forward"); p.declare("marker_ns", "marker namespace"); } @@ -246,6 +250,17 @@ void Stage::setName(const std::string& name) pimpl_->name_ = name; } +void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) +{ + const PropertyMap& src = source.properties(); + PropertyMap& dst = dest.properties(); + for (const auto& name : forwardedProperties()) { + if (!src.hasProperty(name)) + continue; + dst.set(name, src.get(name)); + } +} + Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb) { auto impl = pimpl(); diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 8f2a4828..85d93092 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -61,7 +61,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare("max_ik_solutions", 1); p.declare("ignore_collisions", false); - p.declare>("forward_properties", "to-be-forwarded properties from input"); // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); @@ -370,12 +369,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s) robot_state.update(); InterfaceState state(scene); - const boost::any &forwards = props.get("forward_properties"); - if (!forwards.empty()) { - auto p = s.start()->properties(); - p.exposeTo(state.properties(), boost::any_cast>(forwards)); - } - + forwardProperties(*s.start(), state); spawn(std::move(state), std::move(solution)); } From a8431ce6618cf008cde3f448b11d10f271f6951c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 13:54:55 +0200 Subject: [PATCH 5/7] Container::exposePropertiesOfChild: allow skipping of undefined props --- .../moveit/task_constructor/container.h | 4 +-- core/src/container.cpp | 29 ++++++++++++------- core/src/properties.cpp | 2 +- core/src/stages/simple_grasp.cpp | 4 +-- 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 1149af9c..7b506e78 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -63,10 +63,10 @@ public: virtual void clear(); /// declare all given variables of child and configure child to inherit them from parent - void exposePropertiesOfChild(int child, const std::initializer_list& names); + void exposePropertiesOfChild(int child, const std::initializer_list& names, bool skip_undefined = false); /// declare given child property with another name and configure child to inherit it from parent void exposePropertyOfChildAs(int child, const std::string& child_property_name, - const std::string& parent_property_name); + const std::string& parent_property_name, bool skip_undefined = false); void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; diff --git a/core/src/container.cpp b/core/src/container.cpp index 0b08f0c8..83fa0031 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -194,33 +194,42 @@ void ContainerBase::clear() pimpl()->children_.clear(); } -void ContainerBase::exposePropertiesOfChild(int child, const std::initializer_list& names) +void ContainerBase::exposePropertiesOfChild(int child, const std::initializer_list& names, bool skip_undefined) { auto impl = pimpl(); ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false); if (child_it == impl->children().end()) throw std::runtime_error("invalid child index"); - auto &child_props = (*child_it)->properties(); - // declare variables - child_props.exposeTo(impl->properties_, names); - // configure inheritance - child_props.configureInitFrom(Stage::PARENT, names); + PropertyMap &source = (*child_it)->properties(); + PropertyMap &target = impl->properties_; + for (const std::string& name : names) { + if (skip_undefined && !source.hasProperty(name)) + continue; + // declare variables + source.exposeTo(target, name, name); + // configure inheritance + source.configureInitFrom(Stage::PARENT, {name}); + } } void ContainerBase::exposePropertyOfChildAs(int child, const std::string& child_property_name, - const std::string& parent_property_name) + const std::string& parent_property_name, bool skip_undefined) { auto impl = pimpl(); ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false); if (child_it == impl->children().end()) throw std::runtime_error("invalid child index"); - auto &child_props = (*child_it)->properties(); + PropertyMap &source = (*child_it)->properties(); + PropertyMap &target = impl->properties_; + if (skip_undefined && !source.hasProperty(child_property_name)) + return; + // declare variables - child_props.exposeTo(impl->properties_, child_property_name, parent_property_name); + source.exposeTo(target, child_property_name, parent_property_name); // configure inheritance - child_props.property(child_property_name).configureInitFrom(Stage::PARENT, parent_property_name); + source.property(child_property_name).configureInitFrom(Stage::PARENT, parent_property_name); } void ContainerBase::reset() diff --git a/core/src/properties.cpp b/core/src/properties.cpp index 75f6693e..6e3d5848 100644 --- a/core/src/properties.cpp +++ b/core/src/properties.cpp @@ -140,7 +140,7 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set &prop void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) { const Property& p = property(name); - other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); + other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_); } void PropertyMap::configureInitFrom(Property::SourceId source, const std::set &properties) diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 5412e90e..7d988fbe 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -63,10 +63,10 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bo generator_ = generator.get(); auto ik = new ComputeIK("compute ik", std::move(generator)); const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object" }; - ik->exposePropertiesOfChild(0, grasp_prop_names); + ik->exposePropertiesOfChild(0, grasp_prop_names, true); insert(std::unique_ptr(ik), insertion_position); - exposePropertiesOfChild(insertion_position, grasp_prop_names); + exposePropertiesOfChild(insertion_position, grasp_prop_names, true); exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { From 157caac38527677a44b2c5de603939322718608a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 2 Jun 2018 22:39:22 +0200 Subject: [PATCH 6/7] generalize SimpleGrasp / GenerateGraspPose - move "pregrasp", "grasp" property from SimpleGrasp to GenerateGraspPose - Container::exposePropertiesOfChild: decouple exposure from inheritance --- core/demo/plan_pick_pa10.cpp | 9 +-- core/demo/plan_pick_pr2.cpp | 10 +-- core/demo/plan_pick_ur5.cpp | 10 +-- .../stages/generate_grasp_pose.h | 12 ++-- .../task_constructor/stages/simple_grasp.h | 38 +++-------- core/src/container.cpp | 4 -- core/src/stages/compute_ik.cpp | 1 - core/src/stages/generate_grasp_pose.cpp | 21 ++---- core/src/stages/simple_grasp.cpp | 66 ++++++++++++------- 9 files changed, 77 insertions(+), 94 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 7a71156c..9141ab19 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -99,15 +99,16 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); gengrasp->properties().configureInitFrom(Stage::PARENT); - gengrasp->setNamedPose("open"); + gengrasp->setPreGraspPose("open"); gengrasp->setObject("object"); gengrasp->setAngleDelta(M_PI / 10.); gengrasp->setMonitoredStage(initial_stage); auto ik = std::make_unique("compute ik", std::move(gengrasp)); - ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"}); - ik->setIKFrame(Eigen::Translation3d(0,0,.05)* - Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), + PropertyMap &props = ik->properties(); + props.configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"}); + props.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution + ik->setIKFrame(Eigen::Translation3d(0,0,.05) * Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), "lh_tool_frame"); ik->setMaxIKSolutions(1); t.add(std::move(ik)); diff --git a/core/demo/plan_pick_pr2.cpp b/core/demo/plan_pick_pr2.cpp index 0166c2f1..4336980c 100644 --- a/core/demo/plan_pick_pr2.cpp +++ b/core/demo/plan_pick_pr2.cpp @@ -56,14 +56,14 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique("generate grasp pose"); + auto grasp_generator = new stages::GenerateGraspPose("generate grasp pose"); grasp_generator->setAngleDelta(.2); + grasp_generator->setPreGraspPose("open"); + grasp_generator->setGraspPose("closed"); + grasp_generator->setMonitoredStage(initial_stage); - auto grasp = std::make_unique(std::move(grasp_generator)); + auto grasp = std::make_unique(std::unique_ptr(grasp_generator)); grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame"); - grasp->setPreGraspPose("open"); - grasp->setGraspPose("closed"); - grasp->setMonitoredStage(initial_stage); // pick stage auto pick = std::make_unique(std::move(grasp)); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 42ad3c2b..313c586d 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -56,15 +56,15 @@ int main(int argc, char** argv){ t.add(std::move(connect)); // grasp generator - auto grasp_generator = std::make_unique("generate grasp pose"); + auto grasp_generator = new stages::GenerateGraspPose("generate grasp pose"); grasp_generator->setAngleDelta(.2); + grasp_generator->setPreGraspPose("open"); + grasp_generator->setGraspPose("closed"); + grasp_generator->setMonitoredStage(initial_stage); - auto grasp = std::make_unique(std::move(grasp_generator)); + auto grasp = std::make_unique(std::unique_ptr(grasp_generator)); grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); grasp->setMaxIKSolutions(8); - grasp->setPreGraspPose("open"); - grasp->setGraspPose("closed"); - grasp->setMonitoredStage(initial_stage); auto pick = std::make_unique(std::move(grasp)); pick->setProperty("eef", std::string("gripper")); 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 2c2d4863..64eb0983 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -49,10 +49,14 @@ public: void init(const core::RobotModelConstPtr &robot_model) override; void compute() override; - void setEndEffector(const std::string &eef); - void setNamedPose(const std::string &pose_name); - void setObject(const std::string &object); - void setAngleDelta(double delta); + void setEndEffector(const std::string &eef) { setProperty("eef", eef); } + void setObject(const std::string &object) { setProperty("object", object); } + void setAngleDelta(double delta) { setProperty("angle_delta", delta); } + + void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); } + void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); } + void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); } + void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); } protected: void onNewSolution(const SolutionBase& s) override; diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index 95676f06..0b95ea03 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -58,40 +58,20 @@ class GenerateGraspPose; */ class SimpleGraspBase : public SerialContainer { moveit::core::RobotModelConstPtr model_; - MonitoringGenerator* generator_ = nullptr; protected: - void setup(std::unique_ptr&& generator, bool forward); + void setup(std::unique_ptr&& generator, bool forward); public: SimpleGraspBase(const std::string& name); void init(const moveit::core::RobotModelConstPtr& robot_model) override; - void setMonitoredStage(Stage* monitored); - void setEndEffector(const std::string& eef) { - properties().set("eef", eef); - } - void setObject(const std::string& object) { - properties().set("object", object); - } + void setEndEffector(const std::string& eef) { setProperty("eef", eef); } + void setObject(const std::string& object) { setProperty("object", object); } - void setPreGraspPose(const std::string& pregrasp) { - properties().set("pregrasp", pregrasp); - } - void setGraspPose(const std::string& grasp) { - properties().set("grasp", grasp); - } - void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { - properties().set("pregrasp", pregrasp); - } - void setGraspPose(const moveit_msgs::RobotState& grasp) { - properties().set("grasp", grasp); - } - - void setIKFrame(const geometry_msgs::PoseStamped &transform) { - properties().set("ik_frame", transform); - } + /// set properties of IK solver + void setIKFrame(const geometry_msgs::PoseStamped &transform) { setProperty("ik_frame", transform); } void setIKFrame(const Eigen::Affine3d& pose, const std::string& link); template void setIKFrame(const T& t, const std::string& link) { @@ -102,23 +82,21 @@ public: setIKFrame(Eigen::Affine3d::Identity(), link); } - void setMaxIKSolutions(uint32_t max_ik_solutions) { - properties().set("max_ik_solutions", max_ik_solutions); - } + void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); } }; /// specialization of SimpleGraspBase to realize grasping class SimpleGrasp : public SimpleGraspBase { public: - SimpleGrasp(std::unique_ptr&& generator, const std::string& name = "grasp"); + SimpleGrasp(std::unique_ptr&& generator, const std::string& name = "grasp"); }; /// specialization of SimpleGraspBase to realize ungrasping class SimpleUnGrasp : public SimpleGraspBase { public: - SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name = "ungrasp"); + SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name = "ungrasp"); }; } } } diff --git a/core/src/container.cpp b/core/src/container.cpp index 83fa0031..c91cf6c1 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -208,8 +208,6 @@ void ContainerBase::exposePropertiesOfChild(int child, const std::initializer_li continue; // declare variables source.exposeTo(target, name, name); - // configure inheritance - source.configureInitFrom(Stage::PARENT, {name}); } } @@ -228,8 +226,6 @@ void ContainerBase::exposePropertyOfChildAs(int child, const std::string& child_ // declare variables source.exposeTo(target, child_property_name, parent_property_name); - // configure inheritance - source.property(child_property_name).configureInitFrom(Stage::PARENT, parent_property_name); } void ContainerBase::reset() diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 85d93092..453b1000 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -65,7 +65,6 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) // ik_frame and target_pose are read from the interface p.declare("ik_frame", "frame to be moved towards goal pose"); p.declare("target_pose", "goal pose for ik frame"); - p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); } void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link) diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 3ef54dd3..bdb740a7 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -51,25 +51,11 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) { auto& p = properties(); p.declare("eef", "name of end-effector"); - p.declare("pregrasp", "name of end-effector's pregrasp pose"); p.declare("object"); p.declare("angle_delta", 0.1, "angular steps (rad)"); -} -void GenerateGraspPose::setEndEffector(const std::string &eef) { - setProperty("eef", eef); -} - -void GenerateGraspPose::setNamedPose(const std::string &pose_name) { - setProperty("pregrasp", pose_name); -} - -void GenerateGraspPose::setObject(const std::string &object) { - setProperty("object", object); -} - -void GenerateGraspPose::setAngleDelta(double delta){ - setProperty("angle_delta", delta); + p.declare("pregrasp", "pregrasp posture"); + p.declare("grasp", "grasp posture"); } void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) @@ -84,6 +70,8 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model) if (props.get("angle_delta") == 0.) errors.push_back(*this, "angle_delta must be non-zero"); + // check availability of object + props.get("object"); // check availability of eef const std::string& eef = props.get("eef"); if (!robot_model->hasEndEffector(eef)) @@ -141,6 +129,7 @@ void GenerateGraspPose::compute() { InterfaceState state(scene); tf::poseEigenToMsg(target_pose, target_pose_msg.pose); state.properties().set("target_pose", target_pose_msg); + props.exposeTo(state.properties(), {"pregrasp", "grasp"}); SubTrajectory trajectory; trajectory.setCost(0.0); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 7d988fbe..de1c4394 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -51,30 +51,43 @@ namespace moveit { namespace task_constructor { namespace stages { SimpleGraspBase::SimpleGraspBase(const std::string& name) : SerialContainer(name) { - auto &props = properties(); - props.declare("pregrasp", "pregrasp posture"); - props.declare("grasp", "grasp posture"); + PropertyMap& p = properties(); + p.declare("eef", "end-effector to grasp with"); + p.declare("object", "object to grasp"); } -void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) +void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) { + // properties provided by the grasp generator via its Interface or its PropertyMap + const std::set& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" }; + int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order { - generator_ = generator.get(); - auto ik = new ComputeIK("compute ik", std::move(generator)); - const std::initializer_list& grasp_prop_names = { "eef", "pregrasp", "object" }; - ik->exposePropertiesOfChild(0, grasp_prop_names, true); - insert(std::unique_ptr(ik), insertion_position); + // forward properties from generator's to IK's solution (bottom -> up) + generator->setForwardedProperties(grasp_prop_names); + // allow inheritance in top -> down fashion as well + generator->properties().configureInitFrom(Stage::PARENT, { "object", "eef" }); - exposePropertiesOfChild(insertion_position, grasp_prop_names, true); - exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); + auto ik = new ComputeIK("compute ik", std::move(generator)); + ik->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties + + PropertyMap& p = ik->properties(); + p.declare("object"); + p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution + p.configureInitFrom(Stage::PARENT, {"max_ik_solutions", "timeout", "object"}); // derived from parent + p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, {"eef", "ik_frame"}); // derive from both + p.exposeTo(properties(), { "max_ik_solutions", "timeout", "ik_frame" }); + insert(std::unique_ptr(ik), insertion_position); +// exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision"); + allow_touch->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties + PropertyMap& p = allow_touch->properties(); p.declare("eef"); p.declare("object"); - p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" }); allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst(); @@ -91,21 +104,29 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bo pipeline->setPlannerId("RRTConnectkConfigDefault"); auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline); - PropertyMap& p = move->properties(); - p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){ + move->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties + + auto group_initializer = [this](const PropertyMap& parent_map) -> boost::any { const std::string& eef = parent_map.get("eef"); const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef); - return boost::any(jmg->getName()); - }); + return jmg->getName(); + }; + PropertyMap& p = move->properties(); + p.property("group").configureInitFrom(Stage::PARENT | Stage::INTERFACE, group_initializer); + p.property("goal").configureInitFrom(Stage::PARENT | Stage::INTERFACE, forward ? "grasp" : "pregrasp"); + p.exposeTo(properties(), { "group", "goal" }); insert(std::unique_ptr(move), insertion_position); - exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp"); +// exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp", true); } { auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object"); + attach->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties + PropertyMap& p = attach->properties(); p.declare("eef"); p.declare("object"); - p.configureInitFrom(Stage::PARENT, { "eef", "object" }); + p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" }); + attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){ const std::string& eef = p.get("eef"); moveit_msgs::AttachedCollisionObject obj; @@ -125,11 +146,6 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model) SerialContainer::init(robot_model); } -void SimpleGraspBase::setMonitoredStage(Stage* monitored) -{ - generator_->setMonitoredStage(monitored); -} - void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) { geometry_msgs::PoseStamped pose_msg; pose_msg.header.frame_id = link; @@ -137,13 +153,13 @@ void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& setIKFrame(pose_msg); } -SimpleGrasp::SimpleGrasp(std::unique_ptr&& generator, const std::string& name) +SimpleGrasp::SimpleGrasp(std::unique_ptr&& generator, const std::string& name) : SimpleGraspBase(name) { setup(std::move(generator), true); } -SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name) +SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr&& generator, const std::string& name) : SimpleGraspBase(name) { setup(std::move(generator), false); } From 2e4608252156b792713473fb84378d62badd09f0 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 3 Jun 2018 01:55:07 +0200 Subject: [PATCH 7/7] remove Container::exposePropertiesOfChild() --- .../moveit/task_constructor/container.h | 6 ---- core/src/container.cpp | 34 ------------------- core/src/stages/simple_grasp.cpp | 2 -- 3 files changed, 42 deletions(-) diff --git a/core/include/moveit/task_constructor/container.h b/core/include/moveit/task_constructor/container.h index 7b506e78..d8b46490 100644 --- a/core/include/moveit/task_constructor/container.h +++ b/core/include/moveit/task_constructor/container.h @@ -62,12 +62,6 @@ public: virtual bool remove(int pos); virtual void clear(); - /// declare all given variables of child and configure child to inherit them from parent - void exposePropertiesOfChild(int child, const std::initializer_list& names, bool skip_undefined = false); - /// declare given child property with another name and configure child to inherit it from parent - void exposePropertyOfChildAs(int child, const std::string& child_property_name, - const std::string& parent_property_name, bool skip_undefined = false); - void reset() override; void init(const moveit::core::RobotModelConstPtr& robot_model) override; /// validate connectivity of children (after init() was done) diff --git a/core/src/container.cpp b/core/src/container.cpp index c91cf6c1..b39e36ec 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -194,40 +194,6 @@ void ContainerBase::clear() pimpl()->children_.clear(); } -void ContainerBase::exposePropertiesOfChild(int child, const std::initializer_list& names, bool skip_undefined) -{ - auto impl = pimpl(); - ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false); - if (child_it == impl->children().end()) - throw std::runtime_error("invalid child index"); - - PropertyMap &source = (*child_it)->properties(); - PropertyMap &target = impl->properties_; - for (const std::string& name : names) { - if (skip_undefined && !source.hasProperty(name)) - continue; - // declare variables - source.exposeTo(target, name, name); - } -} - -void ContainerBase::exposePropertyOfChildAs(int child, const std::string& child_property_name, - const std::string& parent_property_name, bool skip_undefined) -{ - auto impl = pimpl(); - ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false); - if (child_it == impl->children().end()) - throw std::runtime_error("invalid child index"); - - PropertyMap &source = (*child_it)->properties(); - PropertyMap &target = impl->properties_; - if (skip_undefined && !source.hasProperty(child_property_name)) - return; - - // declare variables - source.exposeTo(target, child_property_name, parent_property_name); -} - void ContainerBase::reset() { auto impl = pimpl(); diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index de1c4394..315b166b 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -78,7 +78,6 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, {"eef", "ik_frame"}); // derive from both p.exposeTo(properties(), { "max_ik_solutions", "timeout", "ik_frame" }); insert(std::unique_ptr(ik), insertion_position); -// exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" }); } { auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision"); @@ -116,7 +115,6 @@ void SimpleGraspBase::setup(std::unique_ptr&& generator, bool forward) p.property("goal").configureInitFrom(Stage::PARENT | Stage::INTERFACE, forward ? "grasp" : "pregrasp"); p.exposeTo(properties(), { "group", "goal" }); insert(std::unique_ptr(move), insertion_position); -// exposePropertyOfChildAs(insertion_position, "goal", forward ? "grasp" : "pregrasp", true); } { auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object");