OptiTrack

The goal of this procedure is to learn the transformation for Hand-Eye Calibration between a robot joint and a marker. For example, between the arm_left_7_link (a robot joint) and the Hand_Marker (an OptiTrack marker).

Code Repositories

• Hand-Eye Calibration
• TALOS Hip Marker STL Model
  • The Hip Marker is defined in this repository and provides a stable reference frame for calibration.
  • The Hand Marker does not have a fixed STL model and can be created by attaching individual reflective markers. This allows flexible placement but makes it less stable for long-term operation compared to the hip marker.

---

Prerequisites

• The robot must have two rigid-body markers, e.g., Hip_Marker and Hand_Marker.
• Start the NatNet Bridge to receive ROS transforms from the markers:
  • world -> Hip_Marker
  • world -> Hand_Marker

---

Workflow

1. Record TF Data

   ToDo: Directly obtain hip transformation during calibration.

   Run the following script to record the required transform data:

   python Forward_TF_Normalized_Quaternions.py
   

   This records base_link -> arm_left_7_link (from robot kinematics) and Hip_Marker -> Hand_Marker (from OptiTrack).

2. Compute Aligned Poses Use the alignment script to synchronize poses and compute the time offset:

   python compute_aligned_poses.py --poses_B_H_csv_file Talos_B_EE.csv --poses_W_E_csv_file Hip_to_Hand.csv --aligned_poses_B_H_csv_file Talos_B_EE_aligned.csv --aligned_poses_W_E_csv_file Hip_to_Hand_aligned.csv --time_offset_output_csv_file time_offset.csv
   

3. Run Main Program

   python main.py
   

---

Connecting OptiTrack and Hainer Frames

After calibration, we can use the obtained transformation between the hip marker and the robot's link frame to connect the robot's odometry frame with OptiTrack's world frame. We can use the robot_localization package to fuse the robot's IMU with the OptiTrack detection using an Extended Kalman Filter (EKF).

Alternatively, we can rely solely on OptiTrack to compute and publish the world -> odom transformation such that world -> odom -> base_link -> base_marker matches the OptiTrack detection world -> base_marker. Using the EKF is preferred as it should be smoother and more robust to detection errors.

Fusing EKF for Sensor FusionDirectly Connecting the TF Trees

We can use an EKF to fuse measurements. The learned calibration matrix can be provided as an input to the EKF, serving as the initial guess for the transformation between the systems.

Code Repository: Hainer EKF

See Fusing Optitrack with IMU/ODOM for setup and usage.

This package enables stable and repeatable computation of the world -> odom transform for the TALOS robot. Instead of relying on a hand marker (which may fall off), it uses a hip marker for robust long-term operation.

Code Repository: TF publisher

1. Initial Hand-Eye Calibration (Hand Marker)

   • Perform hand-eye calibration (see Hand-Eye Calibration section) to compute the fixed transform.
   • Publish the TF:

     arm_left_7_link -> Talos_Hand_Marker
     

     using WorldOdomPublisher.py.

2. One-Time Measurement of Hip Marker Transformation

   • Run save_tf_base_to_hip.py to record the current TF:

     base_link -> Talos_Hip_Marker
     

   • The result is saved as a static file: base_to_hip.yaml.

3. Stable World/Odom Publication Using the Hip Marker

   • Run publish_world_to_odom_from_base_yaml.py to continuously compute and publish:

     world -> odom
     

   • It uses the saved static transform (base_to_hip.yaml) and the live OptiTrack marker TFs.

   • The computation follows:

     \[\begin{align} _{\text{odom}}T^{\text{hip}} &= _{\text{odom}}T^{\text{base_link}} \cdot _{\text{base_link}}T^{\text{hip}} \\ _{\text{world}}T^{\text{odom}} &= _{\text{world}}T^{\text{hip}} \cdot \left(_{\text{odom}}T^{\text{hip}}\right)^{-1} \end{align}\]

   • The resulting world -> odom TF is published at runtime, providing a stable connection between OptiTrack and the TALOS robot.