Add parameter struct

This commit is contained in:
Henning Kayser 2019-11-20 17:32:48 +01:00
parent 916d26db25
commit 7f3bdaa607
3 changed files with 338 additions and 392 deletions

View File

@ -51,17 +51,21 @@ void PlanPickPlaceCapability::initialize() {
std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false)); std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this)); as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
as_->start(); as_->start();
ros::NodeHandle nh("~"); pick_place_task_ = std::make_unique<PickPlaceTask>("pick_place_task");
pick_place_task_ = std::make_unique<PickPlaceTask>("", nh);
} }
void PlanPickPlaceCapability::goalCallback( void PlanPickPlaceCapability::goalCallback(
const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) { const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
moveit_task_constructor_msgs::PlanPickPlaceResult result; moveit_task_constructor_msgs::PlanPickPlaceResult result;
// TODO: fill parameters
PickPlaceTask::Parameters parameters;
// initialize task // initialize task
pick_place_task_->init(parameters);
// run plan // run plan
// return result pick_place_task_->plan();
// retrieve and return result
} }
void PlanPickPlaceCapability::preemptCallback() { void PlanPickPlaceCapability::preemptCallback() {

View File

@ -73,55 +73,54 @@ using namespace moveit::task_constructor;
class PickPlaceTask class PickPlaceTask
{ {
public: public:
PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh); struct Parameters
{
// planning group properties
std::string arm_group_name_;
std::string eef_name_;
std::string hand_group_name_;
std::string hand_frame_;
// object + surface
std::vector<std::string> support_surfaces_;
std::string object_reference_frame_;
std::string surface_link_;
std::string object_name_;
std::string world_frame_;
std::string grasp_frame_;
std::vector<double> object_dimensions_;
// Predefined pose targets
std::string hand_open_pose_;
std::string hand_close_pose_;
std::string arm_home_pose_;
// Pick metrics
Eigen::Isometry3d grasp_frame_transform_;
double approach_object_min_dist_;
double approach_object_max_dist_;
double lift_object_min_dist_;
double lift_object_max_dist_;
double place_object_min_dist_;
double place_object_max_dist_;
double retreat_object_min_dist_;
double retreat_object_max_dist_;
// Place metrics
geometry_msgs::PoseStamped place_pose_;
double place_surface_offset_;
};
PickPlaceTask(const std::string& task_name);
~PickPlaceTask() = default; ~PickPlaceTask() = default;
void loadParameters(); void init(const Parameters& parameters);
void init();
bool plan(); bool plan();
bool execute();
private: private:
ros::NodeHandle nh_;
std::string task_name_;
moveit::task_constructor::TaskPtr task_; moveit::task_constructor::TaskPtr task_;
const std::string task_name_;
// planning group properties
std::string arm_group_name_;
std::string eef_name_;
std::string hand_group_name_;
std::string hand_frame_;
// object + surface
std::vector<std::string> support_surfaces_;
std::string object_reference_frame_;
std::string surface_link_;
std::string object_name_;
std::string world_frame_;
std::vector<double> object_dimensions_;
// Predefined pose targets
std::string hand_open_pose_;
std::string hand_close_pose_;
std::string arm_home_pose_;
// Execution
actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction> execute_;
// Pick metrics
Eigen::Isometry3d grasp_frame_transform_;
double approach_object_min_dist_;
double approach_object_max_dist_;
double lift_object_min_dist_;
double lift_object_max_dist_;
// Place metrics
geometry_msgs::Pose place_pose_;
double place_surface_offset_;
}; };
} // namespace tasks } // namespace tasks

View File

