Calibration

base_calibration

Base calibration class for FIGAROH examples.

This module provides the BaseCalibration abstract class extracted from the FIGAROH library for use in the examples. It implements a comprehensive framework for robot kinematic calibration.

class figaroh.calibration.base_calibration.BaseCalibration(robot, config_file: str, del_list: List[int] = None)

Bases: ABC

Abstract base class for robot kinematic calibration.

This class provides a comprehensive framework for calibrating robot kinematic parameters using measurement data. It implements the Template Method pattern, providing common functionality while allowing robot-specific implementations of the cost function.

The calibration process follows these main steps: 1. Parameter initialization from configuration files 2. Data loading and validation 3. Parameter identification using base regressor analysis 4. Robust optimization with outlier detection and removal 5. Solution evaluation and validation 6. Results visualization and export

Key Features: - Automatic parameter identification using QR decomposition - Robust optimization with iterative outlier removal - Unit-aware measurement weighting for position/orientation data - Comprehensive solution evaluation and quality metrics - Extensible framework for different robot types

STATUS

Current calibration status (“NOT CALIBRATED” or “CALIBRATED”)

Type:

str

LM_result

Optimization result from scipy.optimize.least_squares

var_

Calibrated parameter values

Type:

ndarray

evaluation_metrics

Solution quality metrics

Type:

dict

std_dev

Standard deviations of calibrated parameters

Type:

list

std_pctg

Standard deviation percentages

Type:

list

PEE_measured

Measured end-effector poses/positions

Type:

ndarray

q_measured

Measured joint configurations

Type:

ndarray

calib_config

Calibration parameters and configuration

Type:

dict

model

Robot kinematic model (Pinocchio)

data

Robot data structure (Pinocchio)

Example

>>> # Create robot-specific calibration
>>> class MyRobotCalibration(BaseCalibration):
...     def cost_function(self, var):
...         PEEe = calc_updated_fkm(self.model, self.data, var,
...                                self.q_measured, self.calib_config)
...         residuals = self.PEE_measured - PEEe
...         return self.apply_measurement_weighting(residuals)
...
>>> # Run calibration
>>> calibrator = MyRobotCalibration(robot, "config.yaml")
>>> calibrator.initialize()
>>> calibrator.solve()
>>> print(f"RMSE: {calibrator.evaluation_metrics['rmse']:.6f}")

Notes

• Derived classes should implement robot-specific cost_function()

• Default cost_function is provided but issues performance warning

• Configuration files must follow FIGAROH parameter structure

• Supports both “full_params” and “joint_offset” calibration models

See also

• TiagoCalibration: TIAGo robot implementation

• UR10Calibration: Universal Robots UR10 implementation

• calc_updated_fkm: Forward kinematics computation function

• apply_measurement_weighting: Unit-aware weighting utility

apply_measurement_weighting(residuals: ndarray, pos_weight: float | None = None, orient_weight: float | None = None) → ndarray

Apply measurement weighting to handle position/orientation units.

This utility method can be used by derived classes to properly weight position (meter) and orientation (radian) measurements for equivalent influence in the cost function.

Parameters:

• residuals (ndarray) – Raw residual vector

• pos_weight (float, optional) – Weight for position residuals. If None, uses 1/position_std

• orient_weight (float, optional) – Weight for orientation residuals. If None, uses 1/orientation_std

Returns:

Weighted residual vector

Return type:

ndarray

Example

>>> # In derived class cost_function:
>>> raw_residuals = self.PEE_measured - PEEe
>>> weighted_residuals = self.apply_measurement_weighting(
...     raw_residuals, pos_weight=1000.0, orient_weight=100.0)

calc_stddev(result)

Calculate parameter uncertainty statistics from optimization results.

Computes standard deviation and percentage uncertainty for each calibrated parameter using the covariance matrix derived from the Jacobian at the optimal solution. This provides confidence intervals and parameter reliability metrics.

