Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

documenting instructions for the pick-and-place task #238

Merged
merged 7 commits into from
Jul 6, 2024
Merged

documenting instructions for the pick-and-place task #238

Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
2b18369
documenting instructions for the pick place task
FabianSchuetze Feb 23, 2021
162f66c
apply clang-format
FabianSchuetze Feb 23, 2021
f1d0f8b
Merge branch 'master' into illustrate_pick_place_task
FabianSchuetze Mar 1, 2021
aeb8aa4
adjust PR based on comments
FabianSchuetze Mar 3, 2021
2086b04
Merge branch 'master' into illustrate_pick_place_task
rhaschke Jul 6, 2024
b093270
Cleanup comments
rhaschke Jul 6, 2024
c83c99e
minor change
rhaschke Jul 6, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 23 additions & 19 deletions demo/src/pick_place_task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,7 @@ void PickPlaceTask::loadParameters() {
rosparam_shortcuts::shutdownIfError(LOGNAME, errors);
}

// Initialize the task pipeline, defining individual movement stages
bool PickPlaceTask::init() {
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
const std::string object = object_name_;
Expand All @@ -153,10 +154,13 @@ bool PickPlaceTask::init() {
task_.reset();
task_.reset(new moveit::task_constructor::Task());

// Individual movement stages are collected within the Task object
Task& t = *task_;
t.stages()->setName(task_name_);
t.loadRobotModel();

/* Create planners used in various stages. Various options are available,
namely Cartesian, MoveIt pipeline, and joint interpolation. */
// Sampling planner
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
Expand Down Expand Up @@ -201,7 +205,7 @@ bool PickPlaceTask::init() {
* *
***************************************************/
Stage* initial_state_ptr = nullptr;
{ // Open Hand
{
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
stage->setGroup(hand_group_name_);
stage->setGoal(hand_open_pose_);
Expand All @@ -214,7 +218,8 @@ bool PickPlaceTask::init() {
* Move to Pick *
* *
***************************************************/
{ // Move-to pre-grasp
// Connect initial open-hand state with pre-grasp pose defined in the following
{
auto stage = std::make_unique<stages::Connect>(
"move to pick", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
Expand All @@ -229,6 +234,7 @@ bool PickPlaceTask::init() {
***************************************************/
Stage* pick_stage_ptr = nullptr;
{
// A SerialContainer combines several sub-stages, here for picking the object
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" });
Expand All @@ -237,10 +243,11 @@ bool PickPlaceTask::init() {
---- * Approach Object *
***************************************************/
{
// Move the eef link forward along its z-axis by an amount within the given min-max range
auto stage = std::make_unique<stages::MoveRelative>("approach object", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", hand_frame_);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->properties().set("link", hand_frame_); // link to perform IK for
stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage
stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_);

// Set hand forward direction
Expand All @@ -255,29 +262,31 @@ bool PickPlaceTask::init() {
---- * Generate Grasp Pose *
***************************************************/
{
// Sample grasp pose
// Sample grasp pose candidates in angle increments around the z-axis of the object
auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
stage->properties().configureInitFrom(Stage::PARENT);
stage->properties().set("marker_ns", "grasp_pose");
stage->setPreGraspPose(hand_open_pose_);
stage->setObject(object);
stage->setObject(object); // object to sample grasps for
stage->setAngleDelta(M_PI / 12);
stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions

// Compute IK
// Compute IK for sampled grasp poses
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
wrapper->setMaxIKSolutions(8);
wrapper->setMaxIKSolutions(8); // limit number of solutions
wrapper->setMinSolutionDistance(1.0);
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" });
wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" });
wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); // define virtual frame to reach the target_pose
wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent
wrapper->properties().configureInitFrom(Stage::INTERFACE,
{ "target_pose" }); // inherit property from child solution
grasp->insert(std::move(wrapper));
}

/****************************************************
---- * Allow Collision (hand object) *
***************************************************/
{
// Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (hand,object)");
stage->allowCollisions(
object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
Expand All @@ -300,7 +309,7 @@ bool PickPlaceTask::init() {
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
stage->attachObject(object, hand_frame_);
stage->attachObject(object, hand_frame_); // attach object to hand_frame_
grasp->insert(std::move(stage));
}

Expand Down Expand Up @@ -352,6 +361,7 @@ bool PickPlaceTask::init() {
* *
*****************************************************/
{
// Connect the grasped state to the pre-place state, i.e. realize the object transport
auto stage = std::make_unique<stages::Connect>(
"move to place", stages::Connect::GroupPlannerVector{ { arm_group_name_, sampling_planner } });
stage->setTimeout(5.0);
Expand All @@ -364,6 +374,7 @@ bool PickPlaceTask::init() {
* Place Object *
* *
*****************************************************/
// All placing sub-stages are collected within a serial container again
{
auto place = std::make_unique<SerialContainer>("place object");
t.properties().exposeTo(place->properties(), { "eef", "hand", "group" });
Expand Down Expand Up @@ -498,13 +509,6 @@ bool PickPlaceTask::execute() {
moveit_msgs::MoveItErrorCodes execute_result;

execute_result = task_->execute(*task_->solutions().front());
// // If you want to inspect the goal message, use this instead:
// actionlib::SimpleActionClient<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>
// execute("execute_task_solution", true); execute.waitForServer();
// moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal;
// task_->solutions().front()->toMsg(execute_goal.solution);
// execute.sendGoalAndWait(execute_goal);
// 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_result.val);
Expand Down
Loading