One of the difficult problems in robotics is to define a path for the motion of a robot's arms to grasp an object, especially when obstacles may obstruct the most obvious path of motion. Fortunately, a ROS package called MoveIt allows us to plan and execute a complicated trajectory.
A video created at Rethink Robotics shows how to use MoveIt to plan the motion of Baxter's arms and then have MoveIt actually cause a real or simulated Baxter to execute the motion. To see the video, go to https://www.youtube.com/watch?feature=player_detailpage&v=1Zdkwym42P4.
A tutorial is available at the Rethink wiki site at http://sdk.rethinkrobotics.com/wiki/MoveIt_Tutorial.
First, start the Baxter simulator in Gazebo:
$ cd baxter_ws $ ./baxter.sh sim $ roslaunch baxter_gazebo baxter_world.launch
In a second terminal window, untuck Baxter's arms and start the Python script that starts the joint_trajectory_action_server:
$ cd baxter_ws $ ./baxter.sh sim $ rosrun baxter_tools tuck_arms.py -u $ rosrun baxter_interface joint_trajectory_action_server.py
The output on the screen should be as follows:
Initializing node... Initializing joint trajectory action server... Running. Ctrl-c to quit
In a third terminal, start MoveIt and wait for the response:
$ cd baxter_ws $ ./baxter.sh sim $ roslaunch baxter_moveit_config baxter_grippers.launch
The output should end as follows:
. . . All is well! Everyone is happy! You can start planning now!
Most of the screen text has been deleted here, but wait for the happy ending! Looking at the Gazebo window and the MoveIt window, you'll see that Baxter looks the same in terms of the positions of the arms:
MoveIt startup
In the figure, which is the rviz screen, the Displays and Motion Planning windows are shown on the left with the Context tab information showing. On the right, you can see the simulated Baxter in the starting position of MoveIt with arms untucked.
You can select any one of the Displays categories and modify the parameters. For example, the figure shows Baxter with a lightened Background Color chosen under Global Options.
Under the Motion Planning panel, the tabs Context/Planning/Manipulation/Scene Objects/Stored Scenes/Stored States/Status are defined in the following table:
|
Tab |
Uses |
|---|---|
|
Context |
Publish Current Scene and Save scene to a database |
|
Planning |
Set the start state, the goal state, and plan and execute moves of Baxter's arms |
|
Manipulation |
Object detection and manipulation |
|
Scene Objects |
Import or export scenes such as pillars or tabletops from a disk file |
|
Stored Scenes |
Stored scenes on a database |
|
Stored States |
Store and load robot states |
|
Status |
Status |
Click on the Planning tab under Motion Planning. On the Planning panel, look for the Query field and the heading Select Start State. Click on Select Start State to reveal a menu box and set it to <current>. Then, click on the Update button.
- Use arrows and rings to move Baxter's simulated arms to the desired positions. The desired goal positions should appear in orange in the simulation window.
- Under the Commands area, choose the Plan button to see the trajectory of Baxter's arms in MoveIt.
- Choose the Execute button to see Baxter's arms move to the goal positions.
- You should see red arms move from the start state to the final (goal) states:
Baxter's arms and goal state for the arms
Next, have the arms move back to original start position to perform another move. To do this, under the Query field, click on Select Goal State to reveal the menu box and set it to <same as start>. Then, click on the Update button:
Query field to return arms to start positions
Select the Scene Objects tab from the Motion Planning frame. If you have scenes in your computer's directories, you can import them using the Scene Geometry field by selecting Import From Text. Alternatively, use the following scene we created in the file PillarTable.scene. Make sure that this file is saved as a text file.
(noname)+ * pillar 1 box 0.308 0.13056 0.6528 0.7 -0.01 0.03 0.0108439 0.706876 0.0103685 0.707178 0 0 0 0 * tabletop 1 box 0.7 1.3 0.02 0.7 0.04 -0.13 0 0 0 1 0.705882 0.705882 0.705882 1 .
The first line of dimensions under box in the file represents the height, width, and length of the pillar in meters. The next line defines the position from Baxter's origin. The third line is the pose of the pillar as a quaternion. The following screenshot shows the results of importing the scene elements:
Baxter with tabletop and pillar
To manipulate the objects by moving or rotating, select the object name (not the checkbox), and the arrows and rings should appear. Change position with the green and blue arrows and rotate with the ring. Moving the Scale slider will change the size of the object. Move the mouse to rotate the object and roll the mouse wheel, if you have one, to zoom the object's size. You can save the scene (Export as Text) after you finish manipulating it.
Next, select the Context tab from the Motion Planning panel and click on the Publish Current Scene button under the Planning Library field. This step is important and tells MoveIt to plan around the obstacles in the environment!
When pillar is selected under the Current Scene Objects (select the word, not the checkbox), values for its Position (XYZ) and Rotation (RPY) appear under Manage Pose and Scale. The position of x, y, and z of the centroid of the pillar is shown with respect to Baxter's origin. Baxter's x axis extends outward toward the viewer. The positive y axis is to the right in the view and the z axis is upward. Note that the roll, pitch, and yaw are about the x, y, and z axes, respectively.
In our example, the left arm is going to move to the other side of the obstacle. MoveIt will plan the trajectory so that Baxter's arm will not hit the pillar.
Return to the Planning tab on the Motion Planning panel. First, move Baxter's right arm away from the pillar. We can now drag our interactive markers for Baxter's left arm to move the goal state to a location on the opposite side of the pillar. Each time you click on the Plan button, a different arm trajectory path is shown on virtual Baxter. Each path avoids collision with the pillar.
The following screenshot shows Baxter prior to moving the left arm around the pillar. Notice that the right arm is moved out of the way:
Baxter's right arm moved aside
Now, click on Execute to see Baxter's arm avoid the obstacle and move to the goal position on the other side of the pillar. The following screenshot shows Baxter's left arm has moved around the pillar to the other side:
Baxter's simulated arm moved to other side of the obstacle