mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Add parameter struct
This commit is contained in:
parent
916d26db25
commit
7f3bdaa607
@ -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() {
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
|
Loading…
Reference in New Issue
Block a user