@ -42,390 +42,349 @@ namespace task_constructor {
namespace tasks { namespace tasks {
constexpr char LOGNAME[] = "pick_place_task"; constexpr char LOGNAME[] = "pick_place_task";
PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& nh) PickPlaceTask::PickPlaceTask(const std::string& task_name)
: nh_(nh), task_name_(task_name), execute_("execute_task_solution", true) {} : task_name_(task_name) {}
void PickPlaceTask::loadParameters() { void PickPlaceTask::init(const Parameters& parameters)
/**************************************************** {
* * ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
* Load Parameters * // Reset ROS introspection before constructing the new object
* * // TODO(henningkayser): verify this is a bug, fix if possible
***************************************************/ task_.reset();
ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); task_.reset(new moveit::task_constructor::Task(task_name_));
ros::NodeHandle pnh("~"); Task& t = *task_;
t.loadRobotModel();
// Planning group properties // Sampling planner
size_t errors = 0; // TODO(henningkayser): Setup and parameterize alternative planners
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_group_name", arm_group_name_); auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_group_name", hand_group_name_); sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "eef_name", eef_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_frame", hand_frame_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "world_frame", world_frame_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "grasp_frame_transform", grasp_frame_transform_);
// Predefined pose targets // Cartesian planner
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_open_pose", hand_open_pose_); auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "hand_close_pose", hand_close_pose_); cartesian_planner->setMaxVelocityScaling(1.0);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "arm_home_pose", arm_home_pose_); cartesian_planner->setMaxAccelerationScaling(1.0);
cartesian_planner->setStepSize(.01);
// Target object // Set task properties
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name_); t.setProperty("group", parameters.arm_group_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions_); t.setProperty("eef", parameters.eef_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame_); t.setProperty("hand", parameters.hand_group_name_);
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "surface_link", surface_link_); t.setProperty("ik_frame", parameters.hand_frame_);
support_surfaces_ = { surface_link_ };
// Pick/Place metrics /****************************************************
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_min_dist", approach_object_min_dist_); * *
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "approach_object_max_dist", approach_object_max_dist_); * Current State *
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_min_dist", lift_object_min_dist_); * *
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "lift_object_max_dist", lift_object_max_dist_); ***************************************************/
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_surface_offset", place_surface_offset_); Stage* current_state = nullptr; // Forward current_state on to grasp pose generator
errors += !rosparam_shortcuts::get(LOGNAME, pnh, "place_pose", place_pose_); {
rosparam_shortcuts::shutdownIfError(LOGNAME, errors); auto _current_state = std::make_unique<stages::CurrentState>("current state");
} _current_state->setTimeout(10);
void PickPlaceTask::init() { // Verify that object is not attachd
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); auto applicability_filter =
const std::string object = "object"; std::make_unique<stages::PredicateFilter>("applicability test", std::move(_current_state));
applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) {
s.start()->scene()->printKnownObjects(std::cout);
if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_))
{
comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked";
return false;
}
return true;
});
// Reset ROS introspection before constructing the new object current_state = applicability_filter.get();
// TODO(henningkayser): verify this is a bug, fix if possible t.add(std::move(applicability_filter));
task_.reset(); }
task_.reset(new moveit::task_constructor::Task());
Task& t = *task_; /****************************************************
t.stages()->setName(task_name_); * *
t.loadRobotModel(); * Open Hand *
* *
***************************************************/
{ // Open Hand
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(parameters.hand_group_name_);
stage->setGoal(parameters.hand_open_pose_);
t.add(std::move(stage));
}
// Sampling planner /****************************************************
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>(); * *
sampling_planner->setProperty("goal_joint_tolerance", 1e-5); * Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
auto stage = std::make_unique<stages::Connect>(
"move to pick", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(stage));
}
// Cartesian planner /****************************************************
auto cartesian_planner = std::make_shared<solvers::CartesianPath>(); * *
cartesian_planner->setMaxVelocityScaling(1.0); * Pick Object *
cartesian_planner->setMaxAccelerationScaling(1.0); * *
cartesian_planner->setStepSize(.01); ***************************************************/
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
// Set task properties /****************************************************
t.setProperty("group", arm_group_name_);
t.setProperty("eef", eef_name_);
t.setProperty("hand", hand_group_name_);
t.setProperty("hand_grasping_frame", hand_frame_);
t.setProperty("ik_frame", hand_frame_);
/****************************************************
* *
* Current State *
* *
***************************************************/
Stage* current_state = nullptr; // Forward current_state on to grasp pose generator
{
auto _current_state = std::make_unique<stages::CurrentState>("current state");
// Verify that object is not attachd
auto applicability_filter =
std::make_unique<stages::PredicateFilter>("applicability test", std::move(_current_state));
applicability_filter->setPredicate([object](const SolutionBase& s, std::string& comment) {
if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) {
comment = "object with id '" + object + "' is already attached and cannot be picked";
return false;
}
return true;
});
current_state = applicability_filter.get();
t.add(std::move(applicability_filter));
}
/****************************************************
* *
* Open Hand *
* *
***************************************************/
{ // Open Hand
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(hand_group_name_);
stage->setGoal(hand_open_pose_);
t.add(std::move(stage));
}
/****************************************************
* *
* Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
auto stage = std::make_unique<stages::Connect>(
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(stage));
}
/****************************************************
* *
* Pick Object *
* *
***************************************************/
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
{
auto grasp = std::make_unique<SerialContainer>("pick object");
t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });
/****************************************************
---- * Approach Object * ---- * Approach Object *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner); auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object"); stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", hand_frame_); stage->properties().set("link", parameters.hand_frame_);
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_);
// Set hand forward direction // Set hand forward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = hand_frame_; vec.header.frame_id = parameters.object_name_;
vec.vector.z = 1.0; vec.vector.z = -1.0;
stage->setDirection(vec); stage->setDirection(vec);
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
/**************************************************** /****************************************************
---- * Generate Grasp Pose * ---- * Generate Grasp Pose *
***************************************************/ ***************************************************/
{ {
// Sample grasp pose // Sample grasp pose
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose"); auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(Stage::PARENT); stage->properties().configureInitFrom(Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose"); stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose(hand_open_pose_); stage->setPreGraspPose(parameters.hand_open_pose_);
stage->setObject(object); stage->setObject(parameters.object_name_);
stage->setAngleDelta(M_PI / 12); stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(current_state); // Hook into current state stage->setMonitoredStage(current_state); // Hook into current state
// Compute IK // Compute IK
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage)); auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8); wrapper->setMaxIKSolutions(2);
wrapper->setMinSolutionDistance(1.0); wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); //wrapper->setIgnoreCollisions(true);
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
grasp->insert(std::move(wrapper)); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
} grasp->insert(std::move(wrapper));
}
/**************************************************** /****************************************************
---- * Allow Collision (hand object) * ---- * Allow Collision (hand object) *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)"); auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions( stage->allowCollisions(
object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), parameters.object_name_,
true); t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
grasp->insert(std::move(stage)); true);
} grasp->insert(std::move(stage));
}
/**************************************************** /****************************************************
---- * Close Hand * ---- * Close Hand *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner); auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
stage->setGoal(hand_close_pose_); stage->setGoal(parameters.hand_close_pose_);
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
/**************************************************** /****************************************************
.... * Attach Object * .... * Attach Object *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object"); auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, hand_frame_); stage->attachObject(parameters.object_name_, parameters.hand_frame_);
attach_object_stage = stage.get(); attach_object_stage = stage.get();
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
/**************************************************** /****************************************************
.... * Allow collision (object support) * .... * Allow collision (object support) *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)"); auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
stage->allowCollisions({ object }, support_surfaces_, true); stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
/**************************************************** /****************************************************
.... * Lift object * .... * Lift object *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner); auto stage = std::make_unique<stages::MoveRelative>("lift object", cartesian_planner);
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(lift_object_min_dist_, lift_object_max_dist_); stage->setMinMaxDistance(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_);
stage->setIKFrame(hand_frame_); stage->setIKFrame(parameters.hand_frame_);
stage->properties().set("marker_ns", "lift_object"); stage->properties().set("marker_ns", "lift_object");
// Set upward direction // Set upward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = world_frame_; vec.header.frame_id = parameters.world_frame_;
vec.vector.z = 1.0; vec.vector.z = 1.0;
stage->setDirection(vec); stage->setDirection(vec);
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
/**************************************************** /****************************************************
.... * Forbid collision (object support) * .... * Forbid collision (object support) *
***************************************************/ ***************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)"); auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (object,surface)");
stage->allowCollisions({ object }, support_surfaces_, false); stage->allowCollisions({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize
grasp->insert(std::move(stage)); grasp->insert(std::move(stage));
} }
// Add grasp container to task // Add grasp container to task
t.add(std::move(grasp)); t.add(std::move(grasp));
} }
/****************************************************** /******************************************************
* * * *
* Move to Place * * Move to Place *
* * * *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::Connect>( auto stage = std::make_unique<stages::Connect>(
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } }); "move to place", stages::Connect::GroupPlannerVector{ { parameters.arm_group_name_, sampling_planner } });
stage->setTimeout(5.0); stage->setTimeout(5.0);
stage->properties().configureInitFrom(Stage::PARENT); stage->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(stage)); t.add(std::move(stage));
} }
/****************************************************** /******************************************************
* * * *
* Place Object * * Place Object *
* * * *
*****************************************************/ *****************************************************/
{ {
auto place = std::make_unique<SerialContainer>("place object"); auto place = std::make_unique<SerialContainer>("place object");
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" });
/****************************************************** /******************************************************
---- * Lower Object * ---- * Lower Object *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner); auto stage = std::make_unique<stages::MoveRelative>("lower object", cartesian_planner);
stage->properties().set("marker_ns", "lower_object"); stage->properties().set("marker_ns", "lower_object");
stage->properties().set("link", hand_frame_); stage->properties().set("link", parameters.hand_frame_);
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setMinMaxDistance(.03, .13); stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_);
// Set downward direction // Set downward direction
geometry_msgs::Vector3Stamped vec; geometry_msgs::Vector3Stamped vec;
vec.header.frame_id = world_frame_; vec.header.frame_id = parameters.world_frame_;
vec.vector.z = -1.0; vec.vector.z = -1.0;
stage->setDirection(vec); stage->setDirection(vec);
place->insert(std::move(stage)); place->insert(std::move(stage));
} }
/****************************************************** /******************************************************
---- * Generate Place Pose * ---- * Generate Place Pose *
*****************************************************/ *****************************************************/
{ {
// Generate Place Pose // Generate Place Pose
auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose"); auto stage = std::make_unique<stages::GeneratePlacePose>("generate place pose");
stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" });
stage->properties().set("marker_ns", "place_pose"); stage->properties().set("marker_ns", "place_pose");
stage->setObject(object); stage->setObject(parameters.object_name_);
stage->setPose(parameters.place_pose_);
// Hook into attach_object_stage which allows us to use the attached object as IK frame
stage->setMonitoredStage(attach_object_stage);
// Set target pose // Compute IK
geometry_msgs::PoseStamped p; auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
p.header.frame_id = object_reference_frame_; wrapper->setMaxIKSolutions(2);
p.pose = place_pose_; wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; // TODO(henningkayser): Enable collisions
stage->setPose(p); wrapper->setIgnoreCollisions(true);
stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
// Compute IK /******************************************************
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
wrapper->setMaxIKSolutions(2);
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
place->insert(std::move(wrapper));
}
/******************************************************
---- * Open Hand * ---- * Open Hand *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner); auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->properties().property("group").configureInitFrom(Stage::PARENT, hand_group_name_); stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
stage->setGoal(hand_open_pose_); stage->setGoal(parameters.hand_open_pose_);
place->insert(std::move(stage)); place->insert(std::move(stage));
} }
/****************************************************** /******************************************************
---- * Forbid collision (hand, object) * ---- * Forbid collision (hand, object) *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)"); // TODO(henningkayser): Forbid collision after retreat?
stage->allowCollisions( auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
object_name_, stage->allowCollisions(
t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), false); parameters.object_name_,
place->insert(std::move(stage)); t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
} false);
place->insert(std::move(stage));
}
/****************************************************** /******************************************************
---- * Detach Object * ---- * Detach Object *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object"); auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
stage->detachObject(object_name_, hand_frame_); stage->detachObject(parameters.object_name_, parameters.hand_frame_);
place->insert(std::move(stage)); place->insert(std::move(stage));
} }
/****************************************************** /******************************************************
---- * Retreat Motion * ---- * Retreat Motion *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner); // TODO(henningkayser): Do we need this if items are dropped?
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
stage->setMinMaxDistance(.12, .25); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setIKFrame(hand_frame_); stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_);
stage->properties().set("marker_ns", "retreat"); stage->setIKFrame(parameters.hand_frame_);
geometry_msgs::Vector3Stamped vec; stage->properties().set("marker_ns", "retreat");
vec.header.frame_id = hand_frame_; geometry_msgs::Vector3Stamped vec;
vec.vector.z = -1.0; vec.header.frame_id = parameters.hand_frame_;
stage->setDirection(vec); vec.vector.z = -1.0;
place->insert(std::move(stage)); stage->setDirection(vec);
} place->insert(std::move(stage));
}
// Add place container to task // Add place container to task
t.add(std::move(place)); t.add(std::move(place));
} }
/****************************************************** /******************************************************
* * * *
* Move to Home * * Move to Home *
* * * *
*****************************************************/ *****************************************************/
{ {
auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner); auto stage = std::make_unique<stages::MoveTo>("move home", sampling_planner);
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setGoal(arm_home_pose_); stage->setGoal(parameters.arm_home_pose_);
stage->restrictDirection(stages::MoveTo::FORWARD); stage->restrictDirection(stages::MoveTo::FORWARD);
t.add(std::move(stage)); t.add(std::move(stage));
} }
} }
bool PickPlaceTask::plan() { bool PickPlaceTask::plan() {
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
ros::NodeHandle pnh("~");
int planning_attempts = pnh.param<int>("planning_attempts", 10);
try { try {
task_->plan(planning_attempts); task_->plan(10); // TODO: parameterize
} catch (InitStageException& e) { } catch (InitStageException& e) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
return false; return false;
@ -437,22 +396,6 @@ bool PickPlaceTask::plan() {
return true; return true;
} }
bool PickPlaceTask::execute() {
ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory");
moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
task_->solutions().front()->fillMessage(execute_goal.solution);
execute_.sendGoal(execute_goal);
execute_.waitForResult();
moveit_msgs::MoveItErrorCodes execute_result = execute_.getResult()->error_code;
if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_.getState().toString());
return false;
}
return true;
}
} // namespace tasks } // namespace tasks
} // namespace task_constructor } // namespace task_constructor
} // namespace moveit } // namespace moveit