Create Place Moves¶
The gripper moves through three poses in order to place the box: Approach, place and retreat. In this exercise, we will create these place poses for the TCP coordinate frame and then transform them to the arm’s wrist coordinate frame.
- In the main program , locate the function call to
- Go to the source file of that function by clicking in any part of the function and pressing F2 in QtCreator.
- Remove the first line containing the following
ROS_ERROR_STREAM ...so that the program runs.
Find every line that begins with the comment
Fill Code:and read the description. Then, replace every instance of the comment
ENTER CODE HEREwith the appropriate line of code.
/* Fill Code: . . . */ /* ======== ENTER CODE HERE ======== */
The box’s position at the place location is saved in the global variable
create_manipulation_poses()uses the values of the approach and retreat distances in order to create the corresponding poses at the desired target.
Since MoveIt! plans the robot path for the arm’s wrist, it is necessary to convert all the place poses to the wrist coordinate frame.
The lookupTransform() method can provide the pose of a target relative to another pose.
Build Code and Run¶
Compile the pick and place node:
Alternatively, in a terminal:
catkin build collision_avoidance_pick_and_place
Run the supporting nodes with the launch file:
roslaunch collision_avoidance_pick_and_place ur5_setup.launch
In another terminal, run your node with the launch file:
roslaunch collision_avoidance_pick_and_place ur5_pick_and_place.launch
The tcp and wrist position at the place location will be printed on the terminal. You should see something like:
[ INFO] [1400556479.404133995]: Execution completed: SUCCEEDED [ INFO] [1400556479.404574973]: Pick Move 2 Succeeded [ INFO] [1400556479.404866351]: tcp position at place: [-0.4, 0.6, 0.17] [ INFO] [1400556479.404934796]: wrist position at place: x: -0.422 y: 0.6 z: 0.3 [ERROR] [1400556479.404981729]: place_box is not implemented yet. Aborting.