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