The calculation uses the linearized uncertainty propagation: σ²(θ) = σ²(residuals) * (J^T J)^-1

Where J is the Jacobian matrix and σ²(residuals) is the residual variance estimate.

Prerequisites:

• Calibration optimization must be completed

• Jacobian matrix must be available from optimization

Side Effects:

• Sets self.std_dev with parameter standard deviations

• Sets self.std_pctg with percentage uncertainties

Raises:

• CalibrationError – If calibration has not been performed

• np.linalg.LinAlgError – If Jacobian matrix is singular or ill-conditioned

Example

>>> calibrator.solve()
>>> calibrator.calc_stddev()
>>> print(f"Parameter uncertainties: {calibrator.std_dev}")
>>> print(f"Percentage errors: {calibrator.std_pctg}")

cost_function(var: ndarray) → ndarray

Calculate cost function for optimization.

This method provides a default implementation but should be overridden by derived classes to define robot-specific cost computation with appropriate weighting and regularization.

Parameters:

var (ndarray) – Parameter vector to evaluate

Returns:

Residual vector

Return type:

ndarray

Warning

Using default cost function. Consider implementing robot-specific cost function for optimal performance.

Example implementation:

>>> def cost_function(self, var):
...     PEEe = calc_updated_fkm(self.model, self.data, var,
...                            self.q_measured, self.calib_config)
...     raw_residuals = self.PEE_measured - PEEe
...     weighted_residuals = self.apply_measurement_weighting(
...         raw_residuals, pos_weight=1000.0, orient_weight=100.0)
...     # Add regularization if needed
...     return weighted_residuals

create_param_list(q: ndarray | None = None)

Initialize calibration parameter structure and validate setup.

This method sets up the fundamental parameter structure for calibration by computing kinematic regressors and ensuring proper frame naming conventions. It serves as a critical initialization step that must be called before optimization begins.

The method performs several key operations: 1. Computes base kinematic regressors for parameter identification 2. Adds default names for unknown base and tip frames 3. Validates the parameter structure for calibration readiness

Parameters:

q (array_like, optional) – Joint configuration for regressor computation. If None, uses empty list which may limit regressor accuracy

Returns:

Always returns True to indicate successful completion

Return type:

bool

Side Effects:

• Updates self.calib_config with frame names if not known

• Computes and caches kinematic regressors

• May modify parameter structure for calibration compatibility

Raises:

• ValueError – If robot model is not properly initialized

• AttributeError – If required calibration parameters are missing

• CalibrationError – If parameter creation fails

Example

>>> calibrator = BaseCalibration(robot)
>>> calibrator.load_param("config.yaml")
>>> calibrator.create_param_list()  # Basic setup
>>> # Or with specific joint configuration
>>> q_nominal = np.zeros(robot.nq)
>>> calibrator.create_param_list(q_nominal)

See also

calculate_base_kinematics_regressor: Core regressor computation add_base_name: Base frame naming utilities add_pee_name: End-effector frame naming utilities

get_pose_from_measure(res_: ndarray) → ndarray

Calculate forward kinematics with calibrated parameters.

Computes robot end-effector poses using the updated kinematic model with calibrated parameters. This method applies the calibration results to predict poses for the measured joint configurations.

Parameters:

res (ndarray) – Calibrated parameter vector containing kinematic corrections (geometric parameters, base transform, tool transform, etc.)

Returns:

Predicted end-effector poses corresponding to the

measured joint configurations. Shape depends on the number of measurements and pose representation format.

Return type:

ndarray

Prerequisites:

• Joint configurations must be loaded (q_measured available)

• Calibration parameters must be initialized

• Robot model must be properly configured

Example

>>> # After calibration
>>> calibrated_params = calibrator.LM_result.x
>>> predicted_poses = calibrator.get_pose_from_measure(
...     calibrated_params)
>>> # Compare with measured poses
>>> errors = predicted_poses - calibrator.PEE_measured

