mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'move-to', 'simple-grasp' and 'properties' into master
These branches only work together: - MoveTo / MoveRelative: common handling of "goal" property - properties: required changes to allow for multiple inheritance - generalize SimpleGrasp / GenerateGraspPose
This commit is contained in:
commit
3215880b98
@ -99,15 +99,16 @@ 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->properties().configureInitFrom(Stage::PARENT);
|
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
||||||
gengrasp->setNamedPose("open");
|
gengrasp->setPreGraspPose("open");
|
||||||
gengrasp->setObject("object");
|
gengrasp->setObject("object");
|
||||||
gengrasp->setAngleDelta(M_PI / 10.);
|
gengrasp->setAngleDelta(M_PI / 10.);
|
||||||
gengrasp->setMonitoredStage(initial_stage);
|
gengrasp->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
ik->properties().configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
|
PropertyMap &props = ik->properties();
|
||||||
ik->setIKFrame(Eigen::Translation3d(0,0,.05)*
|
props.configureInitFrom(Stage::PARENT, {"group", "eef", "default_pose"});
|
||||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
props.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution
|
||||||
|
ik->setIKFrame(Eigen::Translation3d(0,0,.05) * Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||||
"lh_tool_frame");
|
"lh_tool_frame");
|
||||||
ik->setMaxIKSolutions(1);
|
ik->setMaxIKSolutions(1);
|
||||||
t.add(std::move(ik));
|
t.add(std::move(ik));
|
||||||
|
|||||||
@ -1,6 +1,7 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
#include <moveit/task_constructor/stages/pick.h>
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
#include <moveit/task_constructor/stages/connect.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
@ -55,15 +56,17 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(connect));
|
t.add(std::move(connect));
|
||||||
|
|
||||||
// grasp generator
|
// grasp generator
|
||||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
auto grasp_generator = new stages::GenerateGraspPose("generate grasp pose");
|
||||||
grasp_generator->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
|
||||||
grasp_generator->setAngleDelta(.2);
|
grasp_generator->setAngleDelta(.2);
|
||||||
grasp_generator->setPreGraspPose("open");
|
grasp_generator->setPreGraspPose("open");
|
||||||
grasp_generator->setGraspPose("closed");
|
grasp_generator->setGraspPose("closed");
|
||||||
grasp_generator->setMonitoredStage(initial_stage);
|
grasp_generator->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
|
auto grasp = std::make_unique<stages::SimpleGrasp>(std::unique_ptr<MonitoringGenerator>(grasp_generator));
|
||||||
|
grasp->setIKFrame(Eigen::Affine3d::Identity(), "l_gripper_tool_frame");
|
||||||
|
|
||||||
// pick stage
|
// pick stage
|
||||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
auto pick = std::make_unique<stages::Pick>(std::move(grasp));
|
||||||
pick->setProperty("eef", std::string("left_gripper"));
|
pick->setProperty("eef", std::string("left_gripper"));
|
||||||
pick->setProperty("object", std::string("object"));
|
pick->setProperty("object", std::string("object"));
|
||||||
geometry_msgs::TwistStamped approach;
|
geometry_msgs::TwistStamped approach;
|
||||||
|
|||||||
@ -1,6 +1,7 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
#include <moveit/task_constructor/stages/pick.h>
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
#include <moveit/task_constructor/stages/connect.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
@ -55,15 +56,17 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(connect));
|
t.add(std::move(connect));
|
||||||
|
|
||||||
// grasp generator
|
// grasp generator
|
||||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
auto grasp_generator = new stages::GenerateGraspPose("generate grasp pose");
|
||||||
grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
|
||||||
grasp_generator->setMaxIKSolutions(8);
|
|
||||||
grasp_generator->setAngleDelta(.2);
|
grasp_generator->setAngleDelta(.2);
|
||||||
grasp_generator->setPreGraspPose("open");
|
grasp_generator->setPreGraspPose("open");
|
||||||
grasp_generator->setGraspPose("closed");
|
grasp_generator->setGraspPose("closed");
|
||||||
grasp_generator->setMonitoredStage(initial_stage);
|
grasp_generator->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
auto grasp = std::make_unique<stages::SimpleGrasp>(std::unique_ptr<MonitoringGenerator>(grasp_generator));
|
||||||
|
grasp->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
|
grasp->setMaxIKSolutions(8);
|
||||||
|
|
||||||
|
auto pick = std::make_unique<stages::Pick>(std::move(grasp));
|
||||||
pick->setProperty("eef", std::string("gripper"));
|
pick->setProperty("eef", std::string("gripper"));
|
||||||
pick->setProperty("object", std::string("object"));
|
pick->setProperty("object", std::string("object"));
|
||||||
geometry_msgs::TwistStamped approach;
|
geometry_msgs::TwistStamped approach;
|
||||||
|
|||||||
@ -62,12 +62,6 @@ public:
|
|||||||
virtual bool remove(int pos);
|
virtual bool remove(int pos);
|
||||||
virtual void clear();
|
virtual void clear();
|
||||||
|
|
||||||
/// declare all given variables of child and configure child to inherit them from parent
|
|
||||||
void exposePropertiesOfChild(int child, const std::initializer_list<std::string>& names);
|
|
||||||
/// declare given child property with another name and configure child to inherit it from parent
|
|
||||||
void exposePropertyOfChildAs(int child, const std::string& child_property_name,
|
|
||||||
const std::string& parent_property_name);
|
|
||||||
|
|
||||||
void reset() override;
|
void reset() override;
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
/// validate connectivity of children (after init() was done)
|
/// validate connectivity of children (after init() was done)
|
||||||
|
|||||||
@ -108,7 +108,7 @@ public:
|
|||||||
/// exception thrown when trying to set a value not matching the declared type
|
/// exception thrown when trying to set a value not matching the declared type
|
||||||
class type_error;
|
class type_error;
|
||||||
|
|
||||||
typedef int SourceId;
|
typedef uint SourceFlags;
|
||||||
/// function callback used to initialize property value from another PropertyMap
|
/// function callback used to initialize property value from another PropertyMap
|
||||||
typedef std::function<boost::any(const PropertyMap& other)> InitializerFunction;
|
typedef std::function<boost::any(const PropertyMap& other)> InitializerFunction;
|
||||||
/// function callback used to signal value setting to external components
|
/// function callback used to signal value setting to external components
|
||||||
@ -129,7 +129,8 @@ public:
|
|||||||
/// get default value
|
/// get default value
|
||||||
const boost::any& defaultValue() const { return default_; }
|
const boost::any& defaultValue() const { return default_; }
|
||||||
/// serialize current value
|
/// serialize current value
|
||||||
std::string serialize() const;
|
std::string serialize(const boost::any& value) const;
|
||||||
|
std::string serialize() const { return serialize(value()); }
|
||||||
|
|
||||||
/// get description text
|
/// get description text
|
||||||
const std::string& description() const { return description_; }
|
const std::string& description() const { return description_; }
|
||||||
@ -139,14 +140,11 @@ public:
|
|||||||
std::string typeName() const { return type_index_.name(); }
|
std::string typeName() const { return type_index_.name(); }
|
||||||
|
|
||||||
/// return true, if property initialized from given SourceId
|
/// return true, if property initialized from given SourceId
|
||||||
bool initsFrom(SourceId source) const;
|
bool initsFrom(SourceFlags source) const;
|
||||||
/// configure initialization from source using an arbitrary function
|
/// configure initialization from source using an arbitrary function
|
||||||
Property &configureInitFrom(SourceId source, const InitializerFunction& f);
|
Property &configureInitFrom(SourceFlags source, const InitializerFunction& f);
|
||||||
/// configure initialization from source using given other property name
|
/// configure initialization from source using given other property name
|
||||||
Property &configureInitFrom(SourceId source, const std::string& name);
|
Property &configureInitFrom(SourceFlags source, const std::string& name);
|
||||||
|
|
||||||
/// set current value using matching configured initializers
|
|
||||||
void performInitFrom(SourceId source, const PropertyMap& other);
|
|
||||||
|
|
||||||
/// define a function callback to be called on each value update
|
/// define a function callback to be called on each value update
|
||||||
/// note, that boost::any doesn't allow for change detection
|
/// note, that boost::any doesn't allow for change detection
|
||||||
@ -159,7 +157,8 @@ private:
|
|||||||
boost::any value_;
|
boost::any value_;
|
||||||
|
|
||||||
/// used for external initialization
|
/// used for external initialization
|
||||||
SourceId source_id_ = 0;
|
SourceFlags source_flags_ = 0;
|
||||||
|
SourceFlags initialized_from_;
|
||||||
InitializerFunction initializer_;
|
InitializerFunction initializer_;
|
||||||
SignalFunction signaller_;
|
SignalFunction signaller_;
|
||||||
SerializeFunction serialize_;
|
SerializeFunction serialize_;
|
||||||
@ -239,7 +238,7 @@ public:
|
|||||||
const_iterator end() const { return props_.end(); }
|
const_iterator end() const { return props_.end(); }
|
||||||
|
|
||||||
/// allow initialization from given source for listed properties - always using the same 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 = {});
|
void configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties = {});
|
||||||
|
|
||||||
/// set (and, if neccessary, declare) the value of a property
|
/// set (and, if neccessary, declare) the value of a property
|
||||||
template <typename T>
|
template <typename T>
|
||||||
@ -284,7 +283,7 @@ public:
|
|||||||
void reset();
|
void reset();
|
||||||
|
|
||||||
/// perform initialization of still undefined properties using configured initializers
|
/// perform initialization of still undefined properties using configured initializers
|
||||||
void performInitFrom(Property::SourceId source, const PropertyMap& other, bool enforce = false);
|
void performInitFrom(Property::SourceFlags source, const PropertyMap& other);
|
||||||
};
|
};
|
||||||
|
|
||||||
// boost::any needs a specialization to avoid recursion
|
// boost::any needs a specialization to avoid recursion
|
||||||
|
|||||||
@ -132,9 +132,11 @@ public:
|
|||||||
*
|
*
|
||||||
* INTERFACE takes precedence over PARENT.
|
* INTERFACE takes precedence over PARENT.
|
||||||
*/
|
*/
|
||||||
enum PropertyInitializerSource {
|
enum PropertyInitializerSource { // TODO: move to property.cpp
|
||||||
PARENT,
|
DEFAULT = 0,
|
||||||
INTERFACE,
|
MANUAL = 1,
|
||||||
|
PARENT = 2,
|
||||||
|
INTERFACE = 4,
|
||||||
};
|
};
|
||||||
virtual ~Stage();
|
virtual ~Stage();
|
||||||
|
|
||||||
@ -159,6 +161,13 @@ public:
|
|||||||
void setTimeout(double timeout) { setProperty("timeout", timeout); }
|
void setTimeout(double timeout) { setProperty("timeout", timeout); }
|
||||||
double timeout() const { return properties().get<double>("timeout"); }
|
double timeout() const { return properties().get<double>("timeout"); }
|
||||||
|
|
||||||
|
/// forwarding of properties between interface states
|
||||||
|
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
|
||||||
|
std::set<std::string> forwardedProperties() const
|
||||||
|
{ return properties().get<std::set<std::string>>("forwarded_properties"); }
|
||||||
|
void setForwardedProperties(const std::set<std::string>& names)
|
||||||
|
{ setProperty("forwarded_properties", names); }
|
||||||
|
|
||||||
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
typedef std::function<void(const SolutionBase &s)> SolutionCallback;
|
||||||
typedef std::list<SolutionCallback> SolutionCallbackList;
|
typedef std::list<SolutionCallback> SolutionCallbackList;
|
||||||
/// add function to be called for every newly found solution or failure
|
/// add function to be called for every newly found solution or failure
|
||||||
|
|||||||
@ -104,10 +104,6 @@ public:
|
|||||||
void setIgnoreCollisions(bool flag) {
|
void setIgnoreCollisions(bool flag) {
|
||||||
setProperty("ignore_collisions", flag);
|
setProperty("ignore_collisions", flag);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setForwardedProperties(const std::set<std::string>& names) {
|
|
||||||
setProperty("forward_properties", names);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -49,10 +49,14 @@ public:
|
|||||||
void init(const core::RobotModelConstPtr &robot_model) override;
|
void init(const core::RobotModelConstPtr &robot_model) override;
|
||||||
void compute() override;
|
void compute() override;
|
||||||
|
|
||||||
void setEndEffector(const std::string &eef);
|
void setEndEffector(const std::string &eef) { setProperty("eef", eef); }
|
||||||
void setNamedPose(const std::string &pose_name);
|
void setObject(const std::string &object) { setProperty("object", object); }
|
||||||
void setObject(const std::string &object);
|
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
|
||||||
void setAngleDelta(double delta);
|
|
||||||
|
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
|
||||||
|
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }
|
||||||
|
void setGraspPose(const std::string& grasp) { properties().set("grasp", grasp); }
|
||||||
|
void setGraspPose(const moveit_msgs::RobotState& grasp) { properties().set("grasp", grasp); }
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void onNewSolution(const SolutionBase& s) override;
|
void onNewSolution(const SolutionBase& s) override;
|
||||||
|
|||||||
@ -48,39 +48,30 @@ class GenerateGraspPose;
|
|||||||
|
|
||||||
/** base class for simple grasping / ungrasping
|
/** base class for simple grasping / ungrasping
|
||||||
*
|
*
|
||||||
* Given a named pre-grasp and grasp posture the stage generates fully-qualified
|
* Given a pre-grasp and grasp posture the stage generates a trajectory
|
||||||
* pre-grasp and grasp robot states, connected by a grasping trajectory of the end-effector.
|
* connecting these two states by a grasping trajectory.
|
||||||
|
* The class takes a generator stage that provides the grasp pose and
|
||||||
|
* optionally the pre-grasp and grasp postures.
|
||||||
*
|
*
|
||||||
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
|
* Grasping and UnGrasping only differs in the order of subtasks. Hence, the base class
|
||||||
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
|
* provides the common functionality for grasping (forward = true) and ungrasping (forward = false).
|
||||||
*/
|
*/
|
||||||
class SimpleGraspBase : public SerialContainer {
|
class SimpleGraspBase : public SerialContainer {
|
||||||
moveit::core::RobotModelConstPtr model_;
|
moveit::core::RobotModelConstPtr model_;
|
||||||
GenerateGraspPose* grasp_generator_ = nullptr;
|
|
||||||
|
protected:
|
||||||
|
void setup(std::unique_ptr<Stage>&& generator, bool forward);
|
||||||
|
|
||||||
public:
|
public:
|
||||||
SimpleGraspBase(const std::string& name, bool forward);
|
SimpleGraspBase(const std::string& name);
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
|
||||||
void setMonitoredStage(Stage* monitored);
|
|
||||||
|
|
||||||
void setEndEffector(const std::string& eef) {
|
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
|
||||||
properties().set<std::string>("eef", eef);
|
void setObject(const std::string& object) { setProperty("object", object); }
|
||||||
}
|
|
||||||
void setObject(const std::string& object) {
|
|
||||||
properties().set<std::string>("object", object);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setPreGraspPose(const std::string& pregrasp) {
|
/// set properties of IK solver
|
||||||
properties().set<std::string>("pregrasp", pregrasp);
|
void setIKFrame(const geometry_msgs::PoseStamped &transform) { setProperty("ik_frame", transform); }
|
||||||
}
|
|
||||||
void setGraspPose(const std::string& grasp) {
|
|
||||||
properties().set<std::string>("grasp", grasp);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setIKFrame(const geometry_msgs::PoseStamped &transform) {
|
|
||||||
properties().set("ik_frame", transform);
|
|
||||||
}
|
|
||||||
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
|
void setIKFrame(const Eigen::Affine3d& pose, const std::string& link);
|
||||||
template <typename T>
|
template <typename T>
|
||||||
void setIKFrame(const T& t, const std::string& link) {
|
void setIKFrame(const T& t, const std::string& link) {
|
||||||
@ -91,30 +82,21 @@ public:
|
|||||||
setIKFrame(Eigen::Affine3d::Identity(), link);
|
setIKFrame(Eigen::Affine3d::Identity(), link);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setAngleDelta(double angle_delta) {
|
void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); }
|
||||||
properties().set("angle_delta", angle_delta);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setMaxIKSolutions(uint32_t max_ik_solutions) {
|
|
||||||
properties().set("max_ik_solutions", max_ik_solutions);
|
|
||||||
}
|
|
||||||
void setTimeout(double timeout) {
|
|
||||||
properties().set("timeout", timeout);
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/// specialization of SimpleGraspBase to realize grasping
|
/// specialization of SimpleGraspBase to realize grasping
|
||||||
class SimpleGrasp : public SimpleGraspBase {
|
class SimpleGrasp : public SimpleGraspBase {
|
||||||
public:
|
public:
|
||||||
SimpleGrasp(const std::string& name = "grasp") : SimpleGraspBase(name, true) {}
|
SimpleGrasp(std::unique_ptr<Stage>&& generator, const std::string& name = "grasp");
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
/// specialization of SimpleGraspBase to realize ungrasping
|
/// specialization of SimpleGraspBase to realize ungrasping
|
||||||
class SimpleUnGrasp : public SimpleGraspBase {
|
class SimpleUnGrasp : public SimpleGraspBase {
|
||||||
public:
|
public:
|
||||||
SimpleUnGrasp(const std::string& name = "ungrasp") : SimpleGraspBase(name, false) {}
|
SimpleUnGrasp(std::unique_ptr<Stage>&& generator, const std::string& name = "ungrasp");
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -194,35 +194,6 @@ void ContainerBase::clear()
|
|||||||
pimpl()->children_.clear();
|
pimpl()->children_.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ContainerBase::exposePropertiesOfChild(int child, const std::initializer_list<std::string>& names)
|
|
||||||
{
|
|
||||||
auto impl = pimpl();
|
|
||||||
ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false);
|
|
||||||
if (child_it == impl->children().end())
|
|
||||||
throw std::runtime_error("invalid child index");
|
|
||||||
|
|
||||||
auto &child_props = (*child_it)->properties();
|
|
||||||
// declare variables
|
|
||||||
child_props.exposeTo(impl->properties_, names);
|
|
||||||
// configure inheritance
|
|
||||||
child_props.configureInitFrom(Stage::PARENT, names);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ContainerBase::exposePropertyOfChildAs(int child, const std::string& child_property_name,
|
|
||||||
const std::string& parent_property_name)
|
|
||||||
{
|
|
||||||
auto impl = pimpl();
|
|
||||||
ContainerBasePrivate::const_iterator child_it = impl->childByIndex(child, false);
|
|
||||||
if (child_it == impl->children().end())
|
|
||||||
throw std::runtime_error("invalid child index");
|
|
||||||
|
|
||||||
auto &child_props = (*child_it)->properties();
|
|
||||||
// declare variables
|
|
||||||
child_props.exposeTo(impl->properties_, child_property_name, parent_property_name);
|
|
||||||
// configure inheritance
|
|
||||||
child_props.property(child_property_name).configureInitFrom(Stage::PARENT, parent_property_name);
|
|
||||||
}
|
|
||||||
|
|
||||||
void ContainerBase::reset()
|
void ContainerBase::reset()
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
@ -254,7 +225,14 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
// recursively init all children and accumulate errors
|
// recursively init all children and accumulate errors
|
||||||
InitStageException errors;
|
InitStageException errors;
|
||||||
for (auto& child : children) {
|
for (auto& child : children) {
|
||||||
try { child->init(robot_model); } catch (InitStageException &e) { errors.append(e); }
|
try { child->init(robot_model); }
|
||||||
|
catch (const Property::error &e) {
|
||||||
|
std::ostringstream oss;
|
||||||
|
oss << e.what();
|
||||||
|
pimpl()->composePropertyErrorMsg(e.name(), oss);
|
||||||
|
errors.push_back(*child, oss.str());
|
||||||
|
}
|
||||||
|
catch (InitStageException &e) { errors.append(e); }
|
||||||
}
|
}
|
||||||
|
|
||||||
if (errors) throw errors;
|
if (errors) throw errors;
|
||||||
|
|||||||
@ -39,6 +39,7 @@
|
|||||||
#include <moveit/task_constructor/properties.h>
|
#include <moveit/task_constructor/properties.h>
|
||||||
#include <boost/format.hpp>
|
#include <boost/format.hpp>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
#include <ros/console.h>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
@ -49,15 +50,18 @@ Property::Property(const Property::type_index& type_index, const std::string& de
|
|||||||
, type_index_(type_index)
|
, type_index_(type_index)
|
||||||
, default_(default_value)
|
, default_(default_value)
|
||||||
, value_()
|
, value_()
|
||||||
|
, initialized_from_(-1)
|
||||||
, serialize_(serialize)
|
, serialize_(serialize)
|
||||||
{
|
{
|
||||||
// default value's type should match declared type by construction
|
// default value's type should match declared type by construction
|
||||||
assert(default_.empty() || default_.type() == type_index_ || type_index_ == typeid(boost::any));
|
assert(default_.empty() || default_.type() == type_index_ || type_index_ == typeid(boost::any));
|
||||||
|
reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::setValue(const boost::any &value) {
|
void Property::setValue(const boost::any &value) {
|
||||||
setCurrentValue(value);
|
setCurrentValue(value);
|
||||||
default_ = value_;
|
default_ = value_;
|
||||||
|
initialized_from_ = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::setCurrentValue(const boost::any &value)
|
void Property::setCurrentValue(const boost::any &value)
|
||||||
@ -66,46 +70,45 @@ void Property::setCurrentValue(const boost::any &value)
|
|||||||
throw Property::type_error(value.type().name(), type_index_.name());
|
throw Property::type_error(value.type().name(), type_index_.name());
|
||||||
|
|
||||||
value_ = value;
|
value_ = value;
|
||||||
|
initialized_from_ = 1; // manually initialized TODO: use enums
|
||||||
|
|
||||||
if (signaller_)
|
if (signaller_)
|
||||||
signaller_(this);
|
signaller_(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::reset()
|
void Property::reset()
|
||||||
{
|
{
|
||||||
|
if (initialized_from_ == 0) // TODO: use enum
|
||||||
|
return; // keep manually set values
|
||||||
boost::any().swap(value_);
|
boost::any().swap(value_);
|
||||||
|
initialized_from_ = -1; // set to max value
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string Property::serialize() const {
|
std::string Property::serialize(const boost::any& v) const {
|
||||||
if (!serialize_) return "";
|
if (!serialize_ || v.empty()) return "";
|
||||||
return serialize_(value());
|
return serialize_(v);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Property::initsFrom(Property::SourceId source) const
|
bool Property::initsFrom(Property::SourceFlags source) const
|
||||||
{
|
{
|
||||||
return (source == source_id_ && initializer_);
|
return source & source_flags_;
|
||||||
}
|
}
|
||||||
|
|
||||||
Property& Property::configureInitFrom(SourceId source, const Property::InitializerFunction &f)
|
Property& Property::configureInitFrom(SourceFlags source, const Property::InitializerFunction &f)
|
||||||
{
|
{
|
||||||
if (source != source_id_ && initializer_)
|
if (source != source_flags_ && initializer_)
|
||||||
throw error("Property was already configured for initialization from another source id");
|
throw error("Property was already configured for initialization from another source id");
|
||||||
|
|
||||||
source_id_ = source;
|
source_flags_ = f ? source : SourceFlags();
|
||||||
initializer_ = f;
|
initializer_ = f;
|
||||||
return *this;
|
return *this;
|
||||||
}
|
}
|
||||||
|
|
||||||
Property &Property::configureInitFrom(SourceId source, const std::string &name)
|
Property &Property::configureInitFrom(SourceFlags source, const std::string &name)
|
||||||
{
|
{
|
||||||
return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); });
|
return configureInitFrom(source, [name](const PropertyMap& other) { return fromName(other, name); });
|
||||||
}
|
}
|
||||||
|
|
||||||
void Property::performInitFrom(SourceId source, const PropertyMap &other)
|
|
||||||
{
|
|
||||||
if (source_id_ != source || !initializer_) return; // source ids not matching
|
|
||||||
setCurrentValue(initializer_(other));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
Property& PropertyMap::declare(const std::string &name, const Property::type_index &type_index,
|
Property& PropertyMap::declare(const std::string &name, const Property::type_index &type_index,
|
||||||
const std::string &description, const boost::any &default_value,
|
const std::string &description, const boost::any &default_value,
|
||||||
@ -142,10 +145,10 @@ void PropertyMap::exposeTo(PropertyMap& other, const std::set<std::string> &prop
|
|||||||
void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const
|
void PropertyMap::exposeTo(PropertyMap& other, const std::string& name, const std::string& other_name) const
|
||||||
{
|
{
|
||||||
const Property& p = property(name);
|
const Property& p = property(name);
|
||||||
other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_);
|
other.declare(other_name, p.type_index_, p.description_, p.default_, p.serialize_);
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropertyMap::configureInitFrom(Property::SourceId source, const std::set<std::string> &properties)
|
void PropertyMap::configureInitFrom(Property::SourceFlags source, const std::set<std::string> &properties)
|
||||||
{
|
{
|
||||||
for (auto &pair : props_) {
|
for (auto &pair : props_) {
|
||||||
if (properties.empty() || properties.count(pair.first))
|
if (properties.empty() || properties.count(pair.first))
|
||||||
@ -196,23 +199,39 @@ void PropertyMap::reset()
|
|||||||
pair.second.reset();
|
pair.second.reset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void PropertyMap::performInitFrom(Property::SourceId source, const PropertyMap &other, bool enforce)
|
void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMap &other)
|
||||||
{
|
{
|
||||||
for (auto& pair : props_) {
|
for (auto& pair : props_) {
|
||||||
Property &p = pair.second;
|
Property &p = pair.second;
|
||||||
if (enforce || !p.defined())
|
|
||||||
p.performInitFrom(source, other);
|
// don't override value previously set by higher-priority source
|
||||||
|
// MANUAL > CURRENT > PARENT > INTERFACE
|
||||||
|
if (p.initialized_from_ < source && p.defined())
|
||||||
|
continue;
|
||||||
|
// is the property configured for initialization from this source?
|
||||||
|
if (!p.initsFrom(source))
|
||||||
|
continue;
|
||||||
|
|
||||||
|
boost::any value;
|
||||||
|
try {
|
||||||
|
value = p.initializer_(other);
|
||||||
|
} catch (const Property::undeclared&) {
|
||||||
|
// ignore undeclared
|
||||||
|
continue;
|
||||||
|
} catch (const Property::undefined&) {
|
||||||
|
}
|
||||||
|
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Properties", pair.first << ": " << p.initialized_from_ <<
|
||||||
|
" -> " << source << ": " << p.serialize(value));
|
||||||
|
p.setCurrentValue(value);
|
||||||
|
p.initialized_from_ = source;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
boost::any fromName(const PropertyMap& other, const std::string& other_name)
|
boost::any fromName(const PropertyMap& other, const std::string& other_name)
|
||||||
{
|
{
|
||||||
try {
|
return other.get(other_name);
|
||||||
return other.get(other_name);
|
|
||||||
} catch (const std::runtime_error &e) {
|
|
||||||
return boost::any();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -105,6 +105,7 @@ void StagePrivate::sendForward(const InterfaceState& from, InterfaceState&& to,
|
|||||||
assert(nextStarts());
|
assert(nextStarts());
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
me()->forwardProperties(from, to);
|
||||||
|
|
||||||
auto to_it = states_.insert(states_.end(), std::move(to));
|
auto to_it = states_.insert(states_.end(), std::move(to));
|
||||||
|
|
||||||
@ -122,6 +123,7 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
|
|||||||
assert(prevEnds());
|
assert(prevEnds());
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return; // solution dropped
|
return; // solution dropped
|
||||||
|
me()->forwardProperties(to, from);
|
||||||
|
|
||||||
auto from_it = states_.insert(states_.end(), std::move(from));
|
auto from_it = states_.insert(states_.end(), std::move(from));
|
||||||
|
|
||||||
@ -181,6 +183,8 @@ Stage::Stage(StagePrivate *impl)
|
|||||||
assert(impl);
|
assert(impl);
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", "timeout per run (s)");
|
p.declare<double>("timeout", "timeout per run (s)");
|
||||||
|
p.declare<std::set<std::string>>("forwarded_properties", std::set<std::string>(),
|
||||||
|
"set of interface properties to forward");
|
||||||
p.declare<std::string>("marker_ns", "marker namespace");
|
p.declare<std::string>("marker_ns", "marker namespace");
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -222,6 +226,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
impl->properties_.reset();
|
impl->properties_.reset();
|
||||||
if (impl->parent()) {
|
if (impl->parent()) {
|
||||||
try {
|
try {
|
||||||
|
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
|
||||||
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
|
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
|
||||||
} catch (const Property::error &e) {
|
} catch (const Property::error &e) {
|
||||||
std::ostringstream oss;
|
std::ostringstream oss;
|
||||||
@ -246,6 +251,17 @@ void Stage::setName(const std::string& name)
|
|||||||
pimpl_->name_ = name;
|
pimpl_->name_ = name;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest)
|
||||||
|
{
|
||||||
|
const PropertyMap& src = source.properties();
|
||||||
|
PropertyMap& dst = dest.properties();
|
||||||
|
for (const auto& name : forwardedProperties()) {
|
||||||
|
if (!src.hasProperty(name))
|
||||||
|
continue;
|
||||||
|
dst.set(name, src.get(name));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
|
Stage::SolutionCallbackList::const_iterator Stage::addSolutionCallback(SolutionCallback &&cb)
|
||||||
{
|
{
|
||||||
auto impl = pimpl();
|
auto impl = pimpl();
|
||||||
@ -299,7 +315,7 @@ void StagePrivate::composePropertyErrorMsg(const std::string& property_name, std
|
|||||||
os << "declared, but undefined";
|
os << "declared, but undefined";
|
||||||
|
|
||||||
if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent";
|
if (p.initsFrom(Stage::PARENT)) os << ", inherits from parent";
|
||||||
else if (p.initsFrom(Stage::PARENT)) os << ", initializes from interface";
|
if (p.initsFrom(Stage::INTERFACE)) os << ", initializes from interface";
|
||||||
} catch (const Property::undeclared &e) {
|
} catch (const Property::undeclared &e) {
|
||||||
os << "undeclared";
|
os << "undeclared";
|
||||||
}
|
}
|
||||||
@ -440,13 +456,13 @@ void PropagatingEitherWayPrivate::compute()
|
|||||||
if (hasStartState()) {
|
if (hasStartState()) {
|
||||||
const InterfaceState& state = fetchStartState();
|
const InterfaceState& state = fetchStartState();
|
||||||
// enforce property initialization from INTERFACE
|
// enforce property initialization from INTERFACE
|
||||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
||||||
me->computeForward(state);
|
me->computeForward(state);
|
||||||
}
|
}
|
||||||
if (hasEndState()) {
|
if (hasEndState()) {
|
||||||
const InterfaceState& state = fetchEndState();
|
const InterfaceState& state = fetchEndState();
|
||||||
// enforce property initialization from INTERFACE
|
// enforce property initialization from INTERFACE
|
||||||
properties_.performInitFrom(Stage::INTERFACE, state.properties(), true);
|
properties_.performInitFrom(Stage::INTERFACE, state.properties());
|
||||||
me->computeBackward(state);
|
me->computeBackward(state);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -61,12 +61,10 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child)
|
|||||||
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
|
p.declare<std::string>("default_pose", "", "default joint pose of active group (defines cost of IK)");
|
||||||
p.declare<uint32_t>("max_ik_solutions", 1);
|
p.declare<uint32_t>("max_ik_solutions", 1);
|
||||||
p.declare<bool>("ignore_collisions", false);
|
p.declare<bool>("ignore_collisions", false);
|
||||||
p.declare<std::set<std::string>>("forward_properties", "to-be-forwarded properties from input");
|
|
||||||
|
|
||||||
// ik_frame and target_pose are read from the interface
|
// ik_frame and target_pose are read from the interface
|
||||||
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
|
||||||
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for ik frame");
|
p.declare<geometry_msgs::PoseStamped>("target_pose", "goal pose for ik frame");
|
||||||
p.configureInitFrom(Stage::INTERFACE, {"target_pose"});
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
|
void ComputeIK::setIKFrame(const Eigen::Affine3d &pose, const std::string &link)
|
||||||
@ -212,7 +210,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
// -1 TODO: this should not be necessary in my opinion: Why do you think so?
|
// -1 TODO: this should not be necessary in my opinion: Why do you think so?
|
||||||
// It is, because the properties on the interface might change from call to call...
|
// It is, because the properties on the interface might change from call to call...
|
||||||
// enforced initialization from interface ensures that new target_pose is read
|
// enforced initialization from interface ensures that new target_pose is read
|
||||||
properties().performInitFrom(INTERFACE, s.start()->properties(), true);
|
properties().performInitFrom(INTERFACE, s.start()->properties());
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
|
|
||||||
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
const bool ignore_collisions = props.get<bool>("ignore_collisions");
|
||||||
@ -370,12 +368,7 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
robot_state.update();
|
robot_state.update();
|
||||||
|
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
const boost::any &forwards = props.get("forward_properties");
|
forwardProperties(*s.start(), state);
|
||||||
if (!forwards.empty()) {
|
|
||||||
auto p = s.start()->properties();
|
|
||||||
p.exposeTo(state.properties(), boost::any_cast<std::set<std::string>>(forwards));
|
|
||||||
}
|
|
||||||
|
|
||||||
spawn(std::move(state), std::move(solution));
|
spawn(std::move(state), std::move(solution));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -51,25 +51,11 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name)
|
|||||||
{
|
{
|
||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<std::string>("eef", "name of end-effector");
|
p.declare<std::string>("eef", "name of end-effector");
|
||||||
p.declare<std::string>("pregrasp", "name of end-effector's pregrasp pose");
|
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setEndEffector(const std::string &eef) {
|
p.declare<boost::any>("pregrasp", "pregrasp posture");
|
||||||
setProperty("eef", eef);
|
p.declare<boost::any>("grasp", "grasp posture");
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setNamedPose(const std::string &pose_name) {
|
|
||||||
setProperty("pregrasp", pose_name);
|
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setObject(const std::string &object) {
|
|
||||||
setProperty("object", object);
|
|
||||||
}
|
|
||||||
|
|
||||||
void GenerateGraspPose::setAngleDelta(double delta){
|
|
||||||
setProperty("angle_delta", delta);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
|
void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
|
||||||
@ -84,6 +70,8 @@ void GenerateGraspPose::init(const core::RobotModelConstPtr& robot_model)
|
|||||||
if (props.get<double>("angle_delta") == 0.)
|
if (props.get<double>("angle_delta") == 0.)
|
||||||
errors.push_back(*this, "angle_delta must be non-zero");
|
errors.push_back(*this, "angle_delta must be non-zero");
|
||||||
|
|
||||||
|
// check availability of object
|
||||||
|
props.get<std::string>("object");
|
||||||
// check availability of eef
|
// check availability of eef
|
||||||
const std::string& eef = props.get<std::string>("eef");
|
const std::string& eef = props.get<std::string>("eef");
|
||||||
if (!robot_model->hasEndEffector(eef))
|
if (!robot_model->hasEndEffector(eef))
|
||||||
@ -141,6 +129,7 @@ void GenerateGraspPose::compute() {
|
|||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
|
tf::poseEigenToMsg(target_pose, target_pose_msg.pose);
|
||||||
state.properties().set("target_pose", target_pose_msg);
|
state.properties().set("target_pose", target_pose_msg);
|
||||||
|
props.exposeTo(state.properties(), {"pregrasp", "grasp"});
|
||||||
|
|
||||||
SubTrajectory trajectory;
|
SubTrajectory trajectory;
|
||||||
trajectory.setCost(0.0);
|
trajectory.setCost(0.0);
|
||||||
|
|||||||
@ -36,7 +36,6 @@
|
|||||||
|
|
||||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
|
||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
||||||
#include <moveit/task_constructor/stages/move_to.h>
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
@ -49,28 +48,45 @@
|
|||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
SimpleGraspBase::SimpleGraspBase(const std::string& name)
|
||||||
: SerialContainer(name)
|
: SerialContainer(name)
|
||||||
{
|
{
|
||||||
|
PropertyMap& p = properties();
|
||||||
|
p.declare<std::string>("eef", "end-effector to grasp with");
|
||||||
|
p.declare<std::string>("object", "object to grasp");
|
||||||
|
}
|
||||||
|
|
||||||
|
void SimpleGraspBase::setup(std::unique_ptr<Stage>&& generator, bool forward)
|
||||||
|
{
|
||||||
|
// properties provided by the grasp generator via its Interface or its PropertyMap
|
||||||
|
const std::set<std::string>& grasp_prop_names = { "object", "eef", "pregrasp", "grasp" };
|
||||||
|
|
||||||
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
int insertion_position = forward ? -1 : 0; // insert children at end / front, i.e. normal or reverse order
|
||||||
{
|
{
|
||||||
auto gengrasp = std::make_unique<GenerateGraspPose>(forward ? "generate grasp pose" : "generate release pose");
|
// forward properties from generator's to IK's solution (bottom -> up)
|
||||||
grasp_generator_ = gengrasp.get();
|
generator->setForwardedProperties(grasp_prop_names);
|
||||||
|
// allow inheritance in top -> down fashion as well
|
||||||
|
generator->properties().configureInitFrom(Stage::PARENT, { "object", "eef" });
|
||||||
|
|
||||||
auto ik = std::make_unique<ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = new ComputeIK("compute ik", std::move(generator));
|
||||||
const std::initializer_list<std::string>& grasp_prop_names = { "eef", "pregrasp", "object", "angle_delta" };
|
ik->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties
|
||||||
ik->exposePropertiesOfChild(0, grasp_prop_names);
|
|
||||||
insert(std::move(ik), insertion_position);
|
|
||||||
|
|
||||||
exposePropertiesOfChild(insertion_position, grasp_prop_names);
|
PropertyMap& p = ik->properties();
|
||||||
exposePropertiesOfChild(insertion_position, { "max_ik_solutions", "timeout", "ik_frame" });
|
p.declare<std::string>("object");
|
||||||
|
p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); // derived from child's solution
|
||||||
|
p.configureInitFrom(Stage::PARENT, {"max_ik_solutions", "timeout", "object"}); // derived from parent
|
||||||
|
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, {"eef", "ik_frame"}); // derive from both
|
||||||
|
p.exposeTo(properties(), { "max_ik_solutions", "timeout", "ik_frame" });
|
||||||
|
insert(std::unique_ptr<ComputeIK>(ik), insertion_position);
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto allow_touch = std::make_unique<ModifyPlanningScene>(forward ? "allow object collision" : "forbid object collision");
|
auto allow_touch = new ModifyPlanningScene(forward ? "allow object collision" : "forbid object collision");
|
||||||
|
allow_touch->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties
|
||||||
|
|
||||||
PropertyMap& p = allow_touch->properties();
|
PropertyMap& p = allow_touch->properties();
|
||||||
p.declare<std::string>("eef");
|
p.declare<std::string>("eef");
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" });
|
||||||
|
|
||||||
allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
allow_touch->setCallback([this](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||||
collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
|
collision_detection::AllowedCollisionMatrix& acm = scene->getAllowedCollisionMatrixNonConst();
|
||||||
@ -79,29 +95,36 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
|||||||
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
acm.setEntry(object, scene->getRobotModel()->getEndEffector(eef)
|
||||||
->getLinkModelNamesWithCollisionGeometry(), true);
|
->getLinkModelNamesWithCollisionGeometry(), true);
|
||||||
});
|
});
|
||||||
insert(std::move(allow_touch), insertion_position);
|
insert(std::unique_ptr<ModifyPlanningScene>(allow_touch), insertion_position);
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||||
pipeline->setTimeout(8.0);
|
pipeline->setTimeout(8.0);
|
||||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||||
|
|
||||||
auto move = std::make_unique<MoveTo>(forward ? "close gripper" : "open gripper", pipeline);
|
auto move = new MoveTo(forward ? "close gripper" : "open gripper", pipeline);
|
||||||
PropertyMap& p = move->properties();
|
move->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties
|
||||||
p.property("group").configureInitFrom(Stage::PARENT, [this](const PropertyMap& parent_map){
|
|
||||||
|
auto group_initializer = [this](const PropertyMap& parent_map) -> boost::any {
|
||||||
const std::string& eef = parent_map.get<std::string>("eef");
|
const std::string& eef = parent_map.get<std::string>("eef");
|
||||||
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
const moveit::core::JointModelGroup* jmg = model_->getEndEffector(eef);
|
||||||
return boost::any(jmg->getName());
|
return jmg->getName();
|
||||||
});
|
};
|
||||||
insert(std::move(move), insertion_position);
|
PropertyMap& p = move->properties();
|
||||||
exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp");
|
p.property("group").configureInitFrom(Stage::PARENT | Stage::INTERFACE, group_initializer);
|
||||||
|
p.property("goal").configureInitFrom(Stage::PARENT | Stage::INTERFACE, forward ? "grasp" : "pregrasp");
|
||||||
|
p.exposeTo(properties(), { "group", "goal" });
|
||||||
|
insert(std::unique_ptr<MoveTo>(move), insertion_position);
|
||||||
}
|
}
|
||||||
{
|
{
|
||||||
auto attach = std::make_unique<ModifyPlanningScene>(forward ? "attach object" : "detach object");
|
auto attach = new ModifyPlanningScene(forward ? "attach object" : "detach object");
|
||||||
|
attach->setForwardedProperties(grasp_prop_names); // continue forwarding generator's properties
|
||||||
|
|
||||||
PropertyMap& p = attach->properties();
|
PropertyMap& p = attach->properties();
|
||||||
p.declare<std::string>("eef");
|
p.declare<std::string>("eef");
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.configureInitFrom(Stage::PARENT, { "eef", "object" });
|
p.configureInitFrom(Stage::PARENT | Stage::INTERFACE, { "eef", "object" });
|
||||||
|
|
||||||
attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
attach->setCallback([this, forward](const planning_scene::PlanningScenePtr& scene, const PropertyMap& p){
|
||||||
const std::string& eef = p.get<std::string>("eef");
|
const std::string& eef = p.get<std::string>("eef");
|
||||||
moveit_msgs::AttachedCollisionObject obj;
|
moveit_msgs::AttachedCollisionObject obj;
|
||||||
@ -111,7 +134,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward)
|
|||||||
obj.object.id = p.get<std::string>("object");
|
obj.object.id = p.get<std::string>("object");
|
||||||
scene->processAttachedCollisionObjectMsg(obj);
|
scene->processAttachedCollisionObjectMsg(obj);
|
||||||
});
|
});
|
||||||
insert(std::move(attach), insertion_position);
|
insert(std::unique_ptr<ModifyPlanningScene>(attach), insertion_position);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -121,11 +144,6 @@ void SimpleGraspBase::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|||||||
SerialContainer::init(robot_model);
|
SerialContainer::init(robot_model);
|
||||||
}
|
}
|
||||||
|
|
||||||
void SimpleGraspBase::setMonitoredStage(Stage* monitored)
|
|
||||||
{
|
|
||||||
grasp_generator_->setMonitoredStage(monitored);
|
|
||||||
}
|
|
||||||
|
|
||||||
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string& link) {
|
||||||
geometry_msgs::PoseStamped pose_msg;
|
geometry_msgs::PoseStamped pose_msg;
|
||||||
pose_msg.header.frame_id = link;
|
pose_msg.header.frame_id = link;
|
||||||
@ -133,4 +151,15 @@ void SimpleGraspBase::setIKFrame(const Eigen::Affine3d& pose, const std::string&
|
|||||||
setIKFrame(pose_msg);
|
setIKFrame(pose_msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
SimpleGrasp::SimpleGrasp(std::unique_ptr<Stage>&& generator, const std::string& name)
|
||||||
|
: SimpleGraspBase(name)
|
||||||
|
{
|
||||||
|
setup(std::move(generator), true);
|
||||||
|
}
|
||||||
|
|
||||||
|
SimpleUnGrasp::SimpleUnGrasp(std::unique_ptr<Stage>&& generator, const std::string& name)
|
||||||
|
: SimpleGraspBase(name) {
|
||||||
|
setup(std::move(generator), false);
|
||||||
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -115,10 +115,10 @@ protected:
|
|||||||
};
|
};
|
||||||
|
|
||||||
TEST_F(InitFromTest, standard) {
|
TEST_F(InitFromTest, standard) {
|
||||||
slave.configureInitFrom(0); // init all matching vars
|
slave.configureInitFrom(1); // init all matching vars
|
||||||
ASSERT_FALSE(slave.property("double1").defined());
|
ASSERT_FALSE(slave.property("double1").defined());
|
||||||
|
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
||||||
EXPECT_EQ(slave.get<double>("double2"), 2.0);
|
EXPECT_EQ(slave.get<double>("double2"), 2.0);
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -126,8 +126,8 @@ TEST_F(InitFromTest, standard) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, limited) {
|
TEST_F(InitFromTest, limited) {
|
||||||
slave.configureInitFrom(0, {"double1"}); // limit init to listed props
|
slave.configureInitFrom(1, {"double1"}); // limit init to listed props
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
EXPECT_EQ(slave.get<double>("double1"), 1.0);
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -135,8 +135,8 @@ TEST_F(InitFromTest, limited) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, sourceId) {
|
TEST_F(InitFromTest, sourceId) {
|
||||||
slave.configureInitFrom(0); // init all matching vars
|
slave.configureInitFrom(1); // init all matching vars
|
||||||
slave.performInitFrom(1, master); // init with wrong sourceId -> no effect
|
slave.performInitFrom(2, master); // init with wrong sourceId -> no effect
|
||||||
EXPECT_FALSE(slave.property("double1").defined());
|
EXPECT_FALSE(slave.property("double1").defined());
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -144,14 +144,14 @@ TEST_F(InitFromTest, sourceId) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, multipleSourceIds) {
|
TEST_F(InitFromTest, multipleSourceIds) {
|
||||||
slave.configureInitFrom(0);
|
slave.configureInitFrom(1);
|
||||||
slave.configureInitFrom(0); // init is allowed second time with same id
|
slave.configureInitFrom(1); // init is allowed second time with same id
|
||||||
EXPECT_THROW(slave.configureInitFrom(1), std::runtime_error); // but not with other id
|
EXPECT_THROW(slave.configureInitFrom(2), std::runtime_error); // but not with other id
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, otherName) {
|
TEST_F(InitFromTest, otherName) {
|
||||||
slave.property("double1").configureInitFrom(0, "double2"); // init double1 from double2
|
slave.property("double1").configureInitFrom(1, "double2"); // init double1 from double2
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double1"), 2.0);
|
EXPECT_EQ(slave.get<double>("double1"), 2.0);
|
||||||
EXPECT_FALSE(slave.property("double2").defined());
|
EXPECT_FALSE(slave.property("double2").defined());
|
||||||
EXPECT_FALSE(slave.property("double3").defined());
|
EXPECT_FALSE(slave.property("double3").defined());
|
||||||
@ -159,9 +159,9 @@ TEST_F(InitFromTest, otherName) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(InitFromTest, function) {
|
TEST_F(InitFromTest, function) {
|
||||||
slave.property("double3").configureInitFrom(0, [](const PropertyMap& other) -> boost::any {
|
slave.property("double3").configureInitFrom(1, [](const PropertyMap& other) -> boost::any {
|
||||||
return other.get<double>("double1") + other.get<double>("double2");
|
return other.get<double>("double1") + other.get<double>("double2");
|
||||||
});
|
});
|
||||||
slave.performInitFrom(0, master);
|
slave.performInitFrom(1, master);
|
||||||
EXPECT_EQ(slave.get<double>("double3"), 3.0);
|
EXPECT_EQ(slave.get<double>("double3"), 3.0);
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user