Note: This tutorial assumes that you have completed the previous tutorials: Setting up Dynamixel Servos.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Calibrating a Kinect to the TurtleBot Arm for Manipulation

Description: This tutorial will describe how to calibrate the physical position of the kinect to the TurtleBot arm, when the kinect is positioned either on-board the TurtleBot or off-board.

Tutorial Level: INTERMEDIATE

Introduction

What is calibration and why?

In this case, calibration refers to extrinsic calibration, or calibrating the physical position of a camera to some reference frame. If you have the camera in the factory position of the TurtleBot, it's already extrinsically calibrated - that is, its position on the robot is correct. However, that camera can not see the workspace of the arm. So in order to be able to perceive what the arm is doing, there's a few options: add a second kinect overlooking the workspace, or move the kinect on the robot.

If you add a new kinect or move an existing one, you now have the problem of not knowing the precise location of the camera in 3D space. This tutorial will walk you through determining the position of the kinect relative to some fixed frame by using the arm.

On-board vs. Off-board

Having a kinect on-board the TurtleBot (that is, physically mounted on the TurtleBot) has the advantage of letting the robot move its base while performing some manipulation task. However, since the kinect will have to be moved from the position that allows the TurtleBot to navigate, you will either have to add another sensor for navigation or adapt the data coming from the kinect in some way to still allow obstacle avoidance.

Having a kinect off-board allows you to keep your current robot setup, and gives a guaranteed good view of the workspace. However, since the kinect is off-board, it only allows you to use one workspace at a time.

Method

This extrinsic calibration procedure works by using a calibration pattern. The kinect can localize the calibration very precisely, and by moving the arm to certain positions on the pattern, we can calculate the transform between the kinect and any frame on the robot.

Physical Setup

The physical setup for calibration involves having a camera (in this case, kinect) that can see the workspace, and a workspace that the arm can reach. The IKEA Lack side table is actually the perfect height for the TurtleBot arm, though anything of around the same height should do.

Next, print out this check_7x6_27mm.pdf calibration pattern on A4 paper and attach it to the workspace, making sure that the arm can reach every point on the pattern.

physical_setup.jpg

Calibration Instructions

First, if they're not already running, start up the kinect and arm drivers on the TurtleBot.

roslaunch turtlebot_bringup kinect.launch
roslaunch turtlebot_arm_bringup arm.launch

Next, make sure that the kinect is set up in such a way that it can see the entire calibration pattern. The image below is a good example of such a setup.

calibration_raw.jpg

On your PC, make the package and launch the correct launchfile for your application. If you are calibrating an off-board kinect, launch calibrate_old_kinect.launch instead.

catkin_make turtlebot_arm_kinect_calibration
roslaunch turtlebot_arm_kinect_calibration calibrate_turtlebot.launch

This should detect the checkerboard and pop up the image shown below, with the calibration pattern edges overlaid and four points marked on the image.

calibration_overlaid.jpg

[ INFO] [1315859455.713597743]: [calibrate] Initialized.
[ INFO] [1315859455.919527989]: [calibrate] Got image info!
Got an image callback!
Is the checkerboard correct? 
Move edge of gripper to point 1 in image and press Enter. 

The next step is to move the edge of the gripper to the four specified points in order. Note that there is one specific edge you are trying to move: if you orient the arm so that the un-actuated side of the gripper is on the left, it will be the bottom left point. Make sure that your setup matches the one pictured below.

point_of_contact.jpg

Next, move the point of contact to the four points indicated on the image, pressing Enter every time you have the correct position.

step1.jpg step2.jpg step3.jpg step4.jpg

Applying the Calibration

After moving to the fourth point, the calibration script will output something like the following:

Resulting transform (camera frame -> fixed frame): 
       1        0        0  0.22683
       0        1        0 0.122903
       0        0        1 0.537765
       0        0        0        1

Resulting transform (fixed frame -> camera frame): 
0.000796318 0.000796288    0.999999    0.538043
         -1 5.96046e-07 0.000796288   -0.266402
2.98023e-08          -1 0.000796318   -0.122475
4.56515e-41           0 4.56515e-41           1

Static transform publisher (use for external kinect): 
rosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
rosrun tf static_transform_publisher -0.26683 -0.122903 -0.537733 0.5 -0.499602 0.5 0.500398 /arm_base_link /openni_camera 100

URDF output (use for kinect on robot): 
<?xml version="1.0"?>
<robot>
        <property name="turtlebot_calib_cam_x" value="-0.26683" />
        <property name="turtlebot_calib_cam_y" value="-0.122903" />
        <property name="turtlebot_calib_cam_z" value="-0.537733" />
        <property name="turtlebot_calib_cam_rr" value="0.785417" />
        <property name="turtlebot_calib_cam_rp" value="-1.5696" />
        <property name="turtlebot_calib_cam_ry" value="0.785417" />
        <property name="turtlebot_kinect_frame_name" value="/arm_base_link" />
</robot>

On-board

If you're calibrating a kinect on the TurtleBot, you need to update the calibration URDF for the robot to reflect the new position of the camera.

On the TurtleBot, roscd turtlebot_description and open urdf/turtlebot_calibration.xacro. Replace the contents with URDF output of the calibration script. You will need to restart the turtlebot node for the change to take effect.

Off-board

If you're calibrating an external kinect, open up a new terminal window, and run the static transform output by the script, such as

rosrun tf static_transform_publisher -0.26683 -0.122903 -0.537733 0.5 -0.499602 0.5 0.500398 /arm_base_link /openni_camera 100

As long as this command runs, the static_transform_publisher will publish the transform between a frame on the TurtleBot and the kinect frame. If you move the physical camera, you will need to recalibrate again.

Results

Whichever option you chose to take, the kinect pointcloud (usually /camera/rgb/points) should line up with the actual position of the arm in rviz, as shown in the image below.

rviz_calibration_result.jpg

Wiki: turtlebot_kinect_arm_calibration/Tutorials/CalibratingKinectToTurtleBotArm (last edited 2017-03-04 22:17:54 by Jorge Santos)