See also

calc_updated_fkm: Core forward kinematics computation function

initialize()

Initialize calibration data and parameters.

Performs the initialization phase of calibration by: 1. Loading measurement data from files 2. Creating parameter list through base regressor analysis 3. Identifying calibratable parameters using QR decomposition

This method must be called before solve() to prepare the calibration problem. It handles data validation, parameter identification, and sets up the optimization problem structure.

Raises:

• FileNotFoundError – If measurement data file not found

• ValueError – If data format is invalid or incompatible

• AssertionError – If required data dimensions don’t match

• CalibrationError – If initialization fails

Side Effects:

• Populates self.PEE_measured with measurement data

• Populates self.q_measured with joint configuration data

• Updates self.calib_config[“param_name”] with identified parameters

• Validates data consistency and dimensions

Example

>>> calibrator = TiagoCalibration(robot, "config.yaml")
>>> calibrator.initialize()
>>> print(f"Loaded {calibrator.calib_config['NbSample']} samples")
>>> print(f"Calibrating {len(calibrator.calib_config['param_name'])} "
...       f"parameters")

load_data_set()

Load experimental measurement data for calibration.

Reads measurement data from the specified data path and processes it for calibration use. This includes both pose measurements and corresponding joint configurations, with optional data filtering based on the deletion list.

The method handles data preprocessing, validation, and formatting to ensure compatibility with the calibration algorithms. It serves as the primary data ingestion point for the calibration process.

Side Effects:

• Sets self.PEE_measured with processed pose measurements

• Sets self.q_measured with corresponding joint configurations

• Applies data filtering if self.del_list_ is specified

Prerequisites:

• self._data_path must be set to valid measurement data location

• Robot model must be initialized

• Calibration parameters must be loaded

Raises:

• FileNotFoundError – If data files are not found at _data_path

• ValueError – If data format is incompatible or corrupted

• AttributeError – If required attributes are not initialized

• CalibrationError – If data loading fails

See also

load_data: Core data loading and processing function

load_param(config_file: str, setting_type: str = 'calibration')

Load calibration parameters from YAML configuration file.

This method supports both legacy YAML format and the new unified configuration format. It automatically detects the format type and applies the appropriate parser.

Parameters:

• config_file (str) – Path to configuration file (legacy or unified)

• setting_type (str) – Configuration section to load

plot_3d_poses(INCLUDE_UNCALIB: bool = False)

Plot 3D poses comparing measured vs estimated poses.

Parameters:

INCLUDE_UNCALIB (bool) – Whether to include uncalibrated poses

plot_errors_distribution()

Plot error distribution analysis for calibration assessment.

Creates bar plots showing pose error magnitudes across all samples and markers. This visualization helps identify problematic measurements, assess calibration quality, and detect outliers in the dataset.

The plots display error magnitudes (in meters) for each sample, with separate subplots for each marker when multiple markers are used.

Prerequisites:

• Calibration must be completed (STATUS == “CALIBRATED”)

• Error analysis must be computed (self._PEE_dist available)

Side Effects:

• Creates matplotlib figure with error distribution plots

• Figure remains open until explicitly closed or plt.show() called

Raises:

• CalibrationError – If calibration has not been performed

• AttributeError – If error analysis data is not available

See also

plot_3d_poses: 3D visualization of pose comparisons calc_stddev: Error statistics computation

plot_joint_configurations()

Plot joint configurations within range bounds.

plot_results()

Generate comprehensive visualization plots for calibration results.

Creates multiple visualization plots to analyze calibration quality: 1. Error distribution plots showing residual patterns 2. 3D pose visualizations comparing measured vs predicted poses 3. Joint configuration analysis (currently commented)

This method provides essential visual feedback for calibration assessment, helping users understand solution quality and identify potential issues with the calibration process.

Prerequisites:

