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..0e343559 --- /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().configureInitFrom(Stage::PARENT); + move->setTo("open"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("move to pre-grasp"); + 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(Stage::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().configureInitFrom(Stage::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().configureInitFrom(Stage::PARENT); + move->setTo("closed"); + move->graspObject("object"); + t.add(std::move(move)); + } + + { + auto move = std::make_unique("lift object"); + move->properties().configureInitFrom(Stage::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 959c5ccc..e961cf7c 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().configureInitFrom(Stage::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().configureInitFrom(Stage::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().configureInitFrom(Stage::PARENT); move->setMinMaxDistance(.03, 0.1); move->setCartesianStepSize(0.02); @@ -73,11 +76,10 @@ int main(int argc, char** argv){ { auto gengrasp = std::make_unique("generate grasp pose"); - gengrasp->setEndEffector("gripper"); - //gengrasp->setGroup("arm"); + gengrasp->properties().configureInitFrom(Stage::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)); @@ -85,7 +87,7 @@ int main(int argc, char** argv){ { auto move = std::make_unique("grasp"); - move->setEndEffector("gripper"); + move->properties().configureInitFrom(Stage::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().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 new file mode 100644 index 00000000..8555fd32 --- /dev/null +++ b/core/include/moveit/task_constructor/properties.h @@ -0,0 +1,181 @@ +/********************************************************************* + * 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 +#include +#include +#include +#include + +namespace moveit { +namespace task_constructor { + +class Property; +class PropertyMap; + +/// initializer function, using given name from the passed property map +boost::any fromName(const PropertyMap& other, const std::string& other_name); + +/** 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. + * 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. + * Using setCurrentValue() only updates the current value, allowing for later reset to the original default. + */ +class Property { + friend class PropertyMap; + +public: + typedef int SourceId; + typedef std::function InitializerFunction; + typedef std::map InitializerMap; + + 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); + + /// 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 + Property &configureInitFrom(SourceId 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 configured initializers + void performInitFrom(SourceId source, const PropertyMap& other); + +private: + std::string description_; + std::type_index type_index_; + boost::any default_; + boost::any value_; + InitializerMap initializers_; +}; + + +/** 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_; + + /// 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 = "") { + 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 - always using the same name + 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); + /// 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; + + /// 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); + if (value.empty()) + 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; + + /// reset all properties to their defaults + void reset(); + + /// perform initialization of still undefined properties using configured initializers + void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = 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 fef4bb10..67abd00b 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -109,6 +109,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); @@ -116,6 +117,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 @@ -127,6 +139,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; @@ -143,6 +156,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 7200f096..c6a5c428 100644 --- a/core/include/moveit/task_constructor/stage_p.h +++ b/core/include/moveit/task_constructor/stage_p.h @@ -79,7 +79,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; @@ -97,6 +97,7 @@ public: protected: Stage* const me_; // associated/owning Stage instance std::string name_; + PropertyMap properties_; InterfacePtr starts_; InterfacePtr ends_; @@ -157,6 +158,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_; @@ -184,6 +187,10 @@ public: bool canCompute() const override; bool compute() override; + +private: + // initialize properties from parent + void initProperties(); }; PIMPL_FUNCTIONS(Generator) @@ -204,6 +211,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/stages/cartesian_position_motion.h b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h index 50ddb1f0..d6e1094a 100644 --- a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h +++ b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h @@ -70,22 +70,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 08e2e908..88f6bf80 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -39,6 +39,7 @@ #pragma once #include +#include namespace moveit { namespace task_constructor { namespace stages { @@ -54,51 +55,33 @@ 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); void setMaxIKSolutions(uint32_t n); - void ignoreCollisions(bool flag); + 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_; - 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 58e7d6ab..7569155e 100644 --- a/core/include/moveit/task_constructor/stages/gripper.h +++ b/core/include/moveit/task_constructor/stages/gripper.h @@ -67,11 +67,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 e8205a71..3869ed78 100644 --- a/core/include/moveit/task_constructor/stages/move.h +++ b/core/include/moveit/task_constructor/stages/move.h @@ -40,10 +40,6 @@ #include -namespace moveit { -namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)} -} - namespace moveit { namespace task_constructor { namespace stages { class Move : public Connecting { @@ -53,17 +49,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/include/moveit/task_constructor/storage.h b/core/include/moveit/task_constructor/storage.h index 0a244be1..f269b9cc 100644 --- a/core/include/moveit/task_constructor/storage.h +++ b/core/include/moveit/task_constructor/storage.h @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include @@ -87,6 +88,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); } @@ -94,7 +98,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 0cbc3388..72cd0963 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -108,6 +108,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 bb177439..a6fd4b7e 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -2,6 +2,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/container.h ${PROJECT_INCLUDE}/container_p.h ${PROJECT_INCLUDE}/introspection.h + ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h ${PROJECT_INCLUDE}/storage.h @@ -10,10 +11,12 @@ add_library(${PROJECT_NAME} container.cpp introspection.cpp + properties.cpp stage.cpp storage.cpp task.cpp ) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} PUBLIC $ PUBLIC $ diff --git a/core/src/container.cpp b/core/src/container.cpp index 214a4a94..7d329616 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -163,10 +163,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/properties.cpp b/core/src/properties.cpp new file mode 100644 index 00000000..7e714d58 --- /dev/null +++ b/core/src/properties.cpp @@ -0,0 +1,190 @@ +/********************************************************************* + * 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 + +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) + , value_(default_value) +{ + // default value's type should match declared type by construction + assert(default_.empty() || std::type_index(default_.type()) == type_index_); +} + +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::logic_error(boost::str(fmt % value.type().name() % type_index.name())); + } +} +} + +void Property::setValue(const boost::any &value) { + setCurrentValue(value); + 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(SourceId source, const Property::InitializerFunction &f) +{ + initializers_[source] = f; + return *this; +} + +Property &Property::configureInitFrom(SourceId source, const std::string &name) +{ + initializers_[source] = [name](const PropertyMap& other) { return fromName(other, name); }; + return *this; +} + +void Property::performInitFrom(SourceId 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) +{ + 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::logic_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("Undeclared property '" + name + "'"); + return it->second; +} + +void PropertyMap::configureInitFrom(Property::SourceId source, const std::set &properties) +{ + for (auto &pair : props_) { + if (properties.empty() || properties.count(pair.first)) + pair.second.configureInitFrom(source, std::bind(&fromName, std::placeholders::_1, pair.first)); + } +} + +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()) + 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 { + assert(range.first->first == name); + range.first->second.setValue(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(); +} + +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_) + pair.second.reset(); +} + +void PropertyMap::performInitFrom(Property::SourceId source, const PropertyMap &other, bool enforce) +{ + for (auto& pair : props_) { + Property &p = pair.second; + if (enforce || !p.defined()) + p.performInitFrom(source, other); + } +} + + +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 6dc1e306..ad1adbf2 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -91,9 +91,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"); @@ -140,6 +142,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_; } @@ -161,6 +167,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(); @@ -263,6 +278,16 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){ return state; } +void PropagatingEitherWayPrivate::initProperties(const InterfaceState& state) +{ + // reset properties to their defaults + properties_.reset(); + // first init from INTERFACE + properties_.performInitFrom(Stage::INTERFACE, state.properties()); + // then init from PARENT + properties_.performInitFrom(Stage::PARENT, parent()->properties()); +} + bool PropagatingEitherWayPrivate::canCompute() const { if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) @@ -278,11 +303,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; @@ -441,9 +470,18 @@ bool GeneratorPrivate::canCompute() const { } bool GeneratorPrivate::compute() { + initProperties(); return static_cast(me_)->compute(); } +void GeneratorPrivate::initProperties() +{ + // reset properties to their defaults + properties_.reset(); + // then init from PARENT + properties_.performInitFrom(Stage::PARENT, parent()->properties()); +} + Generator::Generator(const std::string &name) : ComputeBase(new GeneratorPrivate(this, name)) @@ -487,6 +525,14 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it) --it_pairs_.second; } +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()); +} + bool ConnectingPrivate::canCompute() const{ // TODO: implement this properly return it_pairs_.first != starts_->end() && @@ -497,6 +543,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/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp index e9739887..b340779a 100644 --- a/core/src/stages/cartesian_position_motion.cpp +++ b/core/src/stages/cartesian_position_motion.cpp @@ -49,44 +49,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 { @@ -104,8 +111,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( @@ -119,12 +130,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 @@ -137,7 +148,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); @@ -145,12 +156,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( @@ -159,14 +171,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"); @@ -187,8 +199,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( @@ -203,14 +219,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; @@ -226,14 +243,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 ff2b5943..9f139612 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -57,6 +57,16 @@ 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("object"); + p.declare("eef_grasp_pose"); + p.declare("timeout", 0.1); + p.declare("max_ik_solutions", 1); + 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); } void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) @@ -66,40 +76,47 @@ void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) } void GenerateGraspPose::setGroup(std::string group){ - group_= group; -} - -void GenerateGraspPose::setLink(std::string ik_link){ - ik_link_= ik_link; + setProperty("group", group); } void GenerateGraspPose::setEndEffector(std::string eef){ - eef_= eef; + setProperty("eef", eef); } void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ - gripper_grasp_pose_= pose_name; + setProperty("eef_grasp_pose", pose_name); } void GenerateGraspPose::setObject(std::string object){ - object_= object; + setProperty("object", object); } -void GenerateGraspPose::setGraspOffset(double offset){ - 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){ - 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{ @@ -134,66 +151,85 @@ 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_; + 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); - if(!gripper_grasp_pose_.empty()){ - grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_); + 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= 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_); - 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); - + 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; } - 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, 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, goal_pose, link_name, 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 665f2ac9..5b0f731e 100644 --- a/core/src/stages/gripper.cpp +++ b/core/src/stages/gripper.cpp @@ -51,7 +51,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) { @@ -60,25 +66,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, @@ -86,24 +92,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; @@ -116,10 +128,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 3251ce08..56c85a1c 100644 --- a/core/src/stages/move.cpp +++ b/core/src/stages/move.cpp @@ -50,9 +50,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) { @@ -60,27 +64,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; diff --git a/core/src/storage.cpp b/core/src/storage.cpp index 91ba6a1c..e646f85b 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -50,7 +50,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 850823d9..079cb1c9 100644 --- a/core/src/task.cpp +++ b/core/src/task.cpp @@ -252,6 +252,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_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)); diff --git a/core/test/test_properties.cpp b/core/test/test_properties.cpp new file mode 100644 index 00000000..9f885a2e --- /dev/null +++ b/core/test/test_properties.cpp @@ -0,0 +1,127 @@ +#include + +#include +#include + +using namespace moveit::task_constructor; + +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"); + + // avoid second declaration with different type + props.declare("double1"); + EXPECT_THROW(props.declare("double1"), std::logic_error); + + // types not matching? + EXPECT_THROW(props.set("double1", 1), std::logic_error); + + props.set("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); +}