From 8b7dee64b85b541314eedf535ac650ae060cadcc Mon Sep 17 00:00:00 2001 From: hello-chintan Date: Mon, 12 Sep 2022 17:53:43 -0400 Subject: [PATCH] Add MoveIt 2 tutorials --- mkdocs.yml | 4 ++- ros2/moveit_basics.md | 65 ++++++++++++++++------------------- ros2/moveit_movegroup_demo.md | 46 +++++++++++++++++++++++++ ros2/moveit_rviz_demo.md | 45 ++++++++++++++++++++++++ 4 files changed, 123 insertions(+), 37 deletions(-) create mode 100644 ros2/moveit_movegroup_demo.md create mode 100644 ros2/moveit_rviz_demo.md diff --git a/mkdocs.yml b/mkdocs.yml index fc1a504..fc96bd3 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -180,7 +180,9 @@ nav: - Internal State of Stretch: ./ros2/internal_state_of_stretch.md - RViz Basics: ./ros2/rviz_basics.md - Navigation Stack: ./ros2/navigation_stack.md - - MoveIt! Basics: ./ros2/moveit_basics.md + - MoveIt 2: ./ros2/moveit_basics.md + - MoveIt with RViz: ./ros2/moveit_rviz_demo.md + - MoveGroup C++ API: ./ros/moveit_movegroup_demo.md - Follow Joint Trajectory Commands: ./ros2/follow_joint_trajectory.md # - Perception: ./ros2/perception.md # - ArUco Marker Detection: ./ros2/aruco_marker_detection.md diff --git a/ros2/moveit_basics.md b/ros2/moveit_basics.md index 4e828fc..941e191 100644 --- a/ros2/moveit_basics.md +++ b/ros2/moveit_basics.md @@ -2,55 +2,48 @@ **NOTE**: ROS 2 tutorials are still under active development. +## Overview + +MoveIt 2 is a whole-body motion planning framework for mobile manipulators that allows planning pose and joint goals in environments with and without obstacles. Stretch being a mobile manipulator is uniquely well-suited to utilize the planning capabilities of MoveIt 2 in different scenarios. + +## Motivation + +Stretch has a kinematically simple 3 DoF arm (+2 with DexWrist) that is suitable for pick and place tasks of varied objects. Its mobile base provides it with 2 additional degrees of freedom that afford it more manipulability and also the ability to move around freely in its environment. To fully utilize these capabilities, we need a planner that can plan for both the arm and the mobile base at the same time. With MoveIt 2 and ROS 2, it is now possible to achieve this, empowering users to plan more complicated robot trajectories in difficult and uncertain environments. + +## Demo with Stretch Robot + +Before we proceed, it's always a good idea to home the robot first by running the following script so that we have the correct joint positions being published on the /joint_states topic. This is necessary for planning trajectories on Stretch with MoveIt. -## MoveIt! on Stretch -To run MoveIt with the actual hardware, (assuming `stretch_driver` is already running) simply run -```bash -roslaunch stretch_moveit_config move_group.launch ``` -This will runs all of the planning capabilities, but without the setup, simulation and interface that the above demo provides. In order to create plans for the robot with the same interface as the offline demo, you can run -```bash -roslaunch stretch_moveit_config moveit_rviz.launch -``` - -## MoveIt! Without Hardware -To begin running MoveIt! on stretch, checkout to the feature/hybrid_planning branch and run the demo launch file. - -```bash -cd ~/ament_ws/src/stretch_ros2 -git checkout feature/hybrid_planning -cd ~/ament_ws -colcon build -ros2 launch stretch_moveit_config movegroup_moveit2.launch.py +stretch_robot_home.py ``` -This will brining up an RViz instance where you can move the robot around using [interactive markers](http://wiki.ros.org/rviz/Tutorials/Interactive%20Markers%3A%20Getting%20Started) and create plans between poses. You can reference the bottom gif as a guide to plan and execute motion. - -![image](https://raw.githubusercontent.com/hello-robot/stretch_tutorials/ROS2/images/moveit.gif) +### Planning with MoveIt 2 Using RViz -Additionally, the demo allows a user to select from the five groups, *stretch_arm*, *stretch_gripper*, *stretch_head*, *mobile_base_arm* and *position* to move. The option to change groups in the in *Planning Request* section in the *Displays* tree. A few notes to be kept in mind: +The easiest way to run MoveIt 2 on your robot is through RViz. With RViz you can plan, visualize, and also execute trajectories for various planning groups on your robot. To launch RViz with MoveIt 2, run the following command. (Press Ctrl+C in the terminal to terminate) -* Pre-defined start and goal states can be specified in Start State and Goal State drop downs in Planning tab of Motion Planning RViz plugin. +``` +ros2 launch stretch_moveit_config movegroup_moveit2.launch.py +``` -* *stretch_gripper* group does not show markers, and is intended to be controlled via the joints tab that is located in the very right of Motion Planning Rviz plugin. +Follow instructions in this [tutorial](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/moveit_rviz_demo) to plan and execute trajectories using the interactive markers in RViz. -* When planning with *stretch_head* group make sure you select *Approx IK Solutions* in Planning tab of Motion Planning RViz plugin. +Use the interactive markers to drag joints to desired positions or go to the manipulation tab in the Motion Planning pane to fine-tune joint values using the sliders. Next, click the 'Plan' button to plan the trajectory. If the plan is valid, you should be able to execute the trajectory by clicking the 'Execute' button. Below we see Stretch raising its arm without any obstacle in the way. +![WithoutObstacle](https://user-images.githubusercontent.com/97639181/162533340-dec4232b-617c-4b90-b4e1-a24fd3027baa.gif) -![image](https://raw.githubusercontent.com/hello-robot/stretch_tutorials/ROS2/images/moveit_groups.gif) +To plan with obstacles, you can insert objects like a box, cyclinder or sphere, in the planning scene to plan trajectories around the object. This can be done by adding an object using the Scene Objects tab in the Motion Planning pane. Below we see Stretch raising its arm with a flat cuboid obstacle in the way. The mobile base allows Stretch to move forward and then back again while raising the arm to avoid the obstacle. +![WithObstacle](https://user-images.githubusercontent.com/97639181/162533356-15955809-f21d-4181-a012-6bca3f48dfc4.gif) -## Running Gazebo with MoveIt! and Stretch +### Planning with MoveIt 2 Using the MoveGroup C++ API +If you want to integrate MoveIt 2 into your planning pipeline and want greater control over its various functionalities, using the MoveGroup API is the way to go. Execute the launch file again and go through the comments in the [code](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_moveit_config/src/movegroup_test.cpp) to understand what's going on. (Press Ctrl+C in the terminal to terminate) - +ros2 launch stretch_moveit_config movegroup_moveit2.launch.py +``` + +Follow instructions in this [tutorial](https://docs.hello-robot.com/0.2/stretch-tutorials/ros2/moveit_movegroup_demo) to plan and execute trajectories using the MoveGroup C++ API. +![StowEdited](https://user-images.githubusercontent.com/97639181/166838248-cbfd537b-973e-4fb4-b60c-b5b3c111e02d.gif) \ No newline at end of file diff --git a/ros2/moveit_movegroup_demo.md b/ros2/moveit_movegroup_demo.md new file mode 100644 index 0000000..2d6ce50 --- /dev/null +++ b/ros2/moveit_movegroup_demo.md @@ -0,0 +1,46 @@ +## Planning with MoveIt 2 Using the MoveGroup C++ API + +If you want to integrate MoveIt 2 into your planning pipeline and want greater control over its various functionalities, using the MoveGroup API is the way to go. For this tutorial we are going to use the RViz Visual Tools plugin to execute the C++ source code part by part to explore more sophisticated functionalities. + +Execute the launch file again to begin the tutorial. You can follow along in the C++ [code](https://github.com/hello-robot/stretch_ros2/blob/galactic/stretch_moveit_config/src/movegroup_test.cpp) to inspect finer details. (Press Ctrl+C in the terminal to terminate) (Ensure you have enough room around the robot before running the script) + +``` +ros2 launch stretch_moveit_config movegroup_moveit2.launch.py +``` + +To execute the script and interact with the robot, all you need to do is press the Next button in the RViz Visual Tools windown at the bottom left. Follow the prompts on the terminal to run through the tutorial. While executing the script, it's also a good idea to study and understand the script that is being executed. Find it [here](https://github.com/hello-robot/stretch_ros2/blob/feature/hybrid_planning/stretch_moveit_config/src/movegroup_test.cpp). + +![RVizVisualTools](https://user-images.githubusercontent.com/97639181/181854836-6f9b43cd-edf9-46e8-8ca1-1ac782d5b70b.png) + + +1. Let's begin by joggin the camera pan and tilt joints. For having a complete 3D representation of its environment, Stretch needs to point its head in all directions, up, down, left, right, you name it! Luckily, we have a planning group that allows you to do just that - the stretch_head planning group. Go ahead and press the Next button to jog the camera. + +![Head_Group](https://user-images.githubusercontent.com/97639181/182997546-1273dcc8-e036-4eb9-93b1-5b40233501a1.gif) + +2. What good is a robot that can't hold your hand on your worst days. We gave Stretch a gripper to do just that and more! Let's exercise it using the stretch_gripper planning group. All you have to do is press Next. + +![Gripper_Group](https://user-images.githubusercontent.com/97639181/182997590-ad3d46df-1add-4cbc-9c5f-13a50baedc87.gif) + +3. What about the good days you ask? Stretch always wants to reach out to you, no matter what. Speaking of reaching out, let's make Stretch exercise its arm for the next time you need it. Press Next. + +![Arm_Group](https://user-images.githubusercontent.com/97639181/182997609-ca44b478-c645-4841-bb68-9413da208b3a.gif) + +4. Stretch doesn't like sitting in a corner fretting about the future. It is the future. Stretch wants to explore and in style. What better way to do it than by rolling around? Press Next and you'll see. That's the mobile_base planning group. + +![Base_Group](https://user-images.githubusercontent.com/97639181/182997648-434c9b0f-be09-4950-bbd3-918b78ed4315.gif) + +5. All that exploring does get tiring and sometimes Stretch just wants to relax and dream about its next adventure. Stretch prefers to relax with its arm down, lest someone trips over it and disturb Stretch's peaceful slumber. Press Next to see the mobile_base_arm planning group. + +![Base_Arm_Group](https://user-images.githubusercontent.com/97639181/182997677-cd5de49f-aba3-4c2c-9828-a3e517b74a1e.gif) + +6. Did someone say adventure? How about dodging some pesky obstacles? They're everywhere, but they don't bother Stretch a lot. It can go around them. Nothing stops Stretch! You know what to do. + +![Add_Object](https://user-images.githubusercontent.com/97639181/182997701-ca93bbb0-e270-4784-b87a-741712ca2449.gif) + +7. Stretch is smart, you don't need to tell it which joint goes where. Just say what you want done and it does it. How about planning a pose goal to see it in action? Press Next. + +![Pose_Goal](https://user-images.githubusercontent.com/97639181/182997727-9a42f12e-9476-4cc5-a74b-71ccdecc637f.gif) + +8. To wrap it up, the final act! This one is a surprise that's only a click away. Go on, do it! + +![Stow](https://user-images.githubusercontent.com/97639181/182997818-7a9a104b-41a9-4cf6-8901-1a7d5ee73017.gif) \ No newline at end of file diff --git a/ros2/moveit_rviz_demo.md b/ros2/moveit_rviz_demo.md new file mode 100644 index 0000000..db64cef --- /dev/null +++ b/ros2/moveit_rviz_demo.md @@ -0,0 +1,45 @@ +## Planning with MoveIt 2 Using RViz + +1. The easiest way to run MoveIt 2 on your robot is through the RViz plugin. With RViz you can plan, visualize, and also execute trajectories for various planning groups on your robot. To get started using MoveIt 2 with RViz, execute the following command in a terminal. (Press Ctrl+C in the terminal to terminate) + +``` +ros2 launch stretch_moveit_config movegroup_moveit2.launch.py +``` + +2. You should see Stretch visualized in RViz with joint positions exactly as they appear on the actual robot (If not, home the robot and start from step 1!). You can select a Planning Group from the drop down menu that allows you to choose a group of joints to plan for and control using MoveIt. When you select a Planning Group the joints that can be controlled are highlighted with interactive markers. Let’s go ahead and select the stretch_arm planning group. + +![edited_planning_group](https://user-images.githubusercontent.com/97639181/181605746-f3b141b8-af4d-4c9e-a3e0-02ec7985c9ab.gif) + +3. Now, click and drag the arrow down to slide the arm lift downwards and then use the wheel to turn the gripper inwards so that it fits squarely over the robot base. At this point if the robot base glows red in RViz, it means the robot arm is in collision with the base. You should move the lift upwards slightly until the red highlight disappears. + +![edited_int_markers](https://user-images.githubusercontent.com/97639181/181605790-623fef72-ba0b-4448-9240-f9ac0cb7e671.gif) + +4. Now click on the Plan button to see the simulated motion of the robot in RViz + +![edited_robot_stow](https://user-images.githubusercontent.com/97639181/181605820-9a5390e6-4a5b-49e6-8eae-b4aaa8e070ed.gif) + +5. Before proceeding, ensure that the robot is in an open space without obstalces. Click the Execute button to execute the plan on the actual robot. Congratulations, you just stowed the robot arm using MoveIt! (Alternatively, if you do not want to review the simulated plan, you can click ‘Plan and Execute’ to execute the planned trajectory directly) + +![edited_plan_execute](https://user-images.githubusercontent.com/97639181/181605939-12160e3a-15c7-40c8-99db-71ef643ff7a6.gif) + +6. Now, let’s move Stretch’s mobile base! Select the mobile_base_arm planning_group from the drop down menu. You should see the base interactive marker appear in RViz. Use the arrow to drag the base forward or backward for about 1m. Click Plan and Execute when you are done. Voila! + +![edited_move_forward](https://user-images.githubusercontent.com/97639181/181606040-c3496c59-6c6c-483d-9b9b-7538c1f8093a.gif) + +The mobile_base_arm planning group also allows you to execute a coordinated base and arm motion plan. Go ahead and move the markers around to plan some fun trajectories, maybe make Stretch do a Pirouette! Similarly, the stretch_gripper and stretch_head planning groups allow opening/closing the gripper and panning/tilting the camera. + +7. The interactive markers are just one way to control the joints. If you want a finer control, you can switch to the Joints tab of the plugin and use the sliders to adjust the desired end state of the joints. + +![edited_joint_sliders](https://user-images.githubusercontent.com/97639181/181606099-dd17005e-2d21-4006-986a-aaa7caf59e01.gif) + +8. MoveIt allows you to plan not just simple trajectories but also avoid obstacles. Let’s add an obstacle to the planning scene. Click on the Scene Objects tab and select the Box object. Define a cube of dimensions 0.1x0.1x0.1m and add it to the scene using the green + button next to it. Now, place it just in front of the mobile base using the fine controls in the Change object pose/scale buttons to the right. Click on the Publish button for MoveIt to account for the object while planning. + +![edited_add_obj](https://user-images.githubusercontent.com/97639181/181606170-2aea3b25-6a95-4cfc-bbf3-064f8de50811.gif) + +9. Now return back to the Planning tab and define an end state such that the Box is in between the robot start and end states. Again, ensure that the robot has enough space around it. Plan and Execute! + +![edited_avoid_obs](https://user-images.githubusercontent.com/97639181/181606192-e42128dc-e8a6-4fb2-b27f-869a99fc414d.gif) + +With a fully functional perception pipeline, the planning scene can represent Stretch’s surroundings accurately and allow Stretch to manipulate and navigate in a cluttered environment + +10. Feel free to explore more sophisticated planners shipped along with MoveIt 2 in the Context tab. End!