• Calibration must be completed (solve() called)

• Measurement data must be loaded

• Matplotlib backend must be configured

Side Effects:

• Displays plots using plt.show()

• May block execution until plots are closed

See also

plot_errors_distribution: Individual error analysis plots plot_3d_poses: 3D pose comparison visualization

save_results(output_dir='results')

Save calibration results using unified results manager.

solve(method='lm', max_iterations=3, outlier_threshold=3.0, enable_logging=True, plotting=False, save_results=False)

Execute the complete calibration process.

This is the main entry point for calibration that: 1. Runs the optimization algorithm via solve_optimisation() 2. Optionally generates visualization plots if enabled 3. Optionally saves results to files if enabled

The method serves as a high-level orchestrator for the calibration workflow, delegating the actual optimization to solve_optimisation() and handling visualization based on user preferences.

Side Effects:

• Updates calibration parameters through optimization

• Sets self.STATUS to “CALIBRATED” on successful completion

• May display plots if self.calib_config[“PLOT”] is True

See also

solve_optimisation: Core optimization implementation plot: Visualization and analysis plotting

solve_optimisation(var_init: ndarray | None = None, method: str = 'lm', max_iterations: int = 3, outlier_threshold: float = 3.0, enable_logging: bool = False)

Solve calibration optimization with robust outlier handling.

This method implements a comprehensive optimization strategy: 1. Sets up logging for progress tracking 2. Iteratively removes outliers and re-optimizes 3. Evaluates solution quality with detailed metrics 4. Stores results for further analysis

Parameters:

• var_init (ndarray, optional) – Initial parameter guess. If None, uses zero initialization.

• max_iterations (int) – Maximum outlier removal iterations

• outlier_threshold (float) – Outlier detection threshold (std devs)

• enable_logging (bool) – Whether to enable terminal logging

Raises:

• ValueError – If optimization fails completely

• AssertionError – If required data is not loaded

• CalibrationError – If optimization fails

Side Effects:

• Updates self.LM_result with optimization results

• Updates self.STATUS to “CALIBRATED” on success

• Creates self.evaluation_metrics with quality metrics

• Sets up logging if enabled

calibration_tools

figaroh.calibration.calibration_tools.add_base_name(calib_config)

Add base frame parameters to parameter list.

Updates calib_config[“param_name”] with base frame parameters depending on calibration model type.

Parameters:

calib_config – Parameter dictionary containing: - calib_model: “full_params” or “joint_offset” - param_name: List of parameter names to update

Side Effects:

Modifies calib_config[“param_name”] in place by: - For full_params: Replaces first 6 entries with base parameters - For joint_offset: Prepends base parameters to list

figaroh.calibration.calibration_tools.add_eemarker_frame(frame_name, p, rpy, model, data)

Add a new frame attached to the end-effector.

Creates and adds a fixed frame to the robot model at the end-effector location, typically used for marker or tool frames.

Parameters:

• frame_name (str) – Name for the new frame

• p (ndarray) – 3D position offset from parent frame

• rpy (ndarray) – Roll-pitch-yaw angles for frame orientation

• model (pin.Model) – Robot model to add frame to

• data (pin.Data) – Robot data structure

Returns:

ID of newly created frame

Return type:

int

Note

Currently hardcoded to attach to “arm_7_joint”. This should be made configurable in future versions.

figaroh.calibration.calibration_tools.add_pee_name(calib_config)

Add end-effector marker parameters to parameter list.

Adds parameters for each active measurement DOF of each marker.

Parameters:

calib_config – Parameter dictionary containing: - NbMarkers: Number of markers - measurability: List of booleans for active DOFs - param_name: List of parameter names to update

Side Effects:

Modifies calib_config[“param_name”] in place by appending marker parameters in format: “{param_type}_{marker_num}”

figaroh.calibration.calibration_tools.calc_updated_fkm(model, data, var, q, calib_config, verbose=0)

