Create Pick Moves¶
The gripper moves through three poses in order to do a pick: approach, target and retreat. In this exercise, we will use the box pick transform to create the pick poses for the TCP coordinate frame.
Locate Function¶
In the main program, locate the function call to
application.computePickToolPoses()
.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
RCLCPP_ERROR_STREAM ...
so that the program runs.
Complete Code¶
Find every line that begins with the comment
Fill Code:
and read the description. Then, replace every instance of the commentUNCOMMENT AND COMPLETE
with the appropriate line of code./* Fill Code: . . . */ /* UNCOMMENT AND COMPLETE: ... */
We want to set where the box currently is as the pick location with setOrigin().
After setting the right location, setRotation() makes the end-effector face the correct direction.
The
createManipulationPoses()
uses the values of the approach and retreat distances in order to create the corresponding poses at the desired target.
Build Code and Run¶
Compile the pick and place node:
In QTCreator:
Alternatively, in a terminal:
colcon build
Run the supporting nodes with the launch file (only if needed):
ros2 launch pick_and_place_application application_setup.launch.py
In another terminal, run your node with the launch file:
ros2 launch pick_and_place_application application_run.launch.py
The TCP and wrist positions at the pick will be printed in the terminal. You should see something like this:
[INFO] [1400555434.918332145] [pick_and_place_node]: Move wait Succeeded [INFO] [1400555435.172714267] [pick_and_place_node]: Gripper opened [INFO] [1400555435.424279410] [pick_and_place_node]: target recognition succeeded [INFO] [1400555435.424848964] [pick_and_place_node]: tcp position at pick: [-0.8, 0.2, 0.17] [INFO] [1400555435.424912520] [pick_and_place_node]: tcp z direction at pick: [8.65611e-17, -8.66301e-17, -1] [ERROR] [1400555435.425051853] [pick_and_place_node]: doBoxPickup is not implemented yet. Aborting.