mikeferguson / robot_calibration

Generic calibration for robots
Apache License 2.0
370 stars 116 forks source link
calibration robot ros ros2

Robot Calibration

This package offers several ROS2 nodes. The primary one is called calibrate, and can be used to calibrate a number of parameters of a robot, such as:

These parameters are then inserted into an updated URDF, or updated camera configuration YAML in the case of camera intrinsics.

Two additional ROS nodes are used for mobile-base related parameter tuning:

The calibrate node

Calibration works in two steps. The first step involves the capture of data samples from the robot. Each "sample" comprises the measured joint positions of the robot and two or more "observations". An observation is a collection of points that have been detected by a "sensor". For instance, a robot could use a camera and an arm to "detect" the pose of corners on a checkerboard. In the case of the camera sensor, the collection of points is simply the detected positions of each corner of the checkerboard, relative to the pose of the camera reference frame. For the arm, it is assumed that the checkerboard is fixed relative to a virtual checkerboard frame which is fixed relative to the end effector of the arm. Within the virtual frame, we know the ideal position of each point of the checkerboard corners since the checkerboard is of known size.

The second step of calibration involves optimization of the robot parameters to minimize the errors. Errors are defined as the difference in the pose of the points based on reprojection throuhg each sensor. In the case of our checkerboard above, the transform between the virtual frame and the end effector becomes additional free parameters. By estimating these parameters alongside the robot parameters, we can find a set of parameters such that the reprojection of the checkerboard corners through the arm is as closely aligned with the reprojection through the camera (and any associated kinematic chain, for instance, a pan/tilt head).

Configuration is typically handled through two sets of YAML files: usually called capture.yaml and calibrate.yaml.

If you want to manually move the robot to poses and capture each time you hit ENTER on the keyboard, you can run robot calibration with:

ros2 run robot_calibration calibrate --manual --ros-args --params-file path-to-capture.yaml --params-file path-to-calibrate.yaml

More commonly, you will generate a third YAML file with the capture pose configuration (as documented below in the section "Calibration Poses"):

ros2 run robot_calibration calibrate path-to-calibration-poses.yaml --ros-args --params-file path-to-capture.yaml --params-file path-to-calibrate.yaml

This is often wrapped into a ROS 2 launch file, which often records a bagfile of the observations allowing to re-run just the calibration part instead of needing to run capture each time. For an example, see the UBR-1 example in the next section.

Example Configuration

All of the parameters that can be defined in the capture and calibrate steps are documented below, but sometimes it is just nice to have a full example. The UBR-1 robot uses this package to calibrate in ROS2. Start with the calibrate_launch.py in ubr1_calibration.

Capture Configuration

The capture.yaml file specifies the details needed for data capture:

Each of these chains and features is then defined by a parameter block of the same name, for example:

robot_calibration:
  ros_parameters:
    # List of chains
    chains:
    - arm
    # List of features
    features:
    - checkerboard_finder
    # Parameter block to define the arm chain
    arm:
      topic: /arm_controller/follow_joint_trajectory
      joints:
      - first_joint
      - second_joint
    # Parameter block to define the feature finder:
    checkerboard_finder:
      type: robot_calibration::CheckerboardFinder
      topic: /head_camera/depth_registered/points
      camera_sensor_name: camera
      chain_sensor_name: arm

Chain Parameters

For each chain, the following parameters can be defined:

Finder Parameters

At a minimum, the following parameters must be set for all finders:

The following types are currently included with robot_calibration although you can create your own plugins. Each finder has it's own additional parameters:

Additionally, any finder that subscribes to a depth camera has the following parameters:

Calibration Configuration

The calibrate.yaml configuration file specifies the configuration for optimization. This specifies several items:

robot_calibration:
  ros__parameters:
    base_link: torso_lift_link
    calibration_steps:
    - single_calibration_step
    single_calibration_step:
      models:
      - first_model
      first_model:
        type: first_model_type

For each calibration step, there are several parameters:

For each model, the type must be specified. The type should be one of:

For each error block, the type must be specified. In addition to the type parameter, each block will have additional parameters:

Calibration Poses

The final piece of configuration is the actual poses from which the robot should capture data. This YAML file can be created by running the capture_poses script. You will be prompted to move the robot to the desired pose and press ENTER, when done collecting all of your poses, you can type EXIT. This will create calibration_poses.yaml which is an array of capture poses:

- features: []
  joints:
  - first_joint
  - second_joint
  positions:
  - -0.09211555123329163
  - 0.013307283632457256