Update forward kinematics with world frame transformations.

Specialized version that explicitly handles transformations between: 1. World frame to start frame (wMo) 2. Start frame to end frame (oMee) 3. End frame to marker frame (eeMf)

Parameters:

• model (pin.Model) – Robot model to update

• data (pin.Data) – Robot data

• var (ndarray) – Parameter vector matching calib_config[“param_name”]

• q (ndarray) – Joint configurations matrix (n_samples, n_joints)

• calib_config (dict) – Calibration parameters containing: - Frames and parameters from update_forward_kinematics() - NbMarkers=1 (only supports single marker)

• verbose (int, optional) – Print update info. Defaults to 0.

Returns:

Flattened marker measurements in world frame

Return type:

ndarray

Notes

• Excludes joint elasticity effects

• Requires base or end-effector parameters in param_name

• Validates all parameters are properly updated

figaroh.calibration.calibration_tools.calculate_base_kinematics_regressor(q, model, data, calib_config, tol_qr=1e-08)

Calculate base regressor matrix for calibration parameters.

Identifies base (identifiable) parameters by: 1. Computing regressors with random/given configurations 2. Eliminating unidentifiable parameters 3. Finding independent regressor columns

Parameters:

• q (ndarray) – Joint configurations matrix

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• calib_config (dict) – Contains calibration settings: - free_flyer: Whether base is floating - calib_model: Either “joint_offset” or “full_params”

• tol_qr (float, optional) – QR decomposition tolerance. Defaults to TOL_QR.

Returns:

• Rrand_b (ndarray): Base regressor from random configs

• R_b (ndarray): Base regressor from given configs

• R_e (ndarray): Full regressor after eliminating unidentifiable params

• paramsrand_base (list): Names of base parameters from random configs

• paramsrand_e (list): Names of identifiable parameters

Return type:

tuple

Side Effects:

• Updates calib_config[“param_name”] with identified base parameters

• Prints regressor matrix shapes

figaroh.calibration.calibration_tools.calculate_identifiable_kinematics_model(q, model, data, calib_config)

Calculate identifiable Jacobian and regressor matrices.

Builds aggregated Jacobian and regressor matrices from either: 1. Given set of configurations, or 2. Random configurations if none provided

Parameters:

• q (ndarray, optional) – Joint configurations matrix. If empty, uses random configs.

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• calib_config (dict) – Parameters containing: - NbSample: Number of configurations - calibration_index: Number of active DOFs - start_frame, end_frame: Frame names - calib_model: Model type

Returns:

Either:

• Joint offset case: Frame Jacobian matrix

• Full params case: Kinematic regressor matrix

Return type:

ndarray

Note

Removes rows corresponding to inactive DOFs and zero elements

figaroh.calibration.calibration_tools.calculate_kinematics_model(q_i, model, data, calib_config)

Calculate Jacobian and kinematic regressor for single configuration.

Computes frame Jacobian and kinematic regressor matrices for tool frame at given joint configuration.

Parameters:

• q_i (ndarray) – Joint configuration vector

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• calib_config (dict) – Parameters containing “IDX_TOOL” frame index

Returns:

• model (pin.Model): Updated model

• data (pin.Data): Updated data

• R (ndarray): (6,6n) Kinematic regressor matrix

• J (ndarray): (6,n) Frame Jacobian matrix

Return type:

tuple

figaroh.calibration.calibration_tools.cartesian_to_SE3(X)

Convert cartesian coordinates to SE3 transformation.

Parameters:

X (ndarray) – (6,) array with [x,y,z,rx,ry,rz] coordinates

Returns:

SE3 transformation with:

• translation from X[0:3]

• rotation matrix from RPY angles X[3:6]

Return type:

pin.SE3

figaroh.calibration.calibration_tools.get_fullparam_offset(joint_names)

Get dictionary of geometric parameter variations.

