From 4514ed5d937481d26bf819b5c235584f9f8f310a Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Mon, 19 Jul 2021 18:46:17 +0200 Subject: [PATCH 1/6] add goal tolerance examples --- .../move_group_interface_tutorial.rst | 6 ++- .../src/move_group_interface_tutorial.cpp | 50 +++++++++++++++++++ 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 9227c6567..508fd5e2b 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -38,8 +38,10 @@ See the `YouTube video `_ at the top of this tutor 6. The robot moves its arm to the pose goal, avoiding collision with the box. 7. The object is attached to the wrist (its color will change to purple/orange/green). - 8. The object is detached from the wrist (its color will change back to green). - 9. The object is removed from the environment. + 8. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box. + 9. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box. + 10. The object is detached from the wrist (its color will change back to green). + 11. The object is removed from the environment. .. |B| image:: ./move_group_interface_tutorial_robot_with_box.png diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 6d81543b4..39a046670 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -452,6 +452,56 @@ int main(int argc, char** argv) // .. image:: ./move_group_interface_tutorial_attached_object.gif // :alt: animation showing the arm moving differently once the object is attached // + + // Moving to a goal pose with orientation tolerances + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // + // Show text in RViz of status + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Goal with orientation tolerance", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + // Let's plan to a goal colliding on the top of the collision object. + move_group_interface.setStartState(*move_group_interface.getCurrentState()); + geometry_msgs::Pose pose_above_collision_object; + pose_above_collision_object.orientation.x = 1.0; + pose_above_collision_object.position.x = 0.45; + pose_above_collision_object.position.y = 0.0; + pose_above_collision_object.position.z = 0.59; + move_group_interface.setPoseTarget(pose_above_collision_object); + + std::vector goal_orientation_tolerance_xyz = { 1e-3, M_PI / 2, M_PI / 4 }; + move_group_interface.setGoalOrientationToleranceXYZ(goal_orientation_tolerance_xyz); + + success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + ROS_INFO_NAMED("tutorial", "Visualizing plan 8 (Pose goal with orientation tolerances avoiding collision) %s", + success ? "" : "FAILED"); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + + // Moving to a goal pose with position tolerances + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // + // Show text in RViz of status + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Goal with position tolerance", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + // Let's plan to a goal colliding on the top of the collision object. + move_group_interface.setStartState(*move_group_interface.getCurrentState()); + move_group_interface.setPoseTarget(pose_above_collision_object); + + goal_orientation_tolerance_xyz = { 1e-3, 1e-3, 1e-3 }; + move_group_interface.setGoalOrientationToleranceXYZ(goal_orientation_tolerance_xyz); + std::vector goal_position_tolerance_xyz = { 1e-4, 1e-4, 0.2 }; + move_group_interface.setGoalPositionToleranceXYZ(goal_position_tolerance_xyz); + + success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + ROS_INFO_NAMED("tutorial", "Visualizing plan 9 (Pose goal with position tolerances avoiding collision) %s", + success ? "" : "FAILED"); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); + // Detaching and Removing Objects // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // From 3da40237880edc1b729bdfb62515fa0e0c42aa1a Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Mon, 16 Aug 2021 17:40:02 +0200 Subject: [PATCH 2/6] detach object and correct step numbers --- .../move_group_interface_tutorial.rst | 11 ++++--- .../src/move_group_interface_tutorial.cpp | 32 +++++++++++-------- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 508fd5e2b..167741557 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -33,15 +33,16 @@ See the `YouTube video `_ at the top of this tutor 2. The robot moves its arm to the joint goal at its side. 3. The robot moves its arm back to a new pose goal while maintaining the end-effector level. 4. The robot moves its arm along the desired Cartesian path (a triangle down, right, up+left). - 5. A box object is added into the environment to the right of the arm. + - A box object is added into the environment to the right of the arm. |B| - + 5. The robot moves its arm to a new pose goal. 6. The robot moves its arm to the pose goal, avoiding collision with the box. - 7. The object is attached to the wrist (its color will change to purple/orange/green). + - The object is attached to the wrist (its color will change to purple/orange/green). + 7. The robot moves its arm to the pose goal, avoiding collision between the cylinder and the box box. + - The object is detached from the wrist (its color will change back to green). 8. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box. 9. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box. - 10. The object is detached from the wrist (its color will change back to green). - 11. The object is removed from the environment. + - The objects are removed from the environment. .. |B| image:: ./move_group_interface_tutorial_robot_with_box.png diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 39a046670..e7b4bf213 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -453,6 +453,21 @@ int main(int argc, char** argv) // :alt: animation showing the arm moving differently once the object is attached // + // Detaching Objects + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // + // Now, let's detach the cylinder from the robot's gripper. + ROS_INFO_NAMED("tutorial", "Detach the object from the robot"); + move_group_interface.detachObject(object_to_attach.id); + + // Show text in RViz of status + visual_tools.deleteAllMarkers(); + visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); + visual_tools.trigger(); + + /* Wait for MoveGroup to receive and process the attached collision object message */ + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); + // Moving to a goal pose with orientation tolerances // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // @@ -502,21 +517,9 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - // Detaching and Removing Objects + // Removing Objects // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // - // Now, let's detach the cylinder from the robot's gripper. - ROS_INFO_NAMED("tutorial", "Detach the object from the robot"); - move_group_interface.detachObject(object_to_attach.id); - - // Show text in RViz of status - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); - // Now, let's remove the objects from the world. ROS_INFO_NAMED("tutorial", "Remove the objects from the world"); std::vector object_ids; @@ -525,11 +528,12 @@ int main(int argc, char** argv) planning_scene_interface.removeCollisionObjects(object_ids); // Show text in RViz of status + visual_tools.deleteAllMarkers(); visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears"); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to shutdown the demo node"); // END_TUTORIAL From f4b7ef564358d3025c3f7440e1844dfb36fc745f Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Tue, 21 Sep 2021 14:59:49 +0200 Subject: [PATCH 3/6] update Youtube video link --- doc/move_group_interface/move_group_interface_tutorial.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 167741557..3eb2712b5 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -6,7 +6,7 @@ Move Group C++ Interface In MoveIt, the simplest user interface is through the :planning_interface:`MoveGroupInterface` class. It provides easy to use functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot. This interface communicates over ROS topics, services, and actions to the `MoveGroup Node `_. -Watch this quick `YouTube video demo `_ to see the power of the move group interface! +Watch this quick `YouTube video demo `_ to see the power of the move group interface! Getting Started --------------- @@ -28,7 +28,7 @@ After a short moment, the RViz window should appear and look similar to the one Expected Output --------------- -See the `YouTube video `_ at the top of this tutorial for expected output. In RViz, we should be able to see the following: +See the `YouTube video `_ at the top of this tutorial for expected output. In RViz, we should be able to see the following: 1. The robot moves its arm to the pose goal to its front. 2. The robot moves its arm to the joint goal at its side. 3. The robot moves its arm back to a new pose goal while maintaining the end-effector level. From bb99c71bee57e9f2a0d333a6140be07a2ee1bfcc Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Tue, 5 Oct 2021 09:45:34 +0200 Subject: [PATCH 4/6] solve title warning --- .../src/move_group_interface_tutorial.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index e7b4bf213..abef39a34 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -469,7 +469,7 @@ int main(int argc, char** argv) visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); // Moving to a goal pose with orientation tolerances - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Show text in RViz of status visual_tools.deleteAllMarkers(); @@ -495,7 +495,7 @@ int main(int argc, char** argv) visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); // Moving to a goal pose with position tolerances - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Show text in RViz of status visual_tools.deleteAllMarkers(); From f9383614193beac9f3dd76e61ddd0c6208670e41 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Wed, 6 Oct 2021 15:49:58 +0200 Subject: [PATCH 5/6] solve list error --- .../move_group_interface_tutorial.rst | 22 ++++---- .../src/move_group_interface_tutorial.cpp | 51 +++++++++++-------- 2 files changed, 41 insertions(+), 32 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 3eb2712b5..795460739 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -29,20 +29,20 @@ After a short moment, the RViz window should appear and look similar to the one Expected Output --------------- See the `YouTube video `_ at the top of this tutorial for expected output. In RViz, we should be able to see the following: - 1. The robot moves its arm to the pose goal to its front. + 1. The robot moves its arm to the pose goal. 2. The robot moves its arm to the joint goal at its side. - 3. The robot moves its arm back to a new pose goal while maintaining the end-effector level. + 3. The robot moves its arm to a new pose goal while maintaining the end-effector level. 4. The robot moves its arm along the desired Cartesian path (a triangle down, right, up+left). - - A box object is added into the environment to the right of the arm. + 5. The robot moves its arm to a new pose goal to its front. + 6. A box object is added into the environment on the previous way. |B| - 5. The robot moves its arm to a new pose goal. - 6. The robot moves its arm to the pose goal, avoiding collision with the box. - - The object is attached to the wrist (its color will change to purple/orange/green). - 7. The robot moves its arm to the pose goal, avoiding collision between the cylinder and the box box. - - The object is detached from the wrist (its color will change back to green). - 8. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box. - 9. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box. - - The objects are removed from the environment. + 7. The robot moves its arm to the pose goal, avoiding collision with the box. + 8 The object is attached to the wrist (its color will change to purple/orange/green). + 9. The robot moves its arm to the pose goal, avoiding collision between the cylinder and the box box. + 10 The object is detached from the wrist (its color will change back to green). + 11. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box. + 12. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box. + 13 The objects are removed from the environment. .. |B| image:: ./move_group_interface_tutorial_robot_with_box.png diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index abef39a34..5b12b004b 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -156,18 +156,18 @@ int main(int argc, char** argv) // Note that this can lead to problems if the robot moved in the meanwhile. // move_group_interface.execute(my_plan); - // Moving to a pose goal - // ^^^^^^^^^^^^^^^^^^^^^ + // 1. Moving to a pose goal + // ^^^^^^^^^^^^^^^^^^^^^^^^ // // If you do not want to inspect the planned trajectory, // the following is a more robust combination of the two-step plan+execute pattern shown above // and should be preferred. Note that the pose goal we had set earlier is still active, // so the robot will try to move to that goal. - // move_group_interface.move(); + move_group_interface.move(); - // Planning to a joint-space goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 2. Planning to a joint-space goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Let's set a joint space goal and move towards it. This will replace the // pose target we set above. @@ -201,8 +201,8 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - // Planning with Path Constraints - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 3. Planning with Path Constraints + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Path constraints can easily be specified for a link on the robot. // Let's specify a path constraint and a pose goal for our group. @@ -273,8 +273,8 @@ int main(int argc, char** argv) // When done with the path constraint be sure to clear it. move_group_interface.clearPathConstraints(); - // Cartesian Paths - // ^^^^^^^^^^^^^^^ + // 4. Cartesian Paths + // ^^^^^^^^^^^^^^^^^^ // You can plan a Cartesian path directly by specifying a list of waypoints // for the end-effector to go through. Note that we are starting // from the new start state above. The initial pose (start state) does not @@ -323,8 +323,8 @@ int main(int argc, char** argv) // You can execute a trajectory like this. move_group_interface.execute(trajectory); - // Adding objects to the environment - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 5. Move without obstacle + // ^^^^^^^^^^^^^^^^^^^^^^^^ // // First let's plan to another simple goal with no objects in the way. move_group_interface.setStartState(*move_group_interface.getCurrentState()); @@ -349,6 +349,9 @@ int main(int argc, char** argv) // .. image:: ./move_group_interface_tutorial_clear_path.gif // :alt: animation showing the arm moving relatively straight toward the goal // + // 6. Adding objects to the environment + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // Now let's define a collision object ROS message for the robot to avoid. moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = move_group_interface.getPlanningFrame(); @@ -388,6 +391,9 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); + // 7. Move avoiding obstacle + // ^^^^^^^^^^^^^^^^^^^^^^^^^ + // // Now when we plan a trajectory it will avoid the obstacle success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); @@ -401,8 +407,8 @@ int main(int argc, char** argv) // .. image:: ./move_group_interface_tutorial_avoid_path.gif // :alt: animation showing the arm moving avoiding the new obstacle // - // Attaching objects to the robot - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 8. Attaching objects to the robot + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // You can attach objects to the robot, so that it moves with the robot geometry. // This simulates picking up the object for the purpose of manipulating it. @@ -439,6 +445,9 @@ int main(int argc, char** argv) /* Wait for MoveGroup to receive and process the attached collision object message */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); + // 9. Move with attached object avoiding obstacle + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // // Replan, but now with the object in hand. move_group_interface.setStartStateToCurrentState(); success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); @@ -453,8 +462,8 @@ int main(int argc, char** argv) // :alt: animation showing the arm moving differently once the object is attached // - // Detaching Objects - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 10. Detaching Objects + // ^^^^^^^^^^^^^^^^^^^^^ // // Now, let's detach the cylinder from the robot's gripper. ROS_INFO_NAMED("tutorial", "Detach the object from the robot"); @@ -468,8 +477,8 @@ int main(int argc, char** argv) /* Wait for MoveGroup to receive and process the attached collision object message */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); - // Moving to a goal pose with orientation tolerances - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 11. Moving to a goal pose with orientation tolerances + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Show text in RViz of status visual_tools.deleteAllMarkers(); @@ -494,8 +503,8 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - // Moving to a goal pose with position tolerances - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 12. Moving to a goal pose with position tolerances + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // // Show text in RViz of status visual_tools.deleteAllMarkers(); @@ -517,8 +526,8 @@ int main(int argc, char** argv) visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - // Removing Objects - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // 13. Removing Objects + // ^^^^^^^^^^^^^^^^^^^^ // // Now, let's remove the objects from the world. ROS_INFO_NAMED("tutorial", "Remove the objects from the world"); From 78d2545697e9d81e0bc0b69e5e48e5633930fae2 Mon Sep 17 00:00:00 2001 From: Gauthier Hentz Date: Thu, 7 Oct 2021 10:13:55 +0200 Subject: [PATCH 6/6] correct formatting --- .../move_group_interface_tutorial.rst | 6 +-- .../src/move_group_interface_tutorial.cpp | 40 +++++++++---------- 2 files changed, 23 insertions(+), 23 deletions(-) diff --git a/doc/move_group_interface/move_group_interface_tutorial.rst b/doc/move_group_interface/move_group_interface_tutorial.rst index 795460739..7e5690e4a 100644 --- a/doc/move_group_interface/move_group_interface_tutorial.rst +++ b/doc/move_group_interface/move_group_interface_tutorial.rst @@ -37,12 +37,12 @@ See the `YouTube video `_ at the top of this tutor 6. A box object is added into the environment on the previous way. |B| 7. The robot moves its arm to the pose goal, avoiding collision with the box. - 8 The object is attached to the wrist (its color will change to purple/orange/green). + 8. The object is attached to the wrist (its color will change to purple/orange/green). 9. The robot moves its arm to the pose goal, avoiding collision between the cylinder and the box box. - 10 The object is detached from the wrist (its color will change back to green). + 10. The object is detached from the wrist (its color will change back to green). 11. The robot moves its arm to a pose goal with orientation tolerance, avoiding collision with the box. 12. The robot moves its arm to a pose goal with position tolerance, avoiding collision with the box. - 13 The objects are removed from the environment. + 13. The objects are removed from the environment. .. |B| image:: ./move_group_interface_tutorial_robot_with_box.png diff --git a/doc/move_group_interface/src/move_group_interface_tutorial.cpp b/doc/move_group_interface/src/move_group_interface_tutorial.cpp index 5b12b004b..d42bd3f16 100644 --- a/doc/move_group_interface/src/move_group_interface_tutorial.cpp +++ b/doc/move_group_interface/src/move_group_interface_tutorial.cpp @@ -122,8 +122,8 @@ int main(int argc, char** argv) // .. _move_group_interface-planning-to-pose-goal: // - // Planning to a Pose goal - // ^^^^^^^^^^^^^^^^^^^^^^^ + // 1. Planning to a Pose goal + // ^^^^^^^^^^^^^^^^^^^^^^^^^^ // We can plan a motion for this group to a desired pose for the // end-effector. geometry_msgs::Pose target_pose1; @@ -153,19 +153,19 @@ int main(int argc, char** argv) visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // Finally, to execute the trajectory stored in my_plan, you could use the following method call: - // Note that this can lead to problems if the robot moved in the meanwhile. // move_group_interface.execute(my_plan); - - // 1. Moving to a pose goal - // ^^^^^^^^^^^^^^^^^^^^^^^^ + // + // Note that this can lead to problems if the robot moved in the meanwhile. + // + // Moving to a pose goal + // ^^^^^^^^^^^^^^^^^^^^^ // // If you do not want to inspect the planned trajectory, - // the following is a more robust combination of the two-step plan+execute pattern shown above - // and should be preferred. Note that the pose goal we had set earlier is still active, - // so the robot will try to move to that goal. - - move_group_interface.move(); - + // the following is a more robust combination of the two-step plan+execute pattern shown above and should be + // preferred: move_group_interface.move(); + // + // Note that the pose goal we had set earlier is still active, so the robot will try to move to that goal. + // // 2. Planning to a joint-space goal // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // @@ -383,7 +383,7 @@ int main(int argc, char** argv) // Now, let's add the collision object into the world // (using a vector that could contain additional objects) - ROS_INFO_NAMED("tutorial", "Add an object into the world"); + ROS_INFO_NAMED("tutorial", "Add an object into the world (6)"); planning_scene_interface.addCollisionObjects(collision_objects); // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message @@ -396,7 +396,7 @@ int main(int argc, char** argv) // // Now when we plan a trajectory it will avoid the obstacle success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); + ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (pose goal move around cuboid) %s", success ? "" : "FAILED"); visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); @@ -439,7 +439,7 @@ int main(int argc, char** argv) ROS_INFO_NAMED("tutorial", "Attach the object to the robot"); move_group_interface.attachObject(object_to_attach.id, "panda_hand"); - visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Object attached to robot (8)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ @@ -451,7 +451,7 @@ int main(int argc, char** argv) // Replan, but now with the object in hand. move_group_interface.setStartStateToCurrentState(); success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); + ROS_INFO_NAMED("tutorial", "Visualizing plan 9 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); @@ -471,7 +471,7 @@ int main(int argc, char** argv) // Show text in RViz of status visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Object detached from robot", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Object detached from robot (10)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */ @@ -497,7 +497,7 @@ int main(int argc, char** argv) move_group_interface.setGoalOrientationToleranceXYZ(goal_orientation_tolerance_xyz); success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 8 (Pose goal with orientation tolerances avoiding collision) %s", + ROS_INFO_NAMED("tutorial", "Visualizing plan 11 (Pose goal with orientation tolerances avoiding collision) %s", success ? "" : "FAILED"); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); @@ -520,7 +520,7 @@ int main(int argc, char** argv) move_group_interface.setGoalPositionToleranceXYZ(goal_position_tolerance_xyz); success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 9 (Pose goal with position tolerances avoiding collision) %s", + ROS_INFO_NAMED("tutorial", "Visualizing plan 12 (Pose goal with position tolerances avoiding collision) %s", success ? "" : "FAILED"); visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); visual_tools.trigger(); @@ -538,7 +538,7 @@ int main(int argc, char** argv) // Show text in RViz of status visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Objects removed", rvt::WHITE, rvt::XLARGE); + visual_tools.publishText(text_pose, "Objects removed (13)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for MoveGroup to receive and process the attached collision object message */