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.

1. In the `ScanNPlan` class's `start` method below the `if (!vision_client_.call(srv))` statement , use the response from the `LocalizePart` service to create a new `move_target` variable:
   ```c++
   geometry_msgs::Pose move_target = srv.response.pose;
   ```

Use the MoveGroupInterface to plan/execute a move to the move_target position:
1. The MoveGroupInterface is part of the moveit_ros_planning_interface package, so you’ll need to add this as a dependency to your myworkcell_core package. Modify your package's CMakeLists.txt (2 lines) and package.xml (2 lines) 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.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.

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page