Creates mapping of geometric offset parameters for each joint’s position and orientation.

Parameters:

joint_names – List of joint names from robot model

Returns:

Mapping of geometric parameter names to initial zero values.

Keys have format: “d_{param}_{joint_name}” where param is: - px, py, pz: Position offsets - phix, phiy, phiz: Orientation offsets

Return type:

dict

Example

>>> geo_params = get_fullparam_offset(robot.model.names[1:])
>>> print(geo_params["d_px_joint1"])
0.0

figaroh.calibration.calibration_tools.get_idxq_from_jname(model, joint_name)

Get index of joint in configuration vector.

Parameters:

• model (pin.Model) – Robot model

• joint_name (str) – Name of joint to find index for

Returns:

Index of joint in configuration vector q

Return type:

int

Raises:

AssertionError – If joint name does not exist in model

figaroh.calibration.calibration_tools.get_joint_offset(model, joint_names)

Get dictionary of joint offset parameters.

Maps joint names to their offset parameters, handling special cases for different joint types and multiple DOF joints.

Parameters:

• model – Pinocchio robot model

• joint_names – List of joint names from model.names

Returns:

Mapping of joint offset parameter names to initial zero values.

Keys have format: “{offset_type}_{joint_name}”

Return type:

dict

Example

>>> offsets = get_joint_offset(robot.model, robot.model.names[1:])
>>> print(offsets["offsetRZ_joint1"])
0.0

figaroh.calibration.calibration_tools.get_param_from_yaml(robot, calib_data) → dict

Parse calibration parameters from YAML configuration file.

Processes robot and calibration data to build a parameter dictionary containing all necessary settings for robot calibration. Handles configuration of: - Frame identifiers and relationships - Marker/measurement settings - Joint indices and configurations - Non-geometric parameters - Eye-hand calibration setup

Parameters:

• robot (pin.RobotWrapper) – Robot instance containing model and data

• calib_data (dict) – Calibration parameters parsed from YAML file containing: - markers: List of marker configurations - calib_level: Calibration model type - base_frame: Starting frame name - tool_frame: End frame name - free_flyer: Whether base is floating - non_geom: Whether to include non-geometric params

Returns:

Parameter dictionary containing:

• robot_name: Name of robot model

• NbMarkers: Number of markers

• measurability: Measurement DOFs per marker

• start_frame, end_frame: Frame names

• base_to_ref_frame: Optional camera frame

• IDX_TOOL: Tool frame index

• actJoint_idx: Active joint indices

• param_name: List of parameter names

• Additional settings from YAML

Return type:

dict

Side Effects:

Prints warning messages if optional frames undefined Prints final parameter dictionary

Example

>>> calib_data = yaml.safe_load(config_file)
>>> params = get_param_from_yaml(robot, calib_data)
>>> print(params['NbMarkers'])
2

figaroh.calibration.calibration_tools.get_param_from_yaml_legacy(robot, calib_data) → dict

Legacy calibration parameter parser - kept for backward compatibility.

This is the original implementation. New code should use the unified config parser from figaroh.utils.config_parser.

Parameters:

• robot – Robot instance

• calib_data – Calibration data dictionary

Returns:

Calibration configuration dictionary

figaroh.calibration.calibration_tools.get_param_from_yaml_unified(robot, calib_data) → dict

Enhanced parameter parser using unified configuration system.

This function provides backward compatibility while using the new unified configuration parser when possible.

Parameters:

• robot – Robot instance

• calib_data – Configuration data (dict or file path)

Returns:

Calibration configuration dictionary

figaroh.calibration.calibration_tools.get_param_from_yaml_with_warning(robot, calib_data) → dict

Original function with deprecation notice.

figaroh.calibration.calibration_tools.get_rel_jac(model, data, start_frame, end_frame, q)

Calculate relative Jacobian matrix between two frames.

Computes the difference between Jacobians of end_frame and start_frame, giving the differential mapping from joint velocities to relative spatial velocity.

