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”.
- Remove the fist 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, then 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:
- In QtCreator:
Build -> Build Project
- Alternatively, in a terminal:
- In QtCreator:
catkin build --cmake-args -G 'CodeBlocks - Unix Makefiles' --workspace collision_avoidance_pick_and_place
- 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.