mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branch 'wip-properties'
This commit is contained in:
commit
e9363919e8
@ -3,3 +3,6 @@ target_link_libraries(plan_pick_ur5 ${PROJECT_NAME}_stages)
|
|||||||
|
|
||||||
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
|
add_executable(plan_pick_trixi plan_pick_trixi.cpp)
|
||||||
target_link_libraries(plan_pick_trixi ${PROJECT_NAME}_stages)
|
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)
|
||||||
|
|||||||
114
core/demo/plan_pick_pa10.cpp
Normal file
114
core/demo/plan_pick_pa10.cpp
Normal 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;
|
||||||
|
}
|
||||||
@ -77,10 +77,9 @@ int main(int argc, char** argv){
|
|||||||
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
auto gengrasp= std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
gengrasp->setEndEffector("left_gripper");
|
gengrasp->setEndEffector("left_gripper");
|
||||||
//gengrasp->setGroup("arm");
|
//gengrasp->setGroup("arm");
|
||||||
gengrasp->setLink("l_gripper_tool_frame");
|
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspOffset(.00);
|
gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame");
|
||||||
gengrasp->setAngleDelta(.2);
|
gengrasp->setAngleDelta(.2);
|
||||||
t.add(std::move(gengrasp));
|
t.add(std::move(gengrasp));
|
||||||
}
|
}
|
||||||
|
|||||||
@ -39,29 +39,32 @@ int main(int argc, char** argv){
|
|||||||
spawnObject();
|
spawnObject();
|
||||||
|
|
||||||
Task t;
|
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"));
|
t.add(std::make_unique<stages::CurrentState>("current state"));
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||||
move->setEndEffector("gripper");
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->setTo("open");
|
move->setTo("open");
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
||||||
move->setGroup("arm");
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->setPlannerId("RRTConnectkConfigDefault");
|
|
||||||
move->setTimeout(8.0);
|
move->setTimeout(8.0);
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||||
move->addSolutionCallback(std::ref(t.introspection()));
|
// move->addSolutionCallback(std::ref(t.introspection()));
|
||||||
move->setGroup("arm");
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->setLink("s_model_tool0");
|
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
move->setMinMaxDistance(.03, 0.1);
|
||||||
move->setCartesianStepSize(0.02);
|
move->setCartesianStepSize(0.02);
|
||||||
|
|
||||||
@ -73,11 +76,10 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
{
|
{
|
||||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
||||||
gengrasp->setEndEffector("gripper");
|
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||||
//gengrasp->setGroup("arm");
|
|
||||||
gengrasp->setGripperGraspPose("open");
|
gengrasp->setGripperGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setGraspOffset(.03);
|
gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(-.2);
|
||||||
gengrasp->setMaxIKSolutions(8);
|
gengrasp->setMaxIKSolutions(8);
|
||||||
t.add(std::move(gengrasp));
|
t.add(std::move(gengrasp));
|
||||||
@ -85,7 +87,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::Gripper>("grasp");
|
auto move = std::make_unique<stages::Gripper>("grasp");
|
||||||
move->setEndEffector("gripper");
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->setTo("closed");
|
move->setTo("closed");
|
||||||
move->graspObject("object");
|
move->graspObject("object");
|
||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
@ -93,8 +95,7 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
{
|
{
|
||||||
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
|
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
|
||||||
move->setGroup("arm");
|
move->properties().configureInitFrom(Stage::PARENT);
|
||||||
move->setLink("s_model_tool0");
|
|
||||||
move->setMinMaxDistance(0.03, 0.05);
|
move->setMinMaxDistance(0.03, 0.05);
|
||||||
move->setCartesianStepSize(0.01);
|
move->setCartesianStepSize(0.01);
|
||||||
|
|
||||||
|
|||||||
181
core/include/moveit/task_constructor/properties.h
Normal file
181
core/include/moveit/task_constructor/properties.h
Normal 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
|
||||||
@ -109,6 +109,7 @@ private:
|
|||||||
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
|
std::ostream& operator<<(std::ostream &os, const InitStageException& e);
|
||||||
|
|
||||||
|
|
||||||
|
class ContainerBase;
|
||||||
class StagePrivate;
|
class StagePrivate;
|
||||||
class Stage {
|
class Stage {
|
||||||
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
friend std::ostream& operator<<(std::ostream &os, const Stage& stage);
|
||||||
@ -116,6 +117,17 @@ class Stage {
|
|||||||
public:
|
public:
|
||||||
PRIVATE_CLASS(Stage)
|
PRIVATE_CLASS(Stage)
|
||||||
typedef std::unique_ptr<Stage> pointer;
|
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();
|
virtual ~Stage();
|
||||||
|
|
||||||
/// auto-convert Stage to StagePrivate* when needed
|
/// auto-convert Stage to StagePrivate* when needed
|
||||||
@ -127,6 +139,7 @@ public:
|
|||||||
/// initialize stage once before planning
|
/// initialize stage once before planning
|
||||||
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
|
virtual void init(const planning_scene::PlanningSceneConstPtr& scene);
|
||||||
|
|
||||||
|
const ContainerBase* parent() const;
|
||||||
const std::string& name() const;
|
const std::string& name() const;
|
||||||
void setName(const std::string& name);
|
void setName(const std::string& name);
|
||||||
virtual size_t numSolutions() const = 0;
|
virtual size_t numSolutions() const = 0;
|
||||||
@ -143,6 +156,12 @@ public:
|
|||||||
/// remove function callback
|
/// remove function callback
|
||||||
void erase(SolutionCallbackList::const_iterator which);
|
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:
|
protected:
|
||||||
/// Stage can only be instantiated through derived classes
|
/// Stage can only be instantiated through derived classes
|
||||||
Stage(StagePrivate *impl);
|
Stage(StagePrivate *impl);
|
||||||
|
|||||||
@ -79,7 +79,7 @@ public:
|
|||||||
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
|
||||||
inline InterfaceConstPtr nextStarts() const { return next_starts_.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
|
/// should be only called by containers' init() method
|
||||||
void validate() const;
|
void validate() const;
|
||||||
|
|
||||||
@ -97,6 +97,7 @@ public:
|
|||||||
protected:
|
protected:
|
||||||
Stage* const me_; // associated/owning Stage instance
|
Stage* const me_; // associated/owning Stage instance
|
||||||
std::string name_;
|
std::string name_;
|
||||||
|
PropertyMap properties_;
|
||||||
|
|
||||||
InterfacePtr starts_;
|
InterfacePtr starts_;
|
||||||
InterfacePtr ends_;
|
InterfacePtr ends_;
|
||||||
@ -157,6 +158,8 @@ protected:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_start_state_;
|
||||||
Interface::const_iterator next_end_state_;
|
Interface::const_iterator next_end_state_;
|
||||||
@ -184,6 +187,10 @@ public:
|
|||||||
|
|
||||||
bool canCompute() const override;
|
bool canCompute() const override;
|
||||||
bool compute() override;
|
bool compute() override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// initialize properties from parent
|
||||||
|
void initProperties();
|
||||||
};
|
};
|
||||||
PIMPL_FUNCTIONS(Generator)
|
PIMPL_FUNCTIONS(Generator)
|
||||||
|
|
||||||
@ -204,6 +211,8 @@ private:
|
|||||||
// get informed when new start or end state becomes available
|
// get informed when new start or end state becomes available
|
||||||
void newStartState(const std::list<InterfaceState>::iterator& it);
|
void newStartState(const std::list<InterfaceState>::iterator& it);
|
||||||
void newEndState(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_;
|
std::pair<Interface::const_iterator, Interface::const_iterator> it_pairs_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -70,22 +70,10 @@ public:
|
|||||||
void setCartesianStepSize(double distance);
|
void setCartesianStepSize(double distance);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string group_;
|
|
||||||
|
|
||||||
std::string link_;
|
|
||||||
|
|
||||||
double min_distance_;
|
|
||||||
double max_distance_;
|
|
||||||
|
|
||||||
enum {
|
enum {
|
||||||
MODE_ALONG= 1,
|
MODE_ALONG= 1,
|
||||||
MODE_TOWARDS= 2
|
MODE_TOWARDS= 2
|
||||||
} mode_;
|
} mode_;
|
||||||
|
|
||||||
geometry_msgs::PointStamped towards_;
|
|
||||||
geometry_msgs::Vector3Stamped along_;
|
|
||||||
|
|
||||||
double step_size_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -39,6 +39,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
#include <moveit/task_constructor/stage.h>
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
@ -54,51 +55,33 @@ public:
|
|||||||
|
|
||||||
void setGroup(std::string group_name);
|
void setGroup(std::string group_name);
|
||||||
|
|
||||||
void setLink(std::string ik_link);
|
|
||||||
|
|
||||||
void setGripperGraspPose(std::string pose_name);
|
void setGripperGraspPose(std::string pose_name);
|
||||||
|
|
||||||
void setObject(std::string object);
|
void setObject(std::string object);
|
||||||
|
|
||||||
void setGraspOffset(double grasp_offset);
|
|
||||||
|
|
||||||
void setTimeout(double timeout);
|
void setTimeout(double timeout);
|
||||||
|
|
||||||
void setAngleDelta(double delta);
|
void setAngleDelta(double delta);
|
||||||
|
|
||||||
void setMaxIKSolutions(uint32_t n);
|
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:
|
protected:
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
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 */
|
/* temp values */
|
||||||
|
|
||||||
double current_angle_ = 0.0;
|
double current_angle_ = 0.0;
|
||||||
|
|
||||||
double remaining_time_;
|
|
||||||
|
|
||||||
bool tried_current_state_as_seed_ = false;
|
bool tried_current_state_as_seed_ = false;
|
||||||
|
|
||||||
std::vector< std::vector<double> > previous_solutions_;
|
std::vector< std::vector<double> > previous_solutions_;
|
||||||
|
|||||||
@ -67,11 +67,6 @@ protected:
|
|||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string eef_;
|
|
||||||
std::string named_target_;
|
|
||||||
std::string grasp_object_;
|
|
||||||
std::string attach_link_;
|
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -40,10 +40,6 @@
|
|||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
#include <moveit/task_constructor/stage.h>
|
||||||
|
|
||||||
namespace moveit {
|
|
||||||
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface)}
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class Move : public Connecting {
|
class Move : public Connecting {
|
||||||
@ -53,17 +49,12 @@ public:
|
|||||||
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
void init(const planning_scene::PlanningSceneConstPtr &scene);
|
||||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||||
|
|
||||||
void setGroup(std::string group);
|
void setGroup(const std::__cxx11::string &group);
|
||||||
void setPlannerId(std::string planner);
|
void setPlannerId(const std::__cxx11::string &planner);
|
||||||
void setTimeout(double timeout);
|
void setTimeout(double timeout);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
std::string group_;
|
|
||||||
std::string planner_id_;
|
|
||||||
double timeout_;
|
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -40,6 +40,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
#include <moveit/task_constructor/properties.h>
|
||||||
#include <moveit_task_constructor_msgs/Solution.h>
|
#include <moveit_task_constructor_msgs/Solution.h>
|
||||||
#include <visualization_msgs/MarkerArray.h>
|
#include <visualization_msgs/MarkerArray.h>
|
||||||
|
|
||||||
@ -87,6 +88,9 @@ public:
|
|||||||
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
|
||||||
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
inline const Solutions& outgoingTrajectories() const { return outgoing_trajectories_; }
|
||||||
|
|
||||||
|
PropertyMap& properties() { return properties_; }
|
||||||
|
const PropertyMap& properties() const { return properties_; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// these methods should be only called by SolutionBase::set[Start|End]State()
|
// these methods should be only called by SolutionBase::set[Start|End]State()
|
||||||
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
|
||||||
@ -94,7 +98,7 @@ private:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
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 incoming_trajectories_;
|
||||||
Solutions outgoing_trajectories_;
|
Solutions outgoing_trajectories_;
|
||||||
};
|
};
|
||||||
|
|||||||
@ -108,6 +108,13 @@ public:
|
|||||||
ContainerBase *stages();
|
ContainerBase *stages();
|
||||||
const ContainerBase *stages() const;
|
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:
|
protected:
|
||||||
void initModel();
|
void initModel();
|
||||||
void initScene();
|
void initScene();
|
||||||
|
|||||||
@ -2,6 +2,7 @@ add_library(${PROJECT_NAME}
|
|||||||
${PROJECT_INCLUDE}/container.h
|
${PROJECT_INCLUDE}/container.h
|
||||||
${PROJECT_INCLUDE}/container_p.h
|
${PROJECT_INCLUDE}/container_p.h
|
||||||
${PROJECT_INCLUDE}/introspection.h
|
${PROJECT_INCLUDE}/introspection.h
|
||||||
|
${PROJECT_INCLUDE}/properties.h
|
||||||
${PROJECT_INCLUDE}/stage.h
|
${PROJECT_INCLUDE}/stage.h
|
||||||
${PROJECT_INCLUDE}/stage_p.h
|
${PROJECT_INCLUDE}/stage_p.h
|
||||||
${PROJECT_INCLUDE}/storage.h
|
${PROJECT_INCLUDE}/storage.h
|
||||||
@ -10,10 +11,12 @@ add_library(${PROJECT_NAME}
|
|||||||
|
|
||||||
container.cpp
|
container.cpp
|
||||||
introspection.cpp
|
introspection.cpp
|
||||||
|
properties.cpp
|
||||||
stage.cpp
|
stage.cpp
|
||||||
storage.cpp
|
storage.cpp
|
||||||
task.cpp
|
task.cpp
|
||||||
)
|
)
|
||||||
|
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
target_include_directories(${PROJECT_NAME}
|
target_include_directories(${PROJECT_NAME}
|
||||||
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
|
||||||
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
|
||||||
|
|||||||
@ -163,10 +163,16 @@ void ContainerBase::reset()
|
|||||||
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
void ContainerBase::init(const planning_scene::PlanningSceneConstPtr &scene)
|
||||||
{
|
{
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
auto& children = pimpl()->children();
|
auto impl = pimpl();
|
||||||
|
auto& children = impl->children();
|
||||||
|
|
||||||
Stage::init(scene);
|
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
|
// we need to have some children to do the actual work
|
||||||
if (children.empty()) {
|
if (children.empty()) {
|
||||||
errors.push_back(*this, "no children");
|
errors.push_back(*this, "no children");
|
||||||
|
|||||||
190
core/src/properties.cpp
Normal file
190
core/src/properties.cpp
Normal 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
|
||||||
@ -91,9 +91,11 @@ void StagePrivate::validate() const {
|
|||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
|
|
||||||
InterfaceFlags f = interfaceFlags();
|
InterfaceFlags f = interfaceFlags();
|
||||||
|
// validate that sendForward() will succeed
|
||||||
if (!implies(f & WRITES_NEXT_START, bool(nextStarts())))
|
if (!implies(f & WRITES_NEXT_START, bool(nextStarts())))
|
||||||
errors.push_back(*me_, "sends forward, but next stage cannot receive");
|
errors.push_back(*me_, "sends forward, but next stage cannot receive");
|
||||||
|
|
||||||
|
// validate that sendBackward() will succeed
|
||||||
if (!implies(f & WRITES_PREV_END, bool(prevEnds())))
|
if (!implies(f & WRITES_PREV_END, bool(prevEnds())))
|
||||||
errors.push_back(*me_, "sends backward, but previous stage cannot receive");
|
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 {
|
const std::string& Stage::name() const {
|
||||||
return pimpl_->name_;
|
return pimpl_->name_;
|
||||||
}
|
}
|
||||||
@ -161,6 +167,15 @@ void Stage::erase(SolutionCallbackList::const_iterator which)
|
|||||||
pimpl()->solution_cbs_.erase(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>
|
template<InterfaceFlag own, InterfaceFlag other>
|
||||||
const char* direction(const StagePrivate& stage) {
|
const char* direction(const StagePrivate& stage) {
|
||||||
InterfaceFlags f = stage.interfaceFlags();
|
InterfaceFlags f = stage.interfaceFlags();
|
||||||
@ -263,6 +278,16 @@ const InterfaceState& PropagatingEitherWayPrivate::fetchEndState(){
|
|||||||
return state;
|
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
|
bool PropagatingEitherWayPrivate::canCompute() const
|
||||||
{
|
{
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState())
|
||||||
@ -278,11 +303,15 @@ bool PropagatingEitherWayPrivate::compute()
|
|||||||
|
|
||||||
bool result = false;
|
bool result = false;
|
||||||
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
if ((dir & PropagatingEitherWay::FORWARD) && hasStartState()) {
|
||||||
if (me->computeForward(fetchStartState()))
|
const InterfaceState& state = fetchStartState();
|
||||||
|
initProperties(state);
|
||||||
|
if (me->computeForward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
if ((dir & PropagatingEitherWay::BACKWARD) && hasEndState()) {
|
||||||
if (me->computeBackward(fetchEndState()))
|
const InterfaceState& state = fetchEndState();
|
||||||
|
initProperties(state);
|
||||||
|
if (me->computeBackward(state))
|
||||||
result |= true;
|
result |= true;
|
||||||
}
|
}
|
||||||
return result;
|
return result;
|
||||||
@ -441,9 +470,18 @@ bool GeneratorPrivate::canCompute() const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GeneratorPrivate::compute() {
|
bool GeneratorPrivate::compute() {
|
||||||
|
initProperties();
|
||||||
return static_cast<Generator*>(me_)->compute();
|
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)
|
Generator::Generator(const std::string &name)
|
||||||
: ComputeBase(new GeneratorPrivate(this, name))
|
: ComputeBase(new GeneratorPrivate(this, name))
|
||||||
@ -487,6 +525,14 @@ void ConnectingPrivate::newEndState(const Interface::iterator& it)
|
|||||||
--it_pairs_.second;
|
--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{
|
bool ConnectingPrivate::canCompute() const{
|
||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
return it_pairs_.first != starts_->end() &&
|
return it_pairs_.first != starts_->end() &&
|
||||||
@ -497,6 +543,7 @@ bool ConnectingPrivate::compute() {
|
|||||||
// TODO: implement this properly
|
// TODO: implement this properly
|
||||||
const InterfaceState& from = *it_pairs_.first;
|
const InterfaceState& from = *it_pairs_.first;
|
||||||
const InterfaceState& to = *(it_pairs_.second++);
|
const InterfaceState& to = *(it_pairs_.second++);
|
||||||
|
initProperties(from, to);
|
||||||
return static_cast<Connecting*>(me_)->compute(from, to);
|
return static_cast<Connecting*>(me_)->compute(from, to);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -49,44 +49,51 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
||||||
: PropagatingEitherWay(name),
|
: PropagatingEitherWay(name)
|
||||||
step_size_(0.005)
|
|
||||||
{
|
{
|
||||||
|
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){
|
void CartesianPositionMotion::setGroup(std::string group){
|
||||||
group_= group;
|
setProperty("group", group);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::setLink(std::string link){
|
void CartesianPositionMotion::setLink(std::string link){
|
||||||
link_= link;
|
setProperty("link", link);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::setMinDistance(double distance){
|
void CartesianPositionMotion::setMinDistance(double distance){
|
||||||
min_distance_= distance;
|
setProperty("min_distance", distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::setMaxDistance(double distance){
|
void CartesianPositionMotion::setMaxDistance(double distance){
|
||||||
max_distance_= distance;
|
setProperty("max_distance", distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
||||||
setMinDistance(min_distance);
|
setProperty("min_distance", min_distance);
|
||||||
setMaxDistance(max_distance);
|
setProperty("max_distance", max_distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
||||||
mode_= CartesianPositionMotion::MODE_TOWARDS;
|
mode_= CartesianPositionMotion::MODE_TOWARDS;
|
||||||
towards_= towards;
|
setProperty("towards", towards);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
||||||
mode_= CartesianPositionMotion::MODE_ALONG;
|
mode_= CartesianPositionMotion::MODE_ALONG;
|
||||||
along_= along;
|
setProperty("along", along);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CartesianPositionMotion::setCartesianStepSize(double distance){
|
void CartesianPositionMotion::setCartesianStepSize(double distance){
|
||||||
step_size_= distance;
|
setProperty("step_size", distance);
|
||||||
}
|
}
|
||||||
|
|
||||||
namespace {
|
namespace {
|
||||||
@ -104,8 +111,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
|
planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
const auto& props = properties();
|
||||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
|
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=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
std::bind(
|
std::bind(
|
||||||
@ -119,12 +130,12 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
bool succeeded= false;
|
bool succeeded= false;
|
||||||
|
|
||||||
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
||||||
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards_.header.frame_id);
|
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_);
|
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
|
||||||
|
|
||||||
Eigen::Vector3d target_point;
|
Eigen::Vector3d target_point;
|
||||||
tf::pointMsgToEigen(towards_.point, target_point);
|
tf::pointMsgToEigen(towards.point, target_point);
|
||||||
target_point= frame*target_point;
|
target_point= frame*target_point;
|
||||||
|
|
||||||
// retain orientation of link
|
// retain orientation of link
|
||||||
@ -137,7 +148,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
link_model,
|
link_model,
|
||||||
target,
|
target,
|
||||||
true, /* global frame */
|
true, /* global frame */
|
||||||
step_size_, /* cartesian step size */
|
props.get<double>("step_size"), /* cartesian step size */
|
||||||
1.5, /* jump threshold */
|
1.5, /* jump threshold */
|
||||||
is_valid);
|
is_valid);
|
||||||
|
|
||||||
@ -145,12 +156,13 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
|
|
||||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
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 ){
|
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;
|
Eigen::Vector3d direction;
|
||||||
tf::vectorMsgToEigen(along_.vector, direction);
|
tf::vectorMsgToEigen(along.vector, direction);
|
||||||
direction= frame.linear()*direction;
|
direction= frame.linear()*direction;
|
||||||
|
|
||||||
double achieved_distance= robot_state.computeCartesianPath(
|
double achieved_distance= robot_state.computeCartesianPath(
|
||||||
@ -159,14 +171,14 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
link_model,
|
link_model,
|
||||||
direction,
|
direction,
|
||||||
true, /* global frame */
|
true, /* global frame */
|
||||||
max_distance_, /* distance */
|
props.get<double>("max_distance"),
|
||||||
step_size_, /* cartesian step size */
|
props.get<double>("step_size"), /* cartesian step size */
|
||||||
1.5, /* jump threshold */
|
1.5, /* jump threshold */
|
||||||
is_valid);
|
is_valid);
|
||||||
|
|
||||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
||||||
|
|
||||||
succeeded= achieved_distance >= min_distance_;
|
succeeded= achieved_distance >= props.get<double>("min_distance");
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
throw std::runtime_error("position motion has neither a goal nor a direction");
|
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();
|
planning_scene::PlanningScenePtr result_scene = to.scene()->diff();
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group_);
|
const auto& props = properties();
|
||||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link_);
|
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=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
std::bind(
|
std::bind(
|
||||||
@ -203,14 +219,15 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
|||||||
switch(mode_){
|
switch(mode_){
|
||||||
case(CartesianPositionMotion::MODE_TOWARDS):
|
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);
|
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case(CartesianPositionMotion::MODE_ALONG):
|
case(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");
|
||||||
tf::vectorMsgToEigen(along_.vector, direction);
|
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
|
||||||
|
tf::vectorMsgToEigen(along.vector, direction);
|
||||||
direction= frame.linear()*direction;
|
direction= frame.linear()*direction;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
@ -226,14 +243,14 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
|||||||
link_model,
|
link_model,
|
||||||
direction,
|
direction,
|
||||||
true, /* global frame */
|
true, /* global frame */
|
||||||
max_distance_, /* distance */
|
props.get<double>("max_distance"),
|
||||||
step_size_, /* cartesian step size */
|
props.get<double>("step_size"), /* cartesian step size */
|
||||||
1.5, /* jump threshold */
|
1.5, /* jump threshold */
|
||||||
is_valid);
|
is_valid);
|
||||||
|
|
||||||
std::cout << "achieved " << achieved_distance << " of cartesian motion" << std::endl;
|
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){
|
if(succeeded){
|
||||||
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||||
|
|||||||
@ -57,6 +57,16 @@ namespace moveit { namespace task_constructor { namespace stages {
|
|||||||
GenerateGraspPose::GenerateGraspPose(std::string name)
|
GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||||
: Generator(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)
|
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){
|
void GenerateGraspPose::setGroup(std::string group){
|
||||||
group_= group;
|
setProperty("group", group);
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setLink(std::string ik_link){
|
|
||||||
ik_link_= ik_link;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setEndEffector(std::string eef){
|
void GenerateGraspPose::setEndEffector(std::string eef){
|
||||||
eef_= eef;
|
setProperty("eef", eef);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
void GenerateGraspPose::setGripperGraspPose(std::string pose_name){
|
||||||
gripper_grasp_pose_= pose_name;
|
setProperty("eef_grasp_pose", pose_name);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setObject(std::string object){
|
void GenerateGraspPose::setObject(std::string object){
|
||||||
object_= object;
|
setProperty("object", object);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setGraspOffset(double offset){
|
void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){
|
||||||
grasp_offset_= offset;
|
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){
|
void GenerateGraspPose::setTimeout(double timeout){
|
||||||
timeout_= timeout;
|
setProperty("timeout", timeout);
|
||||||
remaining_time_= timeout;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setAngleDelta(double delta){
|
void GenerateGraspPose::setAngleDelta(double delta){
|
||||||
angle_delta_= delta;
|
setProperty("angle_delta", delta);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::setMaxIKSolutions(uint32_t n){
|
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{
|
bool GenerateGraspPose::canCompute() const{
|
||||||
@ -134,66 +151,85 @@ namespace {
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool GenerateGraspPose::compute(){
|
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();
|
planning_scene::PlanningScenePtr grasp_scene = scene_->diff();
|
||||||
robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst();
|
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(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()){
|
if (grasp_frame.header.frame_id != link_name) {
|
||||||
grasp_state.setToDefaultValues(jmg_eef, gripper_grasp_pose_);
|
// 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=
|
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
||||||
std::bind(
|
std::bind(
|
||||||
&isValid,
|
&isValid,
|
||||||
scene_,
|
scene_,
|
||||||
ignore_collisions_,
|
props.get<bool>("ignore_collisions"),
|
||||||
&previous_solutions_,
|
&previous_solutions_,
|
||||||
std::placeholders::_1,
|
std::placeholders::_1,
|
||||||
std::placeholders::_2,
|
std::placeholders::_2,
|
||||||
std::placeholders::_3);
|
std::placeholders::_3);
|
||||||
|
|
||||||
geometry_msgs::Pose object_pose, grasp_pose;
|
const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get<std::string>("object"));
|
||||||
const Eigen::Affine3d object_pose_eigen= scene_->getFrameTransform(object_);
|
if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
||||||
if(object_pose_eigen.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all())
|
|
||||||
throw std::runtime_error("requested object does not exist or could not be retrieved");
|
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() ){
|
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_
|
std::cout << "computed angle " << current_angle_
|
||||||
<< " with " << previous_solutions_.size()
|
<< " with " << previous_solutions_.size()
|
||||||
<< " cached ik solutions"
|
<< " cached ik solutions"
|
||||||
<< " and " << remaining_time_ << "s left" << std::endl;
|
<< " and " << remaining_time << "s left" << std::endl;
|
||||||
current_angle_+= angle_delta_;
|
current_angle_+= props.get<double>("angle_delta");
|
||||||
remaining_time_= timeout_;
|
remaining_time = props.get<double>("timeout");
|
||||||
tried_current_state_as_seed_= false;
|
tried_current_state_as_seed_= false;
|
||||||
previous_solutions_.clear();
|
previous_solutions_.clear();
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
grasp_pose= object_pose;
|
// rotate object pose about z-axis
|
||||||
|
Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_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_);
|
|
||||||
|
|
||||||
if(tried_current_state_as_seed_)
|
if(tried_current_state_as_seed_)
|
||||||
grasp_state.setToRandomPositions(jmg_active);
|
grasp_state.setToRandomPositions(jmg_active);
|
||||||
tried_current_state_as_seed_= true;
|
tried_current_state_as_seed_= true;
|
||||||
|
|
||||||
auto now= std::chrono::steady_clock::now();
|
auto now= std::chrono::steady_clock::now();
|
||||||
bool succeeded= grasp_state.setFromIK(jmg_active, grasp_pose, ik_link, 1, remaining_time_, is_valid);
|
bool succeeded= grasp_state.setFromIK(jmg_active, goal_pose, link_name, 1, remaining_time, is_valid);
|
||||||
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
remaining_time-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
||||||
|
|
||||||
if(succeeded) {
|
if(succeeded) {
|
||||||
previous_solutions_.emplace_back();
|
previous_solutions_.emplace_back();
|
||||||
|
|||||||
@ -51,7 +51,13 @@ namespace moveit { namespace task_constructor { namespace stages {
|
|||||||
|
|
||||||
Gripper::Gripper(std::string name)
|
Gripper::Gripper(std::string name)
|
||||||
: PropagatingEitherWay(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)
|
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){
|
void Gripper::setEndEffector(std::string eef){
|
||||||
eef_= eef;
|
setProperty("eef", eef);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::setAttachLink(std::string link){
|
void Gripper::setAttachLink(std::string link){
|
||||||
attach_link_= link;
|
setProperty("link", link);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::setFrom(std::string named_target){
|
void Gripper::setFrom(std::string named_target){
|
||||||
restrictDirection(BACKWARD);
|
restrictDirection(BACKWARD);
|
||||||
named_target_= named_target;
|
setProperty("named_target", named_target);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::setTo(std::string named_target){
|
void Gripper::setTo(std::string named_target){
|
||||||
restrictDirection(FORWARD);
|
restrictDirection(FORWARD);
|
||||||
named_target_= named_target;
|
setProperty("named_target", named_target);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Gripper::graspObject(std::string grasp_object){
|
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,
|
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();
|
scene = state.scene()->diff();
|
||||||
assert(scene->getRobotModel());
|
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_){
|
if(!mgi_){
|
||||||
assert(scene->getRobotModel()->hasEndEffector(eef_) && "no end effector with that name defined in srdf");
|
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
|
||||||
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef_);
|
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef);
|
||||||
const std::string group_name= jmg->getName();
|
const std::string group_name= jmg->getName();
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
||||||
|
|
||||||
if( attach_link_.empty() ){
|
if( link.empty() ){
|
||||||
attach_link_= jmg->getEndEffectorParentGroup().second;
|
link= jmg->getEndEffectorParentGroup().second;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
mgi_->setNamedTarget(named_target_);
|
mgi_->setNamedTarget(named_target);
|
||||||
|
|
||||||
::planning_interface::MotionPlanRequest req;
|
::planning_interface::MotionPlanRequest req;
|
||||||
mgi_->constructMotionPlanRequest(req);
|
mgi_->constructMotionPlanRequest(req);
|
||||||
|
|
||||||
if( !grasp_object_.empty() ){
|
if( !grasp_object.empty() ){
|
||||||
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_, mgi_->getLinkNames(), true);
|
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true);
|
||||||
}
|
}
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
@ -116,10 +128,10 @@ bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScene
|
|||||||
scene->setCurrentState(trajectory->getLastWayPoint());
|
scene->setCurrentState(trajectory->getLastWayPoint());
|
||||||
|
|
||||||
// attach object
|
// attach object
|
||||||
if( !grasp_object_.empty() ){
|
if( !grasp_object.empty() ){
|
||||||
moveit_msgs::AttachedCollisionObject obj;
|
moveit_msgs::AttachedCollisionObject obj;
|
||||||
obj.link_name= attach_link_;
|
obj.link_name= link;
|
||||||
obj.object.id= grasp_object_;
|
obj.object.id= grasp_object;
|
||||||
scene->processAttachedCollisionObjectMsg(obj);
|
scene->processAttachedCollisionObjectMsg(obj);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -50,9 +50,13 @@
|
|||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
Move::Move(std::string name)
|
Move::Move(std::string name)
|
||||||
: Connecting(name),
|
: Connecting(name)
|
||||||
timeout_(5.0)
|
{
|
||||||
{}
|
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)
|
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());
|
planner_ = Task::createPlanner(scene->getRobotModel());
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::setGroup(std::string group){
|
void Move::setGroup(const std::string& group){
|
||||||
group_= group;
|
setProperty("group", group);
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::setPlannerId(std::string planner){
|
void Move::setPlannerId(const std::string& planner){
|
||||||
planner_id_= planner;
|
setProperty("planner", planner);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Move::setTimeout(double timeout){
|
void Move::setTimeout(double timeout){
|
||||||
timeout_= timeout;
|
setProperty("timeout", timeout);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||||
mgi_->setJointValueTarget(to.scene()->getCurrentState());
|
const auto& props = properties();
|
||||||
if( !planner_id_.empty() )
|
moveit::planning_interface::MoveGroupInterface mgi(props.get<std::string>("group"));
|
||||||
mgi_->setPlannerId(planner_id_);
|
mgi.setJointValueTarget(to.scene()->getCurrentState());
|
||||||
mgi_->setPlanningTime(timeout_);
|
|
||||||
|
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;
|
::planning_interface::MotionPlanRequest req;
|
||||||
mgi_->constructMotionPlanRequest(req);
|
mgi.constructMotionPlanRequest(req);
|
||||||
|
|
||||||
ros::Duration(4.0).sleep();
|
ros::Duration(4.0).sleep();
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
|
|||||||
@ -50,7 +50,7 @@ InterfaceState::InterfaceState(const planning_scene::PlanningSceneConstPtr &ps)
|
|||||||
}
|
}
|
||||||
|
|
||||||
InterfaceState::InterfaceState(const InterfaceState &existing)
|
InterfaceState::InterfaceState(const InterfaceState &existing)
|
||||||
: scene_(existing.scene())
|
: scene_(existing.scene_), properties_(existing.properties_)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -252,6 +252,18 @@ const ContainerBase* Task::stages() const
|
|||||||
return const_cast<Task*>(this)->stages();
|
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
|
std::string Task::id() const
|
||||||
{
|
{
|
||||||
return id_;
|
return id_;
|
||||||
|
|||||||
@ -7,8 +7,11 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-container
|
target_link_libraries(${PROJECT_NAME}-test-container
|
||||||
${PROJECT_NAME}
|
${PROJECT_NAME}
|
||||||
${catkin_LIBRARIES}
|
|
||||||
gtest_main)
|
gtest_main)
|
||||||
|
|
||||||
|
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
|
||||||
|
target_link_libraries(${PROJECT_NAME}-test-properties
|
||||||
|
${PROJECT_NAME} gtest_main)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
||||||
|
|||||||
@ -43,7 +43,7 @@ int main(int argc, char** argv){
|
|||||||
st->setObject("object");
|
st->setObject("object");
|
||||||
st->setTimeout(0.5);
|
st->setTimeout(0.5);
|
||||||
st->setAngleDelta(0.1);
|
st->setAngleDelta(0.1);
|
||||||
st->setGraspOffset(0.03);
|
st->setGraspFrame(Eigen::Translation3d(.03,0,0));
|
||||||
|
|
||||||
t.add(std::move(st));
|
t.add(std::move(st));
|
||||||
|
|
||||||
|
|||||||
127
core/test/test_properties.cpp
Normal file
127
core/test/test_properties.cpp
Normal 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);
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue
Block a user