Parameters:

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• start_frame (str) – Starting frame name

• end_frame (str) – Target frame name

• q (ndarray) – Joint configuration vector

Returns:

(6, n) relative Jacobian matrix where:

• Rows represent [dx,dy,dz,wx,wy,wz] spatial velocities

• Columns represent joint velocities

• n is number of joints

Return type:

ndarray

Note

Updates forward kinematics before computing Jacobians

figaroh.calibration.calibration_tools.get_rel_kinreg(model, data, start_frame, end_frame, q)

Calculate relative kinematic regressor between frames.

Computes frame Jacobian-based regressor matrix mapping small joint displacements to spatial velocities.

Parameters:

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• start_frame (str) – Starting frame name

• end_frame (str) – Target frame name

• q (ndarray) – Joint configuration vector

Returns:

(6, 6n) regressor matrix for n joints

Return type:

ndarray

figaroh.calibration.calibration_tools.get_rel_transform(model, data, start_frame, end_frame)

Get relative transformation between two frames.

Calculates the transform from start_frame to end_frame in the kinematic chain. Assumes forward kinematics has been updated.

Parameters:

• model (pin.Model) – Robot model

• data (pin.Data) – Robot data

• start_frame (str) – Starting frame name

• end_frame (str) – Target frame name

Returns:

Relative transformation sMt from start to target frame

Return type:

pin.SE3

Raises:

AssertionError – If frame names don’t exist in model

figaroh.calibration.calibration_tools.get_sup_joints(model, start_frame, end_frame)

Get list of supporting joints between two frames in kinematic chain.

Finds all joints that contribute to relative motion between start_frame and end_frame by analyzing their support branches in the kinematic tree.

Parameters:

• model (pin.Model) – Robot model

• start_frame (str) – Name of starting frame

• end_frame (str) – Name of ending frame

Returns:

Joint IDs ordered from start to end frame, handling cases:

1. Branch entirely contained in another branch

2. Disjoint branches with fixed root

3. Partially overlapping branches

Return type:

list[int]

Raises:

AssertionError – If end frame appears before start frame in chain

Note

Excludes “universe” joints from returned list since they don’t contribute to relative motion.

figaroh.calibration.calibration_tools.initialize_variables(calib_config, mode=0, seed=0)

Initialize variables for Levenberg-Marquardt optimization.

Creates initial parameter vector either as zeros or random values within bounds.

Parameters:

• calib_config (dict) – Parameter dictionary containing: - param_name: List of parameter names to initialize

• mode (int, optional) – Initialization mode: - 0: Zero initialization - 1: Random uniform initialization. Defaults to 0.

• seed (float, optional) – Range [-seed,seed] for random init. Defaults to 0.

Returns:

• var (ndarray): Initial parameter vector

• nvar (int): Number of parameters

Return type:

tuple

Example

>>> var, n = initialize_variables(params, mode=1, seed=0.1)
>>> print(var.shape)
(42,)

figaroh.calibration.calibration_tools.load_data(path_to_file, model, calib_config, del_list=[])

Load joint configuration and marker data from CSV file.

Reads marker positions/orientations and joint configurations from a CSV file. Handles data validation, bad sample removal, and conversion to numpy arrays.

Parameters:

• path_to_file (str) – Path to CSV file containing recorded data

• model (pin.Model) – Robot model containing joint information

• calib_config (dict) – Parameter dictionary containing: - NbMarkers: Number of markers to load - measurability: List indicating which DOFs are measured - actJoint_idx: List of active joint indices - config_idx: Configuration vector indices - q0: Default configuration vector

• del_list (list, optional) – Indices of bad samples to remove. Defaults to [].

Returns:

• PEEm_exp (ndarray): Flattened marker measurements of shape (n_active_dofs,)

• q_exp (ndarray): Joint configurations of shape (n_samples, n_joints)