- features: []
  joints:
  - first_joint
  - second_joint
  positions:
  - -1.747204065322876
  - -0.07186950743198395

By default, every finder is used for every capture pose. In some cases, you might want to specify specific finders by editing the features:

# This sample pose uses only the `ground_plane_finder` feature finder
- features:
  - ground_plane_finder
  joints:
  - first_joint
  - second_joint
  positions:
  - -0.09211555123329163
  - 0.013307283632457256
# This sample pose will use all features
- features: []
  joints:
  - first_joint
  - second_joint
  positions:
  - -1.747204065322876
  - -0.07186950743198395

Checkerboard Configuration

When using a checkerboard, we need to estimate the transformation from the the tip of the kinematic chain to the virtual checkerboard frame. Calibration will be faster and more accurate if the initial estimate of this transformation is close to the actual value, especially with regards to rotation.

The simplest way to check your initial estimate is to run the calibration with only the six DOF of the checkerboard as free parameters. The output values will be the X, Y, Z, and A, B, C of the transformation. It is important to note that A, B, C are NOT roll, pitch, yaw -- they are the axis-magnitude representation. To get roll, pitch and yaw, run the to_rpy tool with your values of A, B, and C:

ros2 run robot_calibration to_rpy A B C

This will print the ROLL, PITCH, YAW values to put in for initial values. Then insert the values in the calibration.yaml:

free_frames_initial_values:
- checkerboard
checkerboard_initial_values:
  x: 0.0
  y: 0.225
  z: 0
  roll: 0.0
  pitch: 1.571
  yaw: 0.0

This tool can be helfpul for creating checkerboards.

Exported Results

The exported results consist of an updated URDF file, and one or more updated camera calibration YAML files. By default, these files will by exported into the /tmp folder, with filenames that include a timestamp of generation. These files need to be installed in the correct places to be properly loaded.

The fetch_calibration package has an example python script for installing the updated files.

Within the updated URDF file, there are two types of exported results:

If your robot does not support the "calibration" tags, it might be possible to use only free_frames, setting only the rotation in the joint axis to be free.

The _base_calibrationnode

To run the _base_calibrationnode node, you need a somewhat open space with a large (~3 meters wide) wall that you can point the robot at. The robot should be pointed at the wall and it will then spin around at several different speeds. On each rotation it will stop and capture the laser data. Afterwards, the node uses the angle of the wall as measured by the laser scanner to determine how far the robot has actually rotated versus the measurements from the gyro and odometry. We then compute scalar corrections for both the gyro and the odometry.

Node parameters:

Node topics:

The output of the node is a new scale for the gyro and the odometry. The application of these values is largely dependent on the drivers being used for the robot. For robots using _roscontrol or _robotcontrol there is a track_width parameter typically supplied as a ROS parameter in your launch file.

The _magnetometercalibration node

The _magnetometercalibration node records magnetometer data and can compute the hard iron offsets. After calibration, the magnetometer can be used as a compass (typically by piping the data through _imu_filtermadgwick and then _robotlocalization).

Node parameters:

Node topics:

The output of the calibration is three parameters, _mag_biasx, _mag_biasy, and _mag_biasz, which can be used with the imu_filter_madgwick package.

Migrating from ROS1

There are a number of changes in migrating from ROS1 to ROS2. Some of these are due to differences in the ROS2 system, others are to finally cleanup mistakes made in earlier version of robot_calibration.

The chains, models, free_frames and features parameters used to be lists of YAML dictionaries. That format is not easily supported in ROS2 and so they are now lists of string names and the actual dictionaries of information appear under the associated name. For instance, in ROS1, you might have:

models:
 - name: arm
   type: chain
   frame: wrist_roll_link
 - name: camera
   type: camera3d
   frame: head_camera_rgb_optical_frame

In ROS2, this becomes:

models:
- arm
- camera
arm:
  type: chain3d
  frame: wrist_roll_link
camera:
  type: camera3d
  frame: head_camera_rgb_optical_frame

NOTE: the "chain" type has been renamed "chain3d" in ROS2 for consistency (and to allow a future chain2d).

Multi-step calibration is now fully supported. A new parameter, calibration_steps must be declared as a list of step names. The models and free parameters are then specified for each step. As an example:

calibration_steps:
- first_calibration_step
- second_calibration_step
first_calibration_step:
  models: ...
  free_params: ...
second_calibration_step:
  models: ...
  free_params: ...

The capture poses can now be specified as YAML. The convert_ros1_bag_to_yaml script can be run in ROS1 to export your ROS1 bagfile as a YAML file that can be loaded in ROS2.