Motion Planning using C++¶

In this exercise, we’ll explore MoveIt’s C++ interface to programatically move a robot.

Motivation¶

Now that we’ve got a working MoveIt! configuration for your workcell and we’ve played a bit in RViz with the planning tools, let’s perform planning and motion in code. This exercise will introduce you to the basic C++ interface for interacting with the MoveIt! node in your own program. There are lots of ways to use MoveIt!, but for simple applications this is the most straight forward.

Reference Example¶

Move Group Interface tutorial

3. Further Information and Resources¶

Scan-N-Plan Application: Problem Statement¶

In this exercise, your goal is to modify the myworkcell_core node to:

Move the robot’s tool frame to the center of the part location as reported by the service call to your vision node.

Scan-N-Plan Application: Guidance¶

Edit your myworkcell_node.cpp file.
1. Add #include <tf/tf.h> to allow access to the tf library (for frame transforms/utilities).
  - Remember that we already added a dependency on the tf package in a previous exercise.
2. In the ScanNPlan class’s start method, use the response from the LocalizePart service to initialize a new move_target variable:
```
geometry_msgs::Pose move_target = srv.response.pose;
```
  - make sure to place this code after the call to the vision_node’s service.
Use the MoveGroupInterface to plan/execute a move to the move_target position:
1. In order to use the MoveGroupInterface class it is necessary to add the moveit_ros_planning_interface package as a dependency of your myworkcell_core package. Add the moveit_ros_planning_interface dependency by modifying your package’s CMakeLists.txt (2 lines) and package.xml (1 line) as in previous exercises.
2. Add the appropriate “include” reference to allow use of the MoveGroupInterface:
```
#include <moveit/move_group_interface/move_group_interface.h>
```
3. Create a moveit::planning_interface::MoveGroupInterface object in the ScanNPlan class’s start() method. It has a single constructor that takes the name of the planning group you defined when creating the workcell moveit package (“manipulator”).
```
moveit::planning_interface::MoveGroupInterface move_group("manipulator");
```
4. Set the desired cartesian target position using the move_group object’s setPoseTarget function. Call the object’s move() function to plan and execute a move to the target position.
```
// Plan for robot to move to part
move_group.setPoseReferenceFrame(base_frame);
move_group.setPoseTarget(move_target); 
move_group.move();
```
5. As described here, the move_group.move() command requires use of an “asynchronous” spinner, to allow processing of ROS messages during the blocking move() command. Initialize the spinner near the start of the main() routine after ros::init(argc, argv, "myworkcell_node"), and replace the existing ros::spin() command with ros::waitForShutdown(), as shown:
```
ros::AsyncSpinner async_spinner(1);
async_spinner.start();
...
ros::waitForShutdown();
```

Test the system!

catkin build
roslaunch myworkcell_moveit_config myworkcell_planning_execution.launch
roslaunch myworkcell_support workcell.launch

More to explore…
- In RViz, add a “Marker” display of topic “/ar_pose_visual” to confirm that the final robot position matches the position published by fake_ar_publisher
- Try repeating the motion planning sequence:
  1. Use the MoveIt rviz interface to move the arm back to the “allZeros” position
  2. Ctrl+C the workcell.launch file, then rerun
- Try updating the workcell_node’s start method to automatically move back to the allZeros position after moving to the AR_target position. See here for a list of move_group’s available methods.
- Try moving to an “approach position” located a few inches away from the target position, prior to the final move-to-target.