Return type:

tuple

Note

CSV file must contain columns: - For each marker i: [xi, yi, zi, phixi, phiyi, phizi] - Joint names matching model.names for active joints

Raises:

KeyError – If required columns are missing from CSV

Side Effects:

• Prints joint headers

• Updates calib_config[“NbSample”] with number of valid samples

figaroh.calibration.calibration_tools.read_config_data(model, path_to_file)

Read joint configurations from CSV file.

Parameters:

• model (pin.Model) – Robot model containing joint information

• path_to_file (str) – Path to CSV file containing joint configurations

Returns:

Matrix of shape (n_samples, n_joints-1) containing joint positions

Return type:

ndarray

figaroh.calibration.calibration_tools.unified_to_legacy_config(robot, unified_calib_config) → dict

Convert unified configuration format to legacy calib_config format.

Maps the new unified configuration structure to the exact format expected by get_param_from_yaml. This ensures backward compatibility while using the new unified parser.

Parameters:

• robot (pin.RobotWrapper) – Robot instance containing model and data

• unified_calib_config (dict) – Configuration from create_task_config

Returns:

Legacy format calibration configuration matching

get_param_from_yaml output

Return type:

dict

Raises:

KeyError – If required fields are missing from unified config

Example

>>> unified_config = create_task_config(robot, parsed_config,
...                                    "calibration")
>>> legacy_config = unified_to_legacy_config(robot, unified_config)

figaroh.calibration.calibration_tools.update_forward_kinematics(model, data, var, q, calib_config, verbose=0)

Update forward kinematics with calibration parameters.

Applies geometric and kinematic error parameters to update joint placements and compute end-effector poses. Handles: 1. Base/camera transformations 2. Joint placement offsets 3. End-effector marker frames 4. Joint elasticity effects

Parameters:

• model (pin.Model) – Robot model to update

• data (pin.Data) – Robot data

• var (ndarray) – Parameter vector matching calib_config[“param_name”]

• q (ndarray) – Joint configurations matrix (n_samples, n_joints)

• calib_config (dict) – Calibration parameters containing: - calib_model: “full_params” or “joint_offset” - start_frame, end_frame: Frame names - actJoint_idx: Active joint indices - measurability: Active DOFs

• verbose (int, optional) – Print update info. Defaults to 0.

Returns:

Flattened end-effector measurements for all samples

Return type:

ndarray

Side Effects:

• Modifies model joint placements temporarily

• Reverts model to original state before returning

figaroh.calibration.calibration_tools.update_joint_placement(model, joint_idx, xyz_rpy)

Update joint placement with offset parameters.

Modifies a joint’s placement transform by adding position and orientation offsets.

Parameters:

• model (pin.Model) – Robot model to modify

• joint_idx (int) – Index of joint to update

• xyz_rpy (ndarray) – (6,) array of offsets: - xyz_rpy[0:3]: Translation offsets (x,y,z) - xyz_rpy[3:6]: Rotation offsets (roll,pitch,yaw)

Returns:

Updated robot model

Return type:

pin.Model

Side Effects:

Modifies model.jointPlacements[joint_idx] in place

figaroh.calibration.calibration_tools.xyzquat_to_SE3(xyzquat)

Convert XYZ position and quaternion orientation to SE3 transformation.

Takes a 7D vector containing XYZ position and WXYZ quaternion and creates an SE3 transformation matrix.

Parameters:

xyzquat (ndarray) – (7,) array containing: - xyzquat[0:3]: XYZ position coordinates - xyzquat[3:7]: WXYZ quaternion orientation

Returns:

Rigid body transformation with:

• Translation from XYZ position

• Rotation matrix from normalized quaternion

Return type:

pin.SE3

Example

>>> pos_quat = np.array([0.1, 0.2, 0.3, 1.0, 0, 0, 0])
>>> transform = xyzquat_to_SE3(pos_quat)