Merge branch 'wip-properties'

This commit is contained in:
Robert Haschke 2018-02-04 08:59:35 +01:00
commit e9363919e8
26 changed files with 931 additions and 177 deletions

View File

@ -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)

View File

@ -0,0 +1,114 @@
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/gripper.h>
#include <moveit/task_constructor/stages/move.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
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<stages::CurrentState>("current state"));
{
auto move = std::make_unique<stages::Gripper>("open gripper");
move->properties().configureInitFrom(Stage::PARENT);
move->setTo("open");
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::Move>("move to pre-grasp");
move->properties().configureInitFrom(Stage::PARENT);
move->setTimeout(8.0);
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::CartesianPositionMotion>("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<stages::GenerateGraspPose>("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<stages::Gripper>("grasp");
move->properties().configureInitFrom(Stage::PARENT);
move->setTo("closed");
move->graspObject("object");
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::CartesianPositionMotion>("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;
}

View File

@ -77,10 +77,9 @@ int main(int argc, char** argv){
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("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));
}

View File

@ -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<stages::CurrentState>("current state"));
{
auto move = std::make_unique<stages::Gripper>("open gripper");
move->setEndEffector("gripper");
move->properties().configureInitFrom(Stage::PARENT);
move->setTo("open");
t.add(std::move(move));
}
{
auto move = std::make_unique<stages::Move>("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<stages::CartesianPositionMotion>("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<stages::GenerateGraspPose>("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<stages::Gripper>("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<stages::CartesianPositionMotion>("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);

View File

@ -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 <boost/any.hpp>
#include <typeindex>
#include <map>
#include <set>
#include <functional>
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<boost::any(const PropertyMap& other)> InitializerFunction;
typedef std::map<SourceId, InitializerFunction> 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<std::string, Property> 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<typename T>
Property& declare(const std::string& name, const std::string& description = "") {
return declare(name, typeid(T), description);
}
/// declare a property with default value
template<typename T>
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<PropertyMap*>(this)->property(name);
}
/// allow initialization from given source for listed properties - always using the same name
void configureInitFrom(Property::SourceId source, const std::set<std::string> &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<typename T>
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<const T&>(value);
}
/// get typed value of property, using fallback if undefined. Throws bad_any_cast on type mismatch.
template<typename T>
const T& get(const std::string& name, const T& fallback) const {
const boost::any& value = get(name);
return (value.empty()) ? fallback : boost::any_cast<const T&>(value);
}
/// count number of defined properties from given list
size_t countDefined(const std::vector<std::string>& 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

View File

@ -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<Stage> 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<Stage*>(this)->properties();
}
void setProperty(const std::string& name, const boost::any& value);
protected:
/// Stage can only be instantiated through derived classes
Stage(StagePrivate *impl);

View File

@ -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<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::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<InterfaceState>::iterator& it);
void newEndState(const std::list<InterfaceState>::iterator& it);
// initialize properties from parent and/or interface states
void initProperties(const InterfaceState &start, const InterfaceState &end);
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
};

View File

@ -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_;
};
} } }

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/stage.h>
#include <Eigen/Geometry>
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 <typename T>
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<double> > previous_solutions_;

View File

@ -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_;
};

View File

@ -40,10 +40,6 @@
#include <moveit/task_constructor/stage.h>
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_;
};
} } }

View File

@ -40,6 +40,7 @@
#pragma once
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -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<std::string, std::any> to allow passing of parameters or attributes
PropertyMap properties_;
Solutions incoming_trajectories_;
Solutions outgoing_trajectories_;
};

View File

@ -108,6 +108,13 @@ public:
ContainerBase *stages();
const ContainerBase *stages() const;
/// properties access
PropertyMap& properties();
const PropertyMap& properties() const {
return const_cast<Task*>(this)->properties();
}
void setProperty(const std::string& name, const boost::any& value);
protected:
void initModel();
void initScene();

View File

@ -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 $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>

View File

@ -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");

190
core/src/properties.cpp Normal file
View File

@ -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 <moveit/task_constructor/properties.h>
#include <boost/format.hpp>
#include <functional>
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<std::string> &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<std::string> &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

View File

@ -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<InterfaceFlag own, InterfaceFlag other>
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<Generator*>(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<Connecting*>(me_)->compute(from, to);
}

View File

@ -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<std::string>("group", "name of planning group");
p.declare<std::string>("link", "name of link used for IK");
p.declare<double>("min_distance", "minimum distance to move");
p.declare<double>("max_distance", "maximum distance to move");
p.declare<double>("step_size", 0.005);
p.declare<geometry_msgs::PointStamped>("towards", "target point of motion");
p.declare<geometry_msgs::Vector3Stamped>("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<std::string>("group");
const std::string& link = props.get<std::string>("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<geometry_msgs::PointStamped>("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<double>("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<double>("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<geometry_msgs::Vector3Stamped>("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<double>("max_distance"),
props.get<double>("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<double>("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<std::string>("group");
const std::string& link = props.get<std::string>("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<geometry_msgs::Vector3Stamped>("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<double>("max_distance"),
props.get<double>("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<double>("min_distance");
if(succeeded){
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);

View File

@ -57,6 +57,16 @@ namespace moveit { namespace task_constructor { namespace stages {
GenerateGraspPose::GenerateGraspPose(std::string name)
: Generator(name)
{
auto& p = properties();
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("object");
p.declare<std::string>("eef_grasp_pose");
p.declare<double>("timeout", 0.1);
p.declare<uint32_t>("max_ik_solutions", 1);
p.declare<geometry_msgs::TransformStamped>("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping");
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
p.declare<bool>("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<double>("timeout");
const std::string& eef = props.get<std::string>("eef");
const std::string& group = props.get<std::string>("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<geometry_msgs::TransformStamped>("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<std::string>("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<bool>("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<std::string>("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<uint32_t>("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<double>("angle_delta");
remaining_time = props.get<double>("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<double>(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<double>(std::chrono::steady_clock::now()- now).count();
if(succeeded) {
previous_solutions_.emplace_back();

View File

@ -51,7 +51,13 @@ namespace moveit { namespace task_constructor { namespace stages {
Gripper::Gripper(std::string name)
: PropagatingEitherWay(name)
{}
{
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector group");
p.declare<std::string>("link", "name of link the eef is attached to");
p.declare<std::string>("named_target", "named target in eef group");
p.declare<std::string>("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<std::string>("eef");
std::string link = props.get<std::string>("link", "");
const std::string& named_target = props.get<std::string>("named_target");
const std::string& grasp_object = props.get<std::string>("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<moveit::planning_interface::MoveGroupInterface>(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);
}

View File

@ -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<double>("timeout", 5.0, "planning timeout");
p.declare<std::string>("group", "name of planning group");
p.declare<std::string>("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<moveit::planning_interface::MoveGroupInterface>(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<std::string>("group"));
mgi.setJointValueTarget(to.scene()->getCurrentState());
const std::string planner_id = props.get<std::string>("planner");
if( !planner_id.empty() )
mgi.setPlannerId(planner_id);
mgi.setPlanningTime(props.get<double>("timeout"));
::planning_interface::MotionPlanRequest req;
mgi_->constructMotionPlanRequest(req);
mgi.constructMotionPlanRequest(req);
ros::Duration(4.0).sleep();
::planning_interface::MotionPlanResponse res;

View File

@ -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_)
{
}

View File

@ -252,6 +252,18 @@ const ContainerBase* Task::stages() const
return const_cast<Task*>(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_;

View File

@ -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)

View File

@ -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));

View File

@ -0,0 +1,127 @@
#include <moveit/task_constructor/properties.h>
#include <gtest/gtest.h>
#include <initializer_list>
using namespace moveit::task_constructor;
TEST(Property, standard) {
PropertyMap props;
props.declare<double>("double1", 1, "first");
props.declare<double>("double2", 2);
props.declare<double>("double4");
EXPECT_EQ(props.get<double>("double1"), 1.0);
EXPECT_EQ(props.get<double>("double2"), 2.0);
EXPECT_THROW(props.get<double>("double3"), std::runtime_error);
EXPECT_THROW(props.get<double>("double4"), std::runtime_error);
EXPECT_FALSE(props.property("double4").defined());
EXPECT_EQ(props.get<double>("double4", 0.0), 0.0);
props.set("double3", 3.0);
EXPECT_EQ(props.get<double>("double3"), 3.0);
}
TEST(Property, redeclare) {
PropertyMap props;
props.declare<double>("double1");
// avoid second declaration with different type
props.declare<double>("double1");
EXPECT_THROW(props.declare<long double>("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<double>("double1"), 3.14);
}
TEST(Property, reset) {
PropertyMap props;
props.declare<double>("double1");
// setCurrent() only assigns temporary values
props.setCurrent("double1", 1.0);
ASSERT_EQ(props.get<double>("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<double>("double1"), 1.0);
EXPECT_EQ(boost::any_cast<double>(props.property("double1").defaultValue()), 1.0);
props.setCurrent("double1", 2.0);
EXPECT_EQ(props.get<double>("double1"), 2.0);
EXPECT_EQ(boost::any_cast<double>(props.property("double1").defaultValue()), 1.0);
// back to default
props.reset();
EXPECT_EQ(props.get<double>("double1"), 1.0);
}
class InitFromTest : public ::testing::Test {
protected:
void SetUp() {
master.declare<double>("double1", 1);
master.declare<double>("double2", 2);
master.declare<double>("double4", 4);
slave.declare<double>("double1");
slave.declare<double>("double2");
slave.declare<double>("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<double>("double1"), 1.0);
EXPECT_EQ(slave.get<double>("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<double>("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<double>("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<double>("double1") + other.get<double>("double2");
});
slave.performInitFrom(0, master);
EXPECT_EQ(slave.get<double>("double3"), 3.0);
}