Calibration

The calibration module provides tools for geometric calibration of robots, including the reporting & verification suite (print_quality_report(), export_html_report(), verify(), export_verification_report()) attached directly to BaseCalibration.

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.

BaseCalibration(robot, config_file, del_list=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

Attributes:

Name	Type	Description
STATUS	str	Current calibration status ("NOT CALIBRATED" or "CALIBRATED")
LM_result	str	Optimization result from scipy.optimize.least_squares
var_	ndarray	Calibrated parameter values
evaluation_metrics	dict	Solution quality metrics
std_dev	list	Standard deviations of calibrated parameters
std_pctg	list	Standard deviation percentages
PEE_measured	ndarray	Measured end-effector poses/positions
q_measured	ndarray	Measured joint configurations
calib_config	dict	Calibration parameters and configuration
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) ... # Use body-frame position (or position_frame="world" for world frame) ... residuals = self._compute_logmap_residuals( ... self.PEE_measured, PEEe, position_frame="body") ... 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

Initialize robot calibration framework.

Sets up the calibration environment by loading robot model, configuration parameters, and preparing internal data structures for optimization.

Parameters:

Name	Type	Description	Default
robot		Robot object containing kinematic model and data structures. Must have 'model' and 'data' attributes compatible with Pinocchio library.	required
config_file	str	Path to YAML configuration file containing calibration parameters, data paths, and settings.	required
del_list	list	Indices of bad/outlier samples to exclude from calibration data. Defaults to [].	None

Raises:

Type	Description
FileNotFoundError	If config_file does not exist
KeyError	If required parameters missing from configuration
ValueError	If configuration parameters are invalid
CalibrationError	If robot or configuration is invalid

Side Effects

• Loads and validates configuration parameters
• Sets initial calibration status to "NOT CALIBRATED"
• Calculates number of calibration variables
• Resolves absolute path to measurement data file

Example

robot = load_robot_model("tiago.urdf") calibrator = TiagoCalibration(robot, "tiago_config.yaml", ... del_list=[5, 12, 18])

Source code in src/figaroh/calibration/base_calibration.py

@handle_calibration_errors
def __init__(self, robot, config_file: str, del_list: List[int] = None):
    """Initialize robot calibration framework.

    Sets up the calibration environment by loading robot model,
    configuration parameters, and preparing internal data structures
    for optimization.

    Args:
        robot: Robot object containing kinematic model and data structures.
              Must have 'model' and 'data' attributes compatible with
              Pinocchio library.
        config_file (str): Path to YAML configuration file containing
                         calibration parameters, data paths, and settings.
        del_list (list, optional): Indices of bad/outlier samples to
                                 exclude from calibration data.
                                 Defaults to [].

    Raises:
        FileNotFoundError: If config_file does not exist
        KeyError: If required parameters missing from configuration
        ValueError: If configuration parameters are invalid
        CalibrationError: If robot or configuration is invalid

    Side Effects:
        - Loads and validates configuration parameters
        - Sets initial calibration status to "NOT CALIBRATED"
        - Calculates number of calibration variables
        - Resolves absolute path to measurement data file

    Example:
        >>> robot = load_robot_model("tiago.urdf")
        >>> calibrator = TiagoCalibration(robot, "tiago_config.yaml",
        ...                              del_list=[5, 12, 18])
    """
    if del_list is None:
        del_list = []

    # Validate inputs
    if not hasattr(robot, "model") or not hasattr(robot, "data"):
        raise CalibrationError("Robot must have 'model' and 'data' attributes")

    self.robot = robot
    self.model = self.robot.model
    self.data = self.robot.data
    self.del_list_ = del_list
    self.calib_config = None
    self.load_param(config_file)
    self.nvars = len(self.calib_config["param_name"])
    self._data_path = abspath(self.calib_config["data_file"])
    self.STATUS = "NOT CALIBRATED"
    self._val_available = False

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:

Type	Description
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")

Source code in src/figaroh/calibration/base_calibration.py

def initialize(self):
    """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")
    """
    try:
        self.load_data_set()
        self.create_param_list()
    except Exception as e:
        raise CalibrationError(f"Initialization failed: {e}")

solve(method='lm', max_iterations=3, outlier_threshold=3.0, enable_logging=True, plotting=False, save_results=False, html_report=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.

Parameters:

Name	Type	Description	Default
html_report		If True, also export an HTML diagnostic report (see :meth:export_html_report) after the terminal quality report is printed.	False

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 export_html_report: Visual counterpart of the terminal report

Source code in src/figaroh/calibration/base_calibration.py

def solve(
    self,
    method="lm",
    max_iterations=3,
    outlier_threshold=3.0,
    enable_logging=True,
    plotting=False,
    save_results=False,
    html_report=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.

    Args:
        html_report: If True, also export an HTML diagnostic report
            (see :meth:`export_html_report`) after the terminal
            quality report is printed.

    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
        export_html_report: Visual counterpart of the terminal report
    """
    self._run_started_at = datetime.now(timezone.utc).isoformat()
    result, outlier_indices = self.solve_optimisation(
        method=method,
        max_iterations=max_iterations,
        outlier_threshold=outlier_threshold,
        enable_logging=enable_logging,
    )

    # Evaluate solution
    evaluation = self._evaluate_solution(result, outlier_indices)
    self._run_finished_at = datetime.now(timezone.utc).isoformat()

    # Log final results
    if enable_logging:
        logger.info("=" * 30)
        logger.info("FINAL CALIBRATION RESULTS")
        logger.info("=" * 30)
        self._log_iteration_results("FINAL", result, evaluation)

        if len(outlier_indices) > 0:
            logger.info(f"Outlier samples: {outlier_indices}")
        logger.info("Calibration completed successfully!")

    # Store results
    self._store_optimization_results(result, evaluation, outlier_indices)

    # Print quality report
    self.print_quality_report()

    # Generate plots if required
    if plotting:
        self.plot_results()
    if save_results:
        self.save_results()
    if html_report:
        self.export_html_report()
    return result

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

Source code in src/figaroh/calibration/base_calibration.py

def plot_results(self):
    """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
    """

    def _basic_plots():
        try:
            self.plot_errors_distribution()
            self.plot_3d_poses()
            # self.plot_joint_configurations()
            plt.show()
        except Exception as e:
            logger.warning(f"Plotting failed: {e}")

    # Use pre-initialized results manager if available, else go straight
    # to the basic-plotting fallback.
    if hasattr(self, "results_manager") and self.results_manager is not None:
        plot_with_fallback(
            lambda: self.results_manager.plot_calibration_results(),
            _basic_plots,
            logger,
            "calibration",
        )
    else:
        _basic_plots()

load_param(config_file, setting_type='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:

Name	Type	Description	Default
config_file	str	Path to configuration file (legacy or unified)	required
setting_type	str	Configuration section to load	'calibration'

Source code in src/figaroh/calibration/base_calibration.py

def load_param(self, 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.

    Args:
        config_file (str): Path to configuration file (legacy or unified)
        setting_type (str): Configuration section to load
    """
    self._config_file_path = config_file
    try:
        logger.info(f"Loading config from {config_file}")

        # Check if this is a unified configuration format
        if is_unified_config(config_file):
            logger.info("Detected unified configuration format")
            # Use unified parser
            parser = UnifiedConfigParser(config_file)
            unified_config = parser.parse()
            unified_calib_config = create_task_config(
                self.robot, unified_config, setting_type
            )
            # Convert unified format to legacy calib_config format
            self.calib_config = unified_to_legacy_config(
                self.robot, unified_calib_config
            )
        else:
            logger.info("Detected legacy configuration format")
            # Use legacy format parsing
            with open(config_file, "r") as f:
                config = yaml.load(f, Loader=SafeLoader)

            if setting_type not in config:
                raise KeyError(f"Setting type '{setting_type}' not found in config")

            calib_data = config[setting_type]
            self.calib_config = get_param_from_yaml(self.robot, calib_data)

    except FileNotFoundError:
        raise CalibrationError(f"Configuration file not found: {config_file}")
    except Exception as e:
        raise CalibrationError(f"Failed to load configuration: {e}")

create_param_list(q=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:

Name	Type	Description	Default
q	array_like	Joint configuration for regressor computation. If None, uses empty list which may limit regressor accuracy	None

Returns:

Name	Type	Description
bool		Always returns True to indicate successful completion

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:

Type	Description
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

Source code in src/figaroh/calibration/base_calibration.py

def create_param_list(self, q: Optional[np.ndarray] = 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

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

    Returns:
        bool: Always returns True to indicate successful completion

    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
    """
    if q is None:
        q_ = []
    else:
        q_ = q

    try:
        (
            Rrand_b,
            R_b,
            R_e,
            paramsrand_base,
            paramsrand_e,
        ) = calculate_base_kinematics_regressor(
            q_, self.model, self.data, self.calib_config, tol_qr=1e-6
        )

        if self.calib_config["known_baseframe"] is False:
            add_base_name(self.calib_config)
        if self.calib_config["known_tipframe"] is False:
            add_pee_name(self.calib_config)

        return True

    except Exception as e:
        raise CalibrationError(f"Parameter list creation failed: {e}")

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:

Type	Description
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

Source code in src/figaroh/calibration/base_calibration.py

def load_data_set(self):
    """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
    """
    try:
        self.PEE_measured, self.q_measured = load_data(
            self._data_path, self.model, self.calib_config, self.del_list_
        )
    except Exception as e:
        raise CalibrationError(f"Data loading failed: {e}")

    # If validation data path is specified in config, load it
    val_data_path = self.calib_config.get("validation_data_file")
    if val_data_path:
        try:
            self._load_validation_data(val_data_path)
        except Exception:
            pass  # Don't fail calibration if validation data unavailable

get_pose_from_measure(res_)

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:

Name	Type	Description	Default
res_	ndarray	Calibrated parameter vector containing kinematic corrections (geometric parameters, base transform, tool transform, etc.)	required

Returns:

Name	Type	Description
ndarray	ndarray	Predicted end-effector poses corresponding to the measured joint configurations. Shape depends on the number of measurements and pose representation format.

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

Source code in src/figaroh/calibration/base_calibration.py

def get_pose_from_measure(self, res_: np.ndarray) -> np.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.

    Args:
        res_ (ndarray): Calibrated parameter vector containing kinematic
                      corrections (geometric parameters, base transform,
                      tool transform, etc.)

    Returns:
        ndarray: Predicted end-effector poses corresponding to the
                measured joint configurations. Shape depends on the
                number of measurements and pose representation format.

    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
    """
    return calc_updated_fkm(
        self.model, self.data, res_, self.q_measured, self.calib_config
    )

cost_function(var)

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:

Name	Type	Description	Default
var	ndarray	Parameter vector to evaluate	required

Returns:

Name	Type	Description
ndarray	ndarray	Residual vector

Warning

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

Example implementations:

Body-frame position (default, geometrically correct):
    >>> raw_residuals = self._compute_logmap_residuals(
    ...     self.PEE_measured, PEEe, position_frame="body")

World-frame position (more interpretable):
    >>> raw_residuals = self._compute_logmap_residuals(
    ...     self.PEE_measured, PEEe, position_frame="world")

Then apply weighting and regularization:
    >>> weighted_residuals = self.apply_measurement_weighting(
    ...     raw_residuals, pos_weight=1000.0, orient_weight=100.0)

Source code in src/figaroh/calibration/base_calibration.py

def cost_function(self, var: np.ndarray) -> np.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.

    Args:
        var (ndarray): Parameter vector to evaluate

    Returns:
        ndarray: Residual vector

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

    Example implementations:

        Body-frame position (default, geometrically correct):
            >>> raw_residuals = self._compute_logmap_residuals(
            ...     self.PEE_measured, PEEe, position_frame="body")

        World-frame position (more interpretable):
            >>> raw_residuals = self._compute_logmap_residuals(
            ...     self.PEE_measured, PEEe, position_frame="world")

        Then apply weighting and regularization:
            >>> weighted_residuals = self.apply_measurement_weighting(
            ...     raw_residuals, pos_weight=1000.0, orient_weight=100.0)
    """
    import warnings

    # Issue warning about using default implementation
    warnings.warn(
        f"Using default cost function for {self.__class__.__name__}. "
        "Consider implementing a robot-specific cost function with "
        "appropriate weighting and regularization for optimal "
        "performance.",
        UserWarning,
        stacklevel=2,
    )

    # Default implementation: basic residual calculation using SE3 log map
    PEEe = calc_updated_fkm(
        self.model, self.data, var, self.q_measured, self.calib_config
    )
    raw_residuals = self._compute_logmap_residuals(self.PEE_measured, PEEe)

    # Apply basic measurement weighting if configuration is available
    try:
        weighted_residuals = self.apply_measurement_weighting(raw_residuals)
        return weighted_residuals
    except (KeyError, AttributeError):
        # Fallback to unweighted residuals if weighting config unavailable
        return raw_residuals

apply_measurement_weighting(residuals, pos_weight=None, orient_weight=None)

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:

Name	Type	Description	Default
residuals	ndarray	Raw residual vector	required
pos_weight	float	Weight for position residuals. If None, uses 1/position_std	None
orient_weight	float	Weight for orientation residuals. If None, uses 1/orientation_std	None

Returns:

Name	Type	Description
ndarray	ndarray	Weighted residual vector

Example

In derived class cost_function:

raw_residuals = self._compute_logmap_residuals( ... self.PEE_measured, PEEe, ... position_frame="body") # or "world" for world-frame position weighted_residuals = self.apply_measurement_weighting( ... raw_residuals, pos_weight=1000.0, orient_weight=100.0)

Source code in src/figaroh/calibration/base_calibration.py

def apply_measurement_weighting(
    self,
    residuals: np.ndarray,
    pos_weight: Optional[float] = None,
    orient_weight: Optional[float] = None,
) -> np.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.

    Args:
        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:
        ndarray: Weighted residual vector

    Example:
        >>> # In derived class cost_function:
        >>> raw_residuals = self._compute_logmap_residuals(
        ...     self.PEE_measured, PEEe,
        ...     position_frame="body")  # or "world" for world-frame position
        >>> weighted_residuals = self.apply_measurement_weighting(
        ...     raw_residuals, pos_weight=1000.0, orient_weight=100.0)
    """
    # Get weights from parameters or use provided values
    if pos_weight is None:
        pos_std = self.calib_config.get("measurement_std", {}).get(
            "position", 0.001
        )
        pos_weight = 1.0 / pos_std

    if orient_weight is None:
        orient_std = self.calib_config.get("measurement_std", {}).get(
            "orientation", 0.01
        )
        orient_weight = 1.0 / orient_std

    weighted_residuals = []
    residual_idx = 0

    # Process each sample for each marker
    for marker in range(self.calib_config["NbMarkers"]):
        for dof, is_measured in enumerate(self.calib_config["measurability"]):
            if is_measured:
                for sample in range(self.calib_config["NbSample"]):
                    res = residuals[residual_idx]
                    if dof < 3:  # Position components (x,y,z)
                        weighted_residuals.append(res * pos_weight)
                    else:  # Orientation components (rx,ry,rz)
                        weighted_residuals.append(res * orient_weight)
                        # print(f"Residual index: {residual_idx}")
                    residual_idx += 1
    return np.array(weighted_residuals)

solve_optimisation(var_init=None, method='lm', max_iterations=3, outlier_threshold=3.0, enable_logging=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:

Name	Type	Description	Default
var_init	ndarray	Initial parameter guess. If None, uses zero initialization.	None
max_iterations	int	Maximum outlier removal iterations	3
outlier_threshold	float	Outlier detection threshold (std devs)	3.0
enable_logging	bool	Whether to enable terminal logging	False

Raises:

Type	Description
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

Source code in src/figaroh/calibration/base_calibration.py

def solve_optimisation(
    self,
    var_init: Optional[np.ndarray] = 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

    Args:
        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
    """
    # Verify prerequisites
    if not hasattr(self, "PEE_measured"):
        raise CalibrationError("Call load_data_set() first")
    if not hasattr(self, "q_measured"):
        raise CalibrationError("Call load_data_set() first")

    # Setup logging
    if enable_logging:
        logger = self._setup_logging()
        logger.info("Starting calibration optimization")
        logger.info(f"Parameters: {len(self.calib_config['param_name'])}")
        logger.info(f"Parameter names: {self.calib_config['param_name']}")
        logger.info(f"Markers: {self.calib_config['NbMarkers']}")
        logger.info(f"Samples: {self.calib_config['NbSample']}")
        logger.info(f"DOFs: {self.calib_config['calibration_index']}")

    # Initialize parameters
    if var_init is None:
        var_init, _ = initialize_variables(self.calib_config, mode=0)

    try:
        # Run optimization with outlier removal
        result, outlier_indices, final_residuals = (
            self._optimize_with_outlier_removal(
                var_init, method, max_iterations, outlier_threshold
            )
        )

        return result, outlier_indices

    except Exception as e:
        if enable_logging:
            logger = logging.getLogger("calibration")
            logger.error(f"Calibration failed: {str(e)}")
        raise CalibrationError(f"Optimization failed: {str(e)}")

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:

Type	Description
CalibrationError	If calibration has not been performed
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}")

Source code in src/figaroh/calibration/base_calibration.py

def calc_stddev(self, 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}")
    """
    try:
        # self.nvars is set once in __init__, from calib_config["param_name"]
        # *before* initialize()/create_param_list() finishes populating it
        # (e.g. add_base_name/add_pee_name append entries afterward), so it
        # can under-count the actual calibrated parameters. result.x is the
        # solved parameter vector itself — always the true count.
        nvars = len(result.x)
        self.nvars = nvars
        sigma_ro_sq = (result.cost**2) / (
            self.calib_config["NbSample"] * self.calib_config["calibration_index"]
            - nvars
        )
        J = result.jac
        C_param = sigma_ro_sq * np.linalg.pinv(np.dot(J.T, J))
        self._C_param = C_param
        std_dev = []
        std_pctg = []
        for i_ in range(nvars):
            std_dev.append(np.sqrt(C_param[i_, i_]))
            if result.x[i_] != 0:
                std_pctg.append(abs(np.sqrt(C_param[i_, i_]) / result.x[i_]))
            else:
                std_pctg.append(0.0)
        self.std_dev = std_dev
        self.std_pctg = std_pctg
    except Exception as e:
        raise CalibrationError(f"Standard deviation calculation failed: {e}")

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:

Type	Description
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

Source code in src/figaroh/calibration/base_calibration.py

def plot_errors_distribution(self):
    """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
    """
    if self.STATUS != "CALIBRATED":
        raise CalibrationError("Calibration not performed yet")

    fig1, ax1 = plt.subplots(self.calib_config["NbMarkers"], 1)
    colors = ["blue", "red", "yellow", "purple"]

    if self.calib_config["NbMarkers"] == 1:
        ax1.bar(np.arange(self.calib_config["NbSample"]), self._PEE_dist[0, :])
        ax1.set_xlabel("Sample", fontsize=25)
        ax1.set_ylabel("Error (meter)", fontsize=30)
        ax1.tick_params(axis="both", labelsize=30)
        ax1.grid()
    else:
        for i in range(self.calib_config["NbMarkers"]):
            ax1[i].bar(
                np.arange(self.calib_config["NbSample"]),
                self._PEE_dist[i, :],
                color=colors[i],
            )
            ax1[i].set_xlabel("Sample", fontsize=25)
            ax1[i].set_ylabel("Error of marker %s (meter)" % (i + 1), fontsize=25)
            ax1[i].tick_params(axis="both", labelsize=30)
            ax1[i].grid()

plot_3d_poses(INCLUDE_UNCALIB=False)

Plot 3D poses comparing measured vs estimated poses.

Parameters:

Name	Type	Description	Default
INCLUDE_UNCALIB	bool	Whether to include uncalibrated poses	False

Source code in src/figaroh/calibration/base_calibration.py

def plot_3d_poses(self, INCLUDE_UNCALIB: bool = False):
    """Plot 3D poses comparing measured vs estimated poses.

    Args:
        INCLUDE_UNCALIB (bool): Whether to include uncalibrated poses
    """
    if self.STATUS != "CALIBRATED":
        raise CalibrationError("Calibration not performed yet")

    fig2 = plt.figure()
    fig2.suptitle("Visualization of estimated poses and measured pose in Cartesian")
    ax2 = fig2.add_subplot(111, projection="3d")
    PEEm_LM2d = self.PEE_measured.reshape(
        (
            self.calib_config["NbMarkers"] * self.calib_config["calibration_index"],
            self.calib_config["NbSample"],
        )
    )
    PEEe_sol = self.get_pose_from_measure(self.LM_result.x)
    PEEe_sol2d = PEEe_sol.reshape(
        (
            self.calib_config["NbMarkers"] * self.calib_config["calibration_index"],
            self.calib_config["NbSample"],
        )
    )
    PEEe_uncalib = self.get_pose_from_measure(self.uncalib_values)
    PEEe_uncalib2d = PEEe_uncalib.reshape(
        (
            self.calib_config["NbMarkers"] * self.calib_config["calibration_index"],
            self.calib_config["NbSample"],
        )
    )
    for i in range(self.calib_config["NbMarkers"]):
        ax2.scatter3D(
            PEEm_LM2d[i * 3, :],
            PEEm_LM2d[i * 3 + 1, :],
            PEEm_LM2d[i * 3 + 2, :],
            marker="^",
            color="blue",
            label="Measured",
        )
        ax2.scatter3D(
            PEEe_sol2d[i * 3, :],
            PEEe_sol2d[i * 3 + 1, :],
            PEEe_sol2d[i * 3 + 2, :],
            marker="o",
            color="red",
            label="Estimated",
        )
        if INCLUDE_UNCALIB:
            ax2.scatter3D(
                PEEe_uncalib2d[i * 3, :],
                PEEe_uncalib2d[i * 3 + 1, :],
                PEEe_uncalib2d[i * 3 + 2, :],
                marker="x",
                color="green",
                label="Uncalibrated",
            )
        for j in range(self.calib_config["NbSample"]):
            ax2.plot3D(
                [PEEm_LM2d[i * 3, j], PEEe_sol2d[i * 3, j]],
                [PEEm_LM2d[i * 3 + 1, j], PEEe_sol2d[i * 3 + 1, j]],
                [PEEm_LM2d[i * 3 + 2, j], PEEe_sol2d[i * 3 + 2, j]],
                color="red",
            )
            if INCLUDE_UNCALIB:
                ax2.plot3D(
                    [PEEm_LM2d[i * 3, j], PEEe_uncalib2d[i * 3, j]],
                    [
                        PEEm_LM2d[i * 3 + 1, j],
                        PEEe_uncalib2d[i * 3 + 1, j],
                    ],
                    [
                        PEEm_LM2d[i * 3 + 2, j],
                        PEEe_uncalib2d[i * 3 + 2, j],
                    ],
                    color="green",
                )
    ax2.set_xlabel("X - front (meter)")
    ax2.set_ylabel("Y - side (meter)")
    ax2.set_zlabel("Z - height (meter)")
    ax2.grid()
    ax2.legend()

plot_joint_configurations()

Plot joint configurations within range bounds.

Source code in src/figaroh/calibration/base_calibration.py

def plot_joint_configurations(self):
    """Plot joint configurations within range bounds."""
    fig4 = plt.figure()
    fig4.suptitle("Joint configurations with joint bounds")
    ax4 = fig4.add_subplot(111, projection="3d")
    lb = ub = []
    for j in self.calib_config["config_idx"]:
        lb = np.append(lb, self.model.lowerPositionLimit[j])
        ub = np.append(ub, self.model.upperPositionLimit[j])
    q_actJoint = self.q_measured[:, self.calib_config["config_idx"]]
    sample_range = np.arange(self.calib_config["NbSample"])
    for i in range(len(self.calib_config["actJoint_idx"])):
        ax4.scatter3D(q_actJoint[:, i], sample_range, i)
    for i in range(len(self.calib_config["actJoint_idx"])):
        ax4.plot([lb[i], ub[i]], [sample_range[0], sample_range[0]], [i, i])
        ax4.set_xlabel("Angle (rad)")
        ax4.set_ylabel("Sample")
        ax4.set_zlabel("Joint")
        ax4.grid()

save_results(output_dir='results')

Save calibration results using unified results manager.

Source code in src/figaroh/calibration/base_calibration.py

def save_results(self, output_dir="results"):
    """Save calibration results using unified results manager."""
    if not hasattr(self, "result") or self.results_data is None:
        logger.warning("No calibration results to save. Run solve() first.")
        return

    # Use pre-initialized results manager if available
    if hasattr(self, "results_manager") and self.results_manager is not None:
        try:
            # Save using unified manager with self.result data
            saved_files = self.results_manager.save_results(
                output_dir=output_dir, save_formats=["yaml", "csv", "npz"]
            )

            logger.info("Calibration results saved using ResultsManager")
            for fmt, path in saved_files.items():
                logger.info(f"  {fmt}: {path}")

            return saved_files

        except Exception as e:
            logger.error(f"Error saving with ResultsManager: {e}")
            logger.info("Falling back to basic saving...")

export_html_report(output_path=None, output_dir='results')

Export the calibration quality report as a self-contained HTML file — the visual counterpart of :meth:print_quality_report.

Renders the same metrics (convergence, per-DOF residuals, parameter uncertainty, correlation, validation) already computed during :meth:solve, plus an auto-generated "insights" section flagging ill-conditioning, poorly identified parameters, and strongly correlated pairs.

Parameters:

Name	Type	Description	Default
output_path	str	Explicit file path for the report. If omitted, defaults to {output_dir}/calibration_report.html.	None
output_dir	str	Directory used when output_path is omitted.	'results'

Returns:

Name	Type	Description
str	str	The path the report was written to.

Raises:

Type	Description
AttributeError	If called before :meth:solve.

Source code in src/figaroh/calibration/base_calibration.py

def export_html_report(
    self, output_path: str = None, output_dir: str = "results"
) -> str:
    """Export the calibration quality report as a self-contained HTML
    file — the visual counterpart of :meth:`print_quality_report`.

    Renders the same metrics (convergence, per-DOF residuals,
    parameter uncertainty, correlation, validation) already computed
    during :meth:`solve`, plus an auto-generated "insights" section
    flagging ill-conditioning, poorly identified parameters, and
    strongly correlated pairs.

    Args:
        output_path: Explicit file path for the report. If omitted,
            defaults to ``{output_dir}/calibration_report.html``.
        output_dir: Directory used when ``output_path`` is omitted.

    Returns:
        str: The path the report was written to.

    Raises:
        AttributeError: If called before :meth:`solve`.
    """
    if not hasattr(self, "evaluation_metrics"):
        raise AttributeError(
            "No calibration results available. Run solve() first."
        )

    from os import makedirs
    from os.path import join
    from figaroh.tools.report import generate_calibration_report

    if output_path is None:
        makedirs(output_dir, exist_ok=True)
        output_path = join(output_dir, "calibration_report.html")

    generate_calibration_report(self, output_path=output_path)
    logger.info(f"HTML quality report written to {output_path}")
    return output_path

verify(thresholds=None)

Check this calibration's metrics against pass/fail thresholds.

Unlike :meth:print_quality_report/:meth:export_html_report (for a human to read), this returns a machine-checkable :class:~figaroh.tools._report_common.VerificationVerdict a CI script can branch on. Computed entirely from data already gathered during :meth:solve — never raises after a successful solve, and never gates solve() itself (opt-in, called whenever the caller wants a verdict).

Parameters:

Name	Type	Description	Default
thresholds	Optional[Dict[str, Dict[str, Any]]]	Per-metric {"threshold": float, "comparison": "max"|"min"} overrides. Defaults to CALIBRATION_DEFAULT_THRESHOLDS (a 6-DOF arm and a 30-DOF humanoid don't share the same bar — override per robot as needed).	None

Returns:

Name	Type	Description
VerificationVerdict		passed, per-metric checks, the
		raw metrics dict, human-readable insights (the same
		text used by :meth:export_html_report), and metadata
		(git commit, config file hash, timestamp, robot name).

Raises:

Type	Description
AttributeError	If called before :meth:solve.

Source code in src/figaroh/calibration/base_calibration.py

def verify(self, thresholds: Optional[Dict[str, Dict[str, Any]]] = None):
    """Check this calibration's metrics against pass/fail thresholds.

    Unlike :meth:`print_quality_report`/:meth:`export_html_report`
    (for a human to read), this returns a machine-checkable
    :class:`~figaroh.tools._report_common.VerificationVerdict` a CI
    script can branch on. Computed entirely from data already
    gathered during :meth:`solve` — never raises after a successful
    solve, and never gates ``solve()`` itself (opt-in, called
    whenever the caller wants a verdict).

    Args:
        thresholds: Per-metric ``{"threshold": float, "comparison":
            "max"|"min"}`` overrides. Defaults to
            ``CALIBRATION_DEFAULT_THRESHOLDS`` (a 6-DOF arm and a
            30-DOF humanoid don't share the same bar — override per
            robot as needed).

    Returns:
        VerificationVerdict: ``passed``, per-metric ``checks``, the
        raw ``metrics`` dict, human-readable ``insights`` (the same
        text used by :meth:`export_html_report`), and ``metadata``
        (git commit, config file hash, timestamp, robot name).

    Raises:
        AttributeError: If called before :meth:`solve`.
    """
    if not hasattr(self, "evaluation_metrics"):
        raise AttributeError(
            "No calibration results available. Run solve() first."
        )

    from figaroh.tools._report_common import (
        CALIBRATION_DEFAULT_THRESHOLDS,
        evaluate_thresholds,
    )
    from figaroh.tools.report import _build_insights
    from figaroh.tools.provenance import collect_run_provenance

    thresholds = (
        thresholds if thresholds is not None
        else CALIBRATION_DEFAULT_THRESHOLDS
    )

    eval_ = self.evaluation_metrics
    n_samples = self.calib_config.get("NbSample", 0)
    param_names = self.calib_config.get("param_name", [])
    results_data = getattr(self, "results_data", None) or {}
    validation = results_data.get("validation_metrics")

    metrics: Dict[str, float] = {
        "condition_number": eval_.get("condition_number", float("nan")),
        "rmse": eval_.get("rmse", float("nan")),
        "outlier_percentage": eval_.get(
            "outlier_percentage", float("nan")
        ),
    }
    if validation is not None:
        metrics["position_rmse_mm"] = validation.get(
            "pos_rmse_calibrated_mm", float("nan")
        )
        metrics["orientation_rmse_deg"] = validation.get(
            "orient_rmse_calibrated_deg", float("nan")
        )

    verdict = evaluate_thresholds(metrics, thresholds)
    verdict.insights = [
        i["text"]
        for i in _build_insights(eval_, n_samples, param_names, validation)
    ]
    verdict.metadata = getattr(
        self, "_run_provenance", None
    ) or collect_run_provenance(self, "calibration")

    n_dofs = self.calib_config.get("calibration_index", 0)
    dof_names = [
        "X (mm)", "Y (mm)", "Z (mm)",
        "rx (deg)", "ry (deg)", "rz (deg)",
    ][:n_dofs]
    if validation is not None and "error_nominal_per_dof" in validation:
        n_val = validation.get("n_val_samples", 0)
        dof_names = validation.get("dof_names", dof_names)
        verdict.series = {
            "time": list(range(n_val)),
            "dof_names": dof_names,
            "nominal": validation["error_nominal_per_dof"],
            "fitted": validation["error_fitted_per_dof"],
            "measured": {name: [0.0] * n_val for name in dof_names},
        }
    verdict.compat = {
        "dof_names": dof_names,
        "sample_count": n_samples,
        "config_sha256": verdict.metadata.get("config", {}).get("sha256"),
    }
    return verdict

export_verification_report(output_path=None, output_dir='results', thresholds=None)

Write this calibration's :meth:verify verdict as JSON.

Parameters:

Name	Type	Description	Default
output_path	str	Explicit file path. If omitted, defaults to {output_dir}/calibration_verification.json.	None
output_dir	str	Directory used when output_path is omitted.	'results'
thresholds	Optional[Dict[str, Dict[str, Any]]]	Forwarded to :meth:verify.	None

Returns:

Name	Type	Description
str	str	The path the JSON verdict was written to.

Source code in src/figaroh/calibration/base_calibration.py

def export_verification_report(
    self,
    output_path: str = None,
    output_dir: str = "results",
    thresholds: Optional[Dict[str, Dict[str, Any]]] = None,
) -> str:
    """Write this calibration's :meth:`verify` verdict as JSON.

    Args:
        output_path: Explicit file path. If omitted, defaults to
            ``{output_dir}/calibration_verification.json``.
        output_dir: Directory used when ``output_path`` is omitted.
        thresholds: Forwarded to :meth:`verify`.

    Returns:
        str: The path the JSON verdict was written to.
    """
    import dataclasses
    import json
    from os import makedirs
    from os.path import join

    verdict = self.verify(thresholds=thresholds)
    verdict_dict = dataclasses.asdict(verdict)

    results_manager = getattr(self, "results_manager", None)
    if results_manager is not None:
        verdict_dict = results_manager._convert_for_serialization(
            verdict_dict
        )

    if output_path is None:
        makedirs(output_dir, exist_ok=True)
        output_path = join(output_dir, "calibration_verification.json")

    with open(output_path, "w") as f:
        json.dump(verdict_dict, f, indent=2)

    logger.info(f"Verification report written to {output_path}")
    return output_path

print_quality_report()

Print a formatted calibration quality report to the terminal.

Reports convergence, per-DOF residual statistics, validation metrics (if available), parameter uncertainty, and correlations.

Source code in src/figaroh/calibration/base_calibration.py

def print_quality_report(self):
    """Print a formatted calibration quality report to the terminal.

    Reports convergence, per-DOF residual statistics, validation
    metrics (if available), parameter uncertainty, and correlations.
    """
    eval_ = self.evaluation_metrics
    val = (
        self._compute_validation_metrics()
        if hasattr(self, "_compute_validation_metrics")
        else None
    )

    print()
    print("=" * 70)
    print("  CALIBRATION QUALITY REPORT")
    print("=" * 70)

    # ── Convergence ──
    status = (
        "\u2713 converged"
        if eval_["optimization_success"]
        else "\u2717 failed"
    )
    print(
        f"  Convergence:  {status}    "
        f"Iterations: {eval_['n_iterations']}    "
        f"Cost: {eval_['cost']:.6f}"
    )
    print(
        f"  Outliers:     {eval_['n_outliers']} "
        f"/ {self.calib_config['NbSample']} "
        f"({eval_['outlier_percentage']:.1f}%)"
    )

    cond_label = eval_.get("condition_label", "unavailable")
    cond_num = eval_.get("condition_number", float("nan"))
    if not np.isnan(cond_num):
        print(f"  Condition:    {cond_num:.1f} ({cond_label})")
    else:
        print(f"  Condition:    {cond_label}")

    # ── Per-DOF residuals ──
    per_dof = eval_.get("per_dof_stats", {})
    if per_dof and per_dof.get("dof_names"):
        print("-" * 70)
        n = self.calib_config["NbSample"]
        print(f"  Per-DOF Residuals (training set, n={n})")
        names = per_dof["dof_names"]
        means = per_dof.get("mean", [])
        stds = per_dof.get("std", [])
        rmses = per_dof.get("rmse", [])
        maxes = per_dof.get("max_abs", [])
        r2s = per_dof.get("r_squared", [])
        print(
            f"  {'DOF':<12s} {'Mean':>10s} {'Std':>10s} "
            f"{'RMSE':>10s} {'Max':>10s} {'R²':>10s}"
        )
        print(
            f"  {'-'*12} {'-'*10} {'-'*10} "
            f"{'-'*10} {'-'*10} {'-'*10}"
        )
        for i in range(len(names)):
            m = f"{means[i]:10.4f}" if i < len(means) else "         -"
            s = f"{stds[i]:10.4f}" if i < len(stds) else "         -"
            r = f"{rmses[i]:10.4f}" if i < len(rmses) else "         -"
            x = f"{maxes[i]:10.4f}" if i < len(maxes) else "         -"
            q = f"{r2s[i]:10.4f}" if i < len(r2s) else "         -"
            print(f"  {names[i]:<12s} {m} {s} {r} {x} {q}")

    # ── Overall ──
    overall = per_dof.get("overall", {}) if per_dof else {}
    if overall:
        print("-" * 70)
        print("  Overall")
        print(
            f"    Position RMSE:    {overall['pos_rmse_mm']:.2f} mm    "
            f"Orientation RMSE:  {overall['orient_rmse_deg']:.4f} deg"
        )
        print(
            f"    Position max:     {overall['pos_max_mm']:.2f} mm    "
            f"Orientation max:   {overall['orient_max_deg']:.4f} deg"
        )

    # ── Validation ──
    print("-" * 70)
    if val is not None:
        if val.get("validation_source") == "calibration_data_fallback":
            print(
                "  ⚠ WARNING: no separate validation data "
                "provided — falling back to calibration data. "
                "These are NOT an independent generalization test."
            )
            print(f"  Validation (calibration set, n={val['n_val_samples']})")
        else:
            print(f"  Validation (separate set, n={val['n_val_samples']})")
        print(
            f"  {'Metric':<20s} {'Nominal':>10s} "
            f"{'Calibrated':>12s} {'Improvement':>14s}"
        )
        print(f"  {'-'*20} {'-'*10} {'-'*12} {'-'*14}")
        arrow_pos = (
            "\u2193" if val["pos_improvement_pct"] > 0 else "\u2191"
        )
        arrow_orient = (
            "\u2193" if val["orient_improvement_pct"] > 0 else "\u2191"
        )
        print(
            f"  {'Position RMSE':<20s} "
            f"{val['pos_rmse_nominal_mm']:10.2f} mm"
            f"{val['pos_rmse_calibrated_mm']:12.2f} mm"
            f"{val['pos_improvement_pct']:13.1f}%  {arrow_pos}"
        )
        print(
            f"  {'Orientation RMSE':<20s} "
            f"{val['orient_rmse_nominal_deg']:10.4f} deg"
            f"{val['orient_rmse_calibrated_deg']:12.4f} deg"
            f"{val['orient_improvement_pct']:13.1f}%  {arrow_orient}"
        )
        print(
            f"  {'Position max':<20s} "
            f"{val['pos_max_nominal_mm']:10.2f} mm"
            f"{val['pos_max_calibrated_mm']:12.2f} mm"
            f"{val['pos_improvement_pct']:13.1f}%  {arrow_pos}"
        )
        print(
            f"  {'Orientation max':<20s} "
            f"{val['orient_max_nominal_deg']:10.4f} deg"
            f"{val['orient_max_calibrated_deg']:12.4f} deg"
            f"{val['orient_improvement_pct']:13.1f}%  {arrow_orient}"
        )
    else:
        print("  Validation: no separate validation data provided.")
        print(
            "    Collect measurements with random configurations "
            "for FK testing."
        )

    # ── Parameter uncertainty (top 5) ──
    std_pctg = eval_.get("param_stddev_percentage", [])
    std_dev = eval_.get("param_stdev", [])
    param_names = self.calib_config.get("param_name", [])
    if std_pctg and param_names:
        print("-" * 70)
        ranked = sorted(
            zip(param_names, std_dev, std_pctg),
            key=lambda x: x[2],
            reverse=True,
        )[:5]
        n_show = min(5, len(ranked))
        print(
            f"  Parameter Uncertainty (top {n_show} most uncertain)"
        )
        print(
            f"  {'Parameter':<30s} {'Value':>12s} "
            f"{'±σ':>12s} {'σ/|val|':>10s}"
        )
        print(
            f"  {'-'*30} {'-'*12} {'-'*12} {'-'*10}"
        )
        for name, sd, sp in ranked:
            print(
                f"  {name:<30s} {'':>12s} "
                f"{sd:12.6f} {sp:9.1f}%"
            )

    # ── Correlated pairs ──
    corr_pairs = eval_.get("correlated_pairs", [])
    print("-" * 70)
    if corr_pairs:
        print("  Correlated pairs (|\u03c1| > 0.8):")
        for cp in corr_pairs:
            print(
                f"    {cp['param_i']:<30s} \u2194 "
                f"{cp['param_j']:<30s} "
                f"\u03c1 = {cp['correlation']:+.3f}"
            )
    else:
        print(
            "  Parameter correlations: none exceed |\u03c1| > 0.8"
        )

    print("=" * 70)
    print()

calibration_tools

Calibration tools and algorithms for robot kinematic calibration.

This module contains the implementation of calibration algorithms including: - Forward kinematics update functions - Levenberg-Marquardt optimization - Base regressor calculation - Data loading and processing utilities

get_param_from_yaml(robot, calib_data)

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:

Name	Type	Description	Default
robot	RobotWrapper	Robot instance containing model and data	required
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	required

Returns:

Name	Type	Description
dict	dict	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

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

Source code in src/figaroh/calibration/config.py

def 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

    Args:
        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:
        dict: 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

    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
    """
    # NOTE: since joint 0 is universe and it is trivial,
    # indices of joints are different from indices of joint configuration,
    # different from indices of joint velocities
    calib_config = dict()
    robot_name = robot.model.name
    frames = [f.name for f in robot.model.frames]
    calib_config["robot_name"] = robot_name

    # End-effector sensing measurability:
    NbMarkers = len(calib_data["markers"])
    measurability = calib_data["markers"][0]["measure"]
    calib_idx = measurability.count(True)
    calib_config["NbMarkers"] = NbMarkers
    calib_config["measurability"] = measurability
    calib_config["calibration_index"] = calib_idx

    # Calibration model
    calib_config["calib_model"] = calib_data["calib_level"]

    # Get start and end frames
    start_frame = calib_data["base_frame"]
    end_frame = calib_data["tool_frame"]

    # Validate frames exist
    err_msg = "{}_frame {} does not exist"
    if start_frame not in frames:
        raise AssertionError(err_msg.format("Start", start_frame))
    if end_frame not in frames:
        raise AssertionError(err_msg.format("End", end_frame))

    calib_config["start_frame"] = start_frame
    calib_config["end_frame"] = end_frame

    # Handle eye-hand calibration frames
    try:
        base_to_ref_frame = calib_data["base_to_ref_frame"]
        ref_frame = calib_data["ref_frame"]
    except KeyError:
        base_to_ref_frame = None
        ref_frame = None
        logger.warning("base_to_ref_frame and ref_frame are not defined.")

    # Validate base-to-ref frame if provided
    if base_to_ref_frame is not None:
        if base_to_ref_frame not in frames:
            err_msg = "base_to_ref_frame {} does not exist"
            raise AssertionError(err_msg.format(base_to_ref_frame))

    # Validate ref frame if provided
    if ref_frame is not None:
        if ref_frame not in frames:
            err_msg = "ref_frame {} does not exist"
            raise AssertionError(err_msg.format(ref_frame))

    calib_config["base_to_ref_frame"] = base_to_ref_frame
    calib_config["ref_frame"] = ref_frame

    # Get initial poses
    try:
        base_pose = calib_data["base_pose"]
        tip_pose = calib_data["tip_pose"]
    except KeyError:
        base_pose = None
        tip_pose = None
        logger.warning("base_pose and tip_pose are not defined.")

    calib_config["base_pose"] = base_pose
    calib_config["tip_pose"] = tip_pose

    # q0: default zero configuration
    calib_config["q0"] = robot.q0
    calib_config["NbSample"] = calib_data["nb_sample"]

    # IDX_TOOL: frame ID of the tool
    IDX_TOOL = robot.model.getFrameId(end_frame)
    calib_config["IDX_TOOL"] = IDX_TOOL

    # tool_joint: ID of the joint right before the tool's frame (parent)
    tool_joint = robot.model.frames[IDX_TOOL].parentJoint
    calib_config["tool_joint"] = tool_joint

    # indices of active joints: from base to tool_joint
    actJoint_idx = get_sup_joints(robot.model, start_frame, end_frame)
    calib_config["actJoint_idx"] = actJoint_idx

    # indices of joint configuration corresponding to active joints
    config_idx = [robot.model.joints[i].idx_q for i in actJoint_idx]
    calib_config["config_idx"] = config_idx

    # number of active joints
    NbJoint = len(actJoint_idx)
    calib_config["NbJoint"] = NbJoint

    # initialize a list of calibrating parameters name
    param_name = []
    if calib_data["non_geom"]:
        # list of elastic gain parameter names
        elastic_gain = []
        joint_axes = ["PX", "PY", "PZ", "RX", "RY", "RZ"]
        for j_id, joint_name in enumerate(robot.model.names.tolist()):
            if joint_name == "universe":
                axis_motion = "null"
            else:
                # for ii, ax in enumerate(AXIS_MOTION[j_id]):
                #     if ax == 1:
                #         axis_motion = axis[ii]
                shortname = robot.model.joints[
                    j_id
                ].shortname()  # ONLY TAKE PRISMATIC AND REVOLUTE JOINT
                for ja in joint_axes:
                    if ja in shortname:
                        axis_motion = ja
                    elif "RevoluteUnaligned" in shortname:
                        axis_motion = "RZ"  # hard coded fix for canopies

            elastic_gain.append("k_" + axis_motion + "_" + joint_name)
        for i in actJoint_idx:
            param_name.append(elastic_gain[i])
    calib_config["param_name"] = param_name

    calib_config.update(
        {
            "free_flyer": calib_data["free_flyer"],
            "non_geom": calib_data["non_geom"],
            "eps": 1e-3,
            "PLOT": 0,
        }
    )
    try:
        calib_config.update(
            {
                "coeff_regularize": calib_data["coeff_regularize"],
                "data_file": calib_data["data_file"],
                "sample_configs_file": calib_data["sample_configs_file"],
                "outlier_eps": calib_data["outlier_eps"],
            }
        )
    except KeyError:
        calib_config.update(
            {
                "coeff_regularize": None,
                "data_file": None,
                "sample_configs_file": None,
                "outlier_eps": None,
            }
        )
    return calib_config

unified_to_legacy_config(robot, unified_calib_config)

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:

Name	Type	Description	Default
robot	RobotWrapper	Robot instance containing model and data	required
unified_calib_config	dict	Configuration from create_task_config	required

Returns:

Name	Type	Description
dict	dict	Legacy format calibration configuration matching get_param_from_yaml output

Raises:

Type	Description
KeyError	If required fields are missing from unified config
AssertionError	If frame validation fails

Example

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

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        robot (pin.RobotWrapper): Robot instance containing model and data
        unified_calib_config (dict): Configuration from create_task_config

    Returns:
        dict: Legacy format calibration configuration matching
              get_param_from_yaml output

    Raises:
        KeyError: If required fields are missing from unified config
        AssertionError: If frame validation fails

    Example:
        >>> unified_config = create_task_config(robot, parsed_config,
        ...                                    "calibration")
        >>> legacy_config = unified_to_legacy_config(robot, unified_config)
    """
    # Initialize output configuration
    calib_config = {}

    # Extract unified config sections
    joints = unified_calib_config.get("joints", {})
    kinematics = unified_calib_config.get("kinematics", {})
    parameters = unified_calib_config.get("parameters", {})
    measurements = unified_calib_config.get("measurements", {})
    data = unified_calib_config.get("data", {})

    # 1. Extract basic robot information
    calib_config["robot_name"] = robot.model.name
    calib_config["q0"] = robot.q0
    calib_config["instance"] = unified_calib_config.get("instance", {})

    # 2. Extract and validate markers/measurements
    _extract_marker_info(calib_config, measurements)

    # 3. Extract and validate kinematic frames
    _extract_frame_info(calib_config, robot, kinematics)

    # 4. Extract tool frame information
    _extract_tool_info(calib_config, robot, calib_config["end_frame"])

    # 5. Determine active joints
    _determine_active_joints(
        calib_config,
        robot,
        joints,
        calib_config["start_frame"],
        calib_config["end_frame"],
    )

    # 6. Extract poses
    _extract_poses(calib_config, measurements)

    # 7. Extract calibration parameters
    _extract_calibration_params(calib_config, robot, parameters)

    # 8. Extract data configuration
    calib_config["NbSample"] = data.get("number_of_samples", 500)
    calib_config["data_file"] = data.get("source_file")
    calib_config["sample_configs_file"] = data.get("sample_configurations_file")

    return calib_config

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:

Name	Type	Description	Default
model	Model	Robot model	required
start_frame	str	Name of starting frame	required
end_frame	str	Name of ending frame	required

Returns:

Type	Description
	list[int]: 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

Raises:

Type	Description
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.

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        model (pin.Model): Robot model
        start_frame (str): Name of starting frame
        end_frame (str): Name of ending frame

    Returns:
        list[int]: 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

    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.
    """
    start_frameId = model.getFrameId(start_frame)
    end_frameId = model.getFrameId(end_frame)
    start_par = model.frames[start_frameId].parentJoint
    end_par = model.frames[end_frameId].parentJoint
    branch_s = model.supports[start_par].tolist()
    branch_e = model.supports[end_par].tolist()
    # remove 'universe' joint from branches
    if model.names[branch_s[0]] == "universe":
        branch_s.remove(branch_s[0])
    if model.names[branch_e[0]] == "universe":
        branch_e.remove(branch_e[0])

    # find over-lapping joints in two branches
    shared_joints = list(set(branch_s) & set(branch_e))
    # create a list of supporting joints between two frames
    list_1 = [x for x in branch_s if x not in branch_e]
    list_1.reverse()
    list_2 = [y for y in branch_e if y not in branch_s]
    # case 2: root_joint is fixed; branch_s and branch_e are separate
    if shared_joints == []:
        sup_joints = list_1 + list_2
    else:
        # case 1: branch_s is part of branch_e
        if shared_joints == branch_s:
            sup_joints = [branch_s[-1]] + list_2
        else:
            assert shared_joints != branch_e, "End frame should be before start frame."
            # case 3: there are overlapping joints between two branches
            sup_joints = list_1 + [shared_joints[-1]] + list_2
    return sup_joints

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:

Name	Type	Description	Default
model		Pinocchio robot model	required
joint_names		List of joint names from model.names	required

Returns:

Name	Type	Description
dict		Mapping of joint offset parameter names to initial zero values. Keys have format: "{offset_type}_{joint_name}"

Example

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        model: Pinocchio robot model
        joint_names: List of joint names from model.names

    Returns:
        dict: Mapping of joint offset parameter names to initial zero values.
            Keys have format: "{offset_type}_{joint_name}"

    Example:
        >>> offsets = get_joint_offset(robot.model, robot.model.names[1:])
        >>> print(offsets["offsetRZ_joint1"])
        0.0
    """
    joint_off = []
    joint_names = list(model.names[1:])
    joints = list(model.joints[1:])
    assert len(joint_names) == len(
        joints
    ), "Number of jointnames does not match number of joints! Please check\
        imported model."
    for id, joint in enumerate(joints):
        name = joint_names[id]
        shortname = joint.shortname()
        if model.name == "canopies":
            if "RevoluteUnaligned" in shortname:
                shortname = shortname.replace("RevoluteUnaligned", "RZ")
        for i in range(joint.nv):
            if i > 0:
                offset_param = (
                    shortname.replace("JointModel", "offset")
                    + "{}".format(i + 1)
                    + "_"
                    + name
                )
            else:
                offset_param = shortname.replace("JointModel", "offset") + "_" + name
            joint_off.append(offset_param)

    phi_jo = [0] * len(joint_off)  # default zero values
    joint_off = dict(zip(joint_off, phi_jo))
    return joint_off

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:

Name	Type	Description	Default
joint_names		List of joint names from robot model	required

Returns:

Name	Type	Description
dict		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

Example

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

Source code in src/figaroh/calibration/parameter.py

def get_fullparam_offset(joint_names):
    """Get dictionary of geometric parameter variations.

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

    Args:
        joint_names: List of joint names from robot model

    Returns:
        dict: 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

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

    for i in range(len(joint_names)):
        for j in FULL_PARAMTPL:
            # geo_params.append(j + ("_%d" % i))
            geo_params.append(j + "_" + joint_names[i])

    phi_gp = [0] * len(geo_params)  # default zero values
    geo_params = dict(zip(geo_params, phi_gp))
    return geo_params

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:

Name	Type	Description	Default
calib_config		Parameter dictionary containing: - calib_model: "full_params" or "joint_offset" - param_name: List of parameter names to update	required

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        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
    """
    if calib_config["calib_model"] == "full_params":
        calib_config["param_name"][0:6] = BASE_TPL
    elif calib_config["calib_model"] == "joint_offset":
        calib_config["param_name"] = BASE_TPL + calib_config["param_name"]

add_pee_name(calib_config)

Add end-effector marker parameters to parameter list.

Adds parameters for each active measurement DOF of each marker.

Parameters:

Name	Type	Description	Default
calib_config		Parameter dictionary containing: - NbMarkers: Number of markers - measurability: List of booleans for active DOFs - param_name: List of parameter names to update	required

Side Effects

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

Source code in src/figaroh/calibration/parameter.py

def add_pee_name(calib_config):
    """Add end-effector marker parameters to parameter list.

    Adds parameters for each active measurement DOF of each marker.

    Args:
        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}"
    """
    PEE_names = []
    for i in range(calib_config["NbMarkers"]):
        for j, state in enumerate(calib_config["measurability"]):
            if state:
                PEE_names.extend(["{}_{}".format(EE_TPL[j], i + 1)])
    calib_config["param_name"] = calib_config["param_name"] + PEE_names

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:

Name	Type	Description	Default
frame_name	str	Name for the new frame	required
p	ndarray	3D position offset from parent frame	required
rpy	ndarray	Roll-pitch-yaw angles for frame orientation	required
model	Model	Robot model to add frame to	required
data	Data	Robot data structure	required

Returns:

Name	Type	Description
int		ID of newly created frame

Note

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        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:
        int: ID of newly created frame

    Note:
        Currently hardcoded to attach to "arm_7_joint". This should be made
        configurable in future versions.
    """
    p = np.array([0.1, 0.1, 0.1])
    R = pin.rpy.rpyToMatrix(rpy)
    frame_placement = pin.SE3(R, p)

    parent_jointId = model.getJointId("arm_7_joint")
    prev_frameId = model.getFrameId("arm_7_joint")
    ee_frame_id = model.addFrame(
        pin.Frame(
            frame_name,
            parent_jointId,
            prev_frameId,
            frame_placement,
            pin.FrameType(0),
            pin.Inertia.Zero(),
        ),
        False,
    )
    return ee_frame_id

read_config_data(model, path_to_file)

Read joint configurations from CSV file.

Parameters:

Name	Type	Description	Default
model	Model	Robot model containing joint information	required
path_to_file	str	Path to CSV file containing joint configurations	required

Returns:

Name	Type	Description
ndarray		Matrix of shape (n_samples, n_joints-1) containing joint positions

Source code in src/figaroh/calibration/data_loader.py

def read_config_data(model, path_to_file):
    """Read joint configurations from CSV file.

    Args:
        model (pin.Model): Robot model containing joint information
        path_to_file (str): Path to CSV file containing joint configurations

    Returns:
        ndarray: Matrix of shape (n_samples, n_joints-1) containing joint
            positions
    """
    df = pd.read_csv(path_to_file)
    q = np.zeros([len(df), model.njoints - 1])
    for i in range(len(df)):
        for j, name in enumerate(model.names[1:].tolist()):
            jointidx = get_idxq_from_jname(model, name)
            q[i, jointidx] = df[name][i]
    return q

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:

Name	Type	Description	Default
path_to_file	str	Path to CSV file containing recorded data	required
model	Model	Robot model containing joint information	required
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	required
del_list	list	Indices of bad samples to remove. Defaults to [].	[]

Returns:

Name	Type	Description
tuple		PEEm_exp (ndarray): Flattened marker measurements of shape (n_active_dofs,) q_exp (ndarray): Joint configurations of shape (n_samples, n_joints)

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:

Type	Description
KeyError	If required columns are missing from CSV

Side Effects

• Prints joint headers
• Updates calib_config["NbSample"] with number of valid samples

Source code in src/figaroh/calibration/data_loader.py

def 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.

    Args:
        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:
        tuple:
            - PEEm_exp (ndarray): Flattened marker measurements of shape
                (n_active_dofs,)
            - q_exp (ndarray): Joint configurations of shape
                (n_samples, n_joints)

    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
    """
    # read_csv
    df = pd.read_csv(path_to_file)

    # create headers for marker position
    PEE_headers = []
    pee_tpl = ["x", "y", "z", "phix", "phiy", "phiz"]
    for i in range(calib_config["NbMarkers"]):
        for j, state in enumerate(calib_config["measurability"]):
            if state:
                PEE_headers.extend(["{}{}".format(pee_tpl[j], i + 1)])

    # create headers for joint configurations
    joint_headers = [model.names[i] for i in calib_config["actJoint_idx"]]

    # check if all created headers present in csv file
    csv_headers = list(df.columns)
    for header in PEE_headers + joint_headers:
        if header not in csv_headers:
            logger.warning("%s does not exist in the file.", header)
            break

    # Extract marker position/location
    pose_ee = df[PEE_headers].to_numpy()

    # Extract joint configurations
    q_act = df[joint_headers].to_numpy()

    # remove bad data
    if del_list:
        pose_ee = np.delete(pose_ee, del_list, axis=0)
        q_act = np.delete(q_act, del_list, axis=0)

    # update number of data points
    calib_config["NbSample"] = q_act.shape[0]

    PEEm_exp = pose_ee.T
    PEEm_exp = PEEm_exp.flatten("C")

    q_exp = np.empty((calib_config["NbSample"], calib_config["q0"].shape[0]))
    for i in range(calib_config["NbSample"]):
        config = calib_config["q0"]
        config[calib_config["config_idx"]] = q_act[i, :]
        q_exp[i, :] = config

    return PEEm_exp, q_exp

get_idxq_from_jname(model, joint_name)

Get index of joint in configuration vector.

Parameters:

Name	Type	Description	Default
model	Model	Robot model	required
joint_name	str	Name of joint to find index for	required

Returns:

Name	Type	Description
int		Index of joint in configuration vector q

Raises:

Type	Description
AssertionError	If joint name does not exist in model

Source code in src/figaroh/calibration/data_loader.py

def get_idxq_from_jname(model, joint_name):
    """Get index of joint in configuration vector.

    Args:
        model (pin.Model): Robot model
        joint_name (str): Name of joint to find index for

    Returns:
        int: Index of joint in configuration vector q

    Raises:
        AssertionError: If joint name does not exist in model
    """
    assert joint_name in model.names, "Given joint name does not exist."
    jointId = model.getJointId(joint_name)
    joint_idx = model.joints[jointId].idx_q
    return joint_idx

cartesian_to_SE3(X)

Convert cartesian coordinates to SE3 transformation.

Parameters:

Name	Type	Description	Default
X	ndarray	(6,) array with [x,y,z,rx,ry,rz] coordinates	required

Returns:

Type	Description
	pin.SE3: SE3 transformation with: - translation from X[0:3] - rotation matrix from RPY angles X[3:6]

Source code in src/figaroh/calibration/calibration_tools.py

def cartesian_to_SE3(X):
    """Convert cartesian coordinates to SE3 transformation.

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

    Returns:
        pin.SE3: SE3 transformation with:
            - translation from X[0:3]
            - rotation matrix from RPY angles X[3:6]
    """
    X = np.array(X)
    X = X.flatten("C")
    translation = X[0:3]
    rot_matrix = pin.rpy.rpyToMatrix(X[3:6])
    placement = pin.SE3(rot_matrix, translation)
    return placement

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:

Name	Type	Description	Default
xyzquat	ndarray	(7,) array containing: - xyzquat[0:3]: XYZ position coordinates - xyzquat[3:7]: WXYZ quaternion orientation	required

Returns:

Type	Description
	pin.SE3: Rigid body transformation with: - Translation from XYZ position - Rotation matrix from normalized quaternion

Example

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

Source code in src/figaroh/calibration/calibration_tools.py

def 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.

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

    Returns:
        pin.SE3: Rigid body transformation with:
            - Translation from XYZ position
            - Rotation matrix from normalized quaternion

    Example:
        >>> pos_quat = np.array([0.1, 0.2, 0.3, 1.0, 0, 0, 0])
        >>> transform = xyzquat_to_SE3(pos_quat)
    """
    xyzquat = np.array(xyzquat)
    xyzquat = xyzquat.flatten("C")
    translation = xyzquat[0:3]
    rot_matrix = pin.Quaternion(xyzquat[3:7]).normalize().toRotationMatrix()
    placement = pin.SE3(rot_matrix, translation)
    return placement

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:

Name	Type	Description	Default
model	Model	Robot model	required
data	Data	Robot data	required
start_frame	str	Starting frame name	required
end_frame	str	Target frame name	required

Returns:

Type	Description
	pin.SE3: Relative transformation sMt from start to target frame

Raises:

Type	Description
AssertionError	If frame names don't exist in model

Source code in src/figaroh/calibration/calibration_tools.py

def 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.

    Args:
        model (pin.Model): Robot model
        data (pin.Data): Robot data
        start_frame (str): Starting frame name
        end_frame (str): Target frame name

    Returns:
        pin.SE3: Relative transformation sMt from start to target frame

    Raises:
        AssertionError: If frame names don't exist in model
    """
    frames = [f.name for f in model.frames]
    assert start_frame in frames, "{} does not exist.".format(start_frame)
    assert end_frame in frames, "{} does not exist.".format(end_frame)
    start_frameId = model.getFrameId(start_frame)
    oMsf = data.oMf[start_frameId]
    end_frameId = model.getFrameId(end_frame)
    oMef = data.oMf[end_frameId]
    sMef = oMsf.actInv(oMef)
    return sMef

get_rel_kinreg(model, data, start_frame, end_frame, q, backend=None)

Calculate relative kinematic regressor between frames.

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

Parameters:

Name	Type	Description	Default
model	Model	Robot model	required
data	Data	Robot data	required
start_frame	str	Starting frame name	required
end_frame	str	Target frame name	required
q	ndarray	Joint configuration vector	required
backend	DynamicsBackend	If provided, routes forward kinematics calls through the backend abstraction.	None

Returns:

Name	Type	Description
ndarray		(6, 6n) regressor matrix for n joints

Source code in src/figaroh/calibration/calibration_tools.py

def get_rel_kinreg(model, data, start_frame, end_frame, q, backend=None):
    """Calculate relative kinematic regressor between frames.

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

    Args:
        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
        backend (DynamicsBackend, optional): If provided, routes forward kinematics
            calls through the backend abstraction.

    Returns:
        ndarray: (6, 6n) regressor matrix for n joints
    """
    sup_joints = get_sup_joints(model, start_frame, end_frame)
    if backend is not None:
        backend.compute_forward_kinematics(q)
    else:
        pin.framesForwardKinematics(model, data, q)
        pin.updateFramePlacements(model, data)
    kinreg = np.zeros((6, 6 * (model.njoints - 1)))
    frame = model.frames[model.getFrameId(end_frame)]
    oMf = data.oMi[frame.parent] * frame.placement
    for p in sup_joints:
        oMp = data.oMi[model.parents[p]] * model.jointPlacements[p]
        fMp = oMf.actInv(oMp)
        fXp = fMp.toActionMatrix()
        kinreg[:, 6 * (p - 1) : 6 * p] = fXp
    return kinreg

get_rel_jac(model, data, start_frame, end_frame, q, backend=None)

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:

Name	Type	Description	Default
model	Model	Robot model	required
data	Data	Robot data	required
start_frame	str	Starting frame name	required
end_frame	str	Target frame name	required
q	ndarray	Joint configuration vector	required
backend	DynamicsBackend	If provided, routes forward kinematics and Jacobian calls through the backend abstraction.	None

Returns:

Name	Type	Description
ndarray		(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

Note

Updates forward kinematics before computing Jacobians

Source code in src/figaroh/calibration/calibration_tools.py

def get_rel_jac(model, data, start_frame, end_frame, q, backend=None):
    """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.

    Args:
        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
        backend (DynamicsBackend, optional): If provided, routes forward kinematics
            and Jacobian calls through the backend abstraction.

    Returns:
        ndarray: (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

    Note:
        Updates forward kinematics before computing Jacobians
    """
    if backend is not None:
        # compute_jacobian updates FK internally
        J_start = backend.compute_jacobian(q, start_frame)
        J_end = backend.compute_jacobian(q, end_frame)
    else:
        start_frameId = model.getFrameId(start_frame)
        end_frameId = model.getFrameId(end_frame)

        # update frameForwardKinematics and updateFramePlacements
        pin.framesForwardKinematics(model, data, q)
        pin.updateFramePlacements(model, data)

        # relative Jacobian
        J_start = pin.computeFrameJacobian(model, data, q, start_frameId, pin.LOCAL)
        J_end = pin.computeFrameJacobian(model, data, q, end_frameId, pin.LOCAL)
    J_rel = J_end - J_start
    return J_rel

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:

Name	Type	Description	Default
calib_config	dict	Parameter dictionary containing: - param_name: List of parameter names to initialize	required
mode	int	Initialization mode: - 0: Zero initialization - 1: Random uniform initialization. Defaults to 0.	0
seed	float	Range [-seed,seed] for random init. Defaults to 0.	0

Returns:

Name	Type	Description
tuple		var (ndarray): Initial parameter vector nvar (int): Number of parameters

Example

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

Source code in src/figaroh/calibration/calibration_tools.py

def 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.

    Args:
        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:
        tuple:
            - var (ndarray): Initial parameter vector
            - nvar (int): Number of parameters

    Example:
        >>> var, n = initialize_variables(params, mode=1, seed=0.1)
        >>> print(var.shape)
        (42,)
    """
    # initialize all variables at zeros
    nvar = len(calib_config["param_name"])
    if mode == 0:
        var = np.zeros(nvar)
    elif mode == 1:
        var = np.random.uniform(-seed, seed, nvar)
    return var, nvar

update_forward_kinematics(model, data, var, q, calib_config, verbose=0, backend=None)

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:

Name	Type	Description	Default
model	Model	Robot model to update	required
data	Data	Robot data	required
var	ndarray	Parameter vector matching calib_config["param_name"]	required
q	ndarray	Joint configurations matrix (n_samples, n_joints)	required
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	required
verbose	int	Print update info. Defaults to 0.	0
backend	DynamicsBackend	If provided, routes forward kinematics and gravity calls through the backend abstraction.	None

Returns:

Name	Type	Description
ndarray		Flattened end-effector measurements for all samples

Side Effects

• Modifies model joint placements temporarily
• Reverts model to original state before returning

Source code in src/figaroh/calibration/calibration_tools.py

def update_forward_kinematics(
    model, data, var, q, calib_config, verbose=0, backend=None
):
    """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

    Args:
        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.
        backend (DynamicsBackend, optional): If provided, routes forward kinematics
            and gravity calls through the backend abstraction.

    Returns:
        ndarray: Flattened end-effector measurements for all samples

    Side Effects:
        - Modifies model joint placements temporarily
        - Reverts model to original state before returning
    """
    # read calib_config['param_name'] to allocate offset parameters to correct SE3
    # convert translation: add a vector of 3 to SE3.translation
    # convert orientation: convert SE3.rotation 3x3 matrix to vector rpy, add
    #  to vector rpy, convert back to to 3x3 matrix

    # name reference of calibration parameters
    if calib_config["calib_model"] == "full_params":
        axis_tpl = FULL_PARAMTPL
    elif calib_config["calib_model"] == "joint_offset":
        axis_tpl = JOINT_OFFSETTPL

    # order of joint in variables are arranged as in calib_config['actJoint_idx']
    assert len(var) == len(
        calib_config["param_name"]
    ), "Length of variables != length of params"
    param_dict = dict(zip(calib_config["param_name"], var))
    origin_model = model.copy()

    # update model.jointPlacements
    updated_params = []
    start_f = calib_config["start_frame"]
    end_f = calib_config["end_frame"]

    # define transformation for camera frame
    if calib_config["base_to_ref_frame"] is not None:
        start_f = calib_config["ref_frame"]
        # base frame to ref frame (i.e. Tiago: camera transformation)
        base_tf = np.zeros(6)
        for key in param_dict.keys():
            for base_id, base_ax in enumerate(BASE_TPL):
                if base_ax in key:
                    base_tf[base_id] = param_dict[key]
                    updated_params.append(key)
        b_to_cam = get_rel_transform(
            model, data, calib_config["start_frame"], calib_config["base_to_ref_frame"]
        )
        ref_to_cam = cartesian_to_SE3(base_tf)
        cam_to_ref = ref_to_cam.actInv(pin.SE3.Identity())
        bMo = b_to_cam * cam_to_ref
    else:
        if calib_config["calib_model"] == "joint_offset":
            base_tf = np.zeros(6)
            for key in param_dict.keys():
                for base_id, base_ax in enumerate(BASE_TPL):
                    if base_ax in key:
                        base_tf[base_id] = param_dict[key]
                        updated_params.append(key)
            bMo = cartesian_to_SE3(base_tf)

    # update model.jointPlacements with joint 'full_params'/'joint_offset'
    for j_id in calib_config["actJoint_idx"]:
        xyz_rpy = np.zeros(6)
        j_name = model.names[j_id]
        for key in param_dict.keys():
            if j_name in key:
                # update xyz_rpy with kinematic errors
                for axis_id, axis in enumerate(axis_tpl):
                    if axis in key:
                        if verbose == 1:
                            logger.debug(
                                "Updating [{}] joint placement at axis {} with [{}]".format(
                                    j_name, axis, key
                                )
                            )
                        xyz_rpy[axis_id] += param_dict[key]
                        updated_params.append(key)
        model = update_joint_placement(model, j_id, xyz_rpy)
    PEE = np.zeros((calib_config["calibration_index"], calib_config["NbSample"]))

    # update end_effector frame
    for marker_idx in range(1, calib_config["NbMarkers"] + 1):
        pee = np.zeros(6)
        ee_name = "EE"
        for key in param_dict.keys():
            if ee_name in key and str(marker_idx) in key:
                # update xyz_rpy with kinematic errors
                for axis_pee_id, axis_pee in enumerate(EE_TPL):
                    if axis_pee in key:
                        if verbose == 1:
                            logger.debug(
                                "Updating [{}_{}] joint placement at axis {} with [{}]".format(
                                    ee_name, str(marker_idx), axis_pee, key
                                )
                            )
                        pee[axis_pee_id] += param_dict[key]
                        # updated_params.append(key)

        eeMf = cartesian_to_SE3(pee)

    # get transform
    q_ = np.copy(q)
    for i in range(calib_config["NbSample"]):
        if backend is not None:
            backend.compute_forward_kinematics(q_[i, :])
        else:
            pin.framesForwardKinematics(model, data, q_[i, :])
            pin.updateFramePlacements(model, data)
        # update model.jointPlacements with joint elastic error
        if calib_config["non_geom"]:
            if backend is not None:
                tau = backend.compute_gravity_vector(q_[i, :])
            else:
                tau = pin.computeGeneralizedGravity(
                    model, data, q_[i, :]
                )  # vector size of 32 = nq < njoints
            # update xyz_rpy with joint elastic error
            for j_id in calib_config["actJoint_idx"]:
                xyz_rpy = np.zeros(6)
                j_name = model.names[j_id]
                tau_j = tau[j_id - 1]  # nq = njoints -1
                if j_name in key:
                    for elas_id, elas in enumerate(ELAS_TPL):
                        if elas in key:
                            param_dict[key] = param_dict[key] * tau_j
                            xyz_rpy[elas_id + 3] += param_dict[
                                key
                            ]  # +3 to add only on orienation
                            updated_params.append(key)
                model = update_joint_placement(model, j_id, xyz_rpy)
            # get relative transform with updated model
            oMee = get_rel_transform(
                model, data, calib_config["start_frame"], calib_config["end_frame"]
            )
            # revert model back to origin from added joint elastic error
            for j_id in calib_config["actJoint_idx"]:
                xyz_rpy = np.zeros(6)
                j_name = model.names[j_id]
                tau_j = tau[j_id - 1]  # nq = njoints -1
                if j_name in key:
                    for elas_id, elas in enumerate(ELAS_TPL):
                        if elas in key:
                            param_dict[key] = param_dict[key] * tau_j
                            xyz_rpy[elas_id + 3] += param_dict[
                                key
                            ]  # +3 to add only on orienation
                            updated_params.append(key)
                model = update_joint_placement(model, j_id, -xyz_rpy)

        else:
            oMee = get_rel_transform(
                model, data, calib_config["start_frame"], calib_config["end_frame"]
            )

        if len(updated_params) < len(param_dict):

            oMf = oMee * eeMf
            # final transform
            trans = oMf.translation.tolist()
            orient = pin.rpy.matrixToRpy(oMf.rotation).tolist()
            loc = trans + orient
            measure = []
            for mea_id, mea in enumerate(calib_config["measurability"]):
                if mea:
                    measure.append(loc[mea_id])
            # PEE[(marker_idx-1)*calib_config['calibration_index']:marker_idx*calib_config['calibration_index'], i] = np.array(measure)
            PEE[:, i] = np.array(measure)

            # assert len(updated_params) == len(param_dict), "Not all parameters are updated"

    PEE = PEE.flatten("C")
    # revert model back to original
    assert origin_model.jointPlacements != model.jointPlacements, "before revert"
    for j_id in calib_config["actJoint_idx"]:
        xyz_rpy = np.zeros(6)
        j_name = model.names[j_id]
        for key in param_dict.keys():
            if j_name in key:
                # update xyz_rpy
                for axis_id, axis in enumerate(axis_tpl):
                    if axis in key:
                        xyz_rpy[axis_id] = param_dict[key]
        model = update_joint_placement(model, j_id, -xyz_rpy)

    assert origin_model.jointPlacements != model.jointPlacements, "after revert"

    return PEE

calc_updated_fkm(model, data, var, q, calib_config, verbose=0, backend=None)

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:

Name	Type	Description	Default
model	Model	Robot model to update	required
data	Data	Robot data	required
var	ndarray	Parameter vector matching calib_config["param_name"]	required
q	ndarray	Joint configurations matrix (n_samples, n_joints)	required
calib_config	dict	Calibration parameters containing: - Frames and parameters from update_forward_kinematics() - NbMarkers=1 (only supports single marker)	required
verbose	int	Print update info. Defaults to 0.	0
backend	DynamicsBackend	If provided, routes forward kinematics calls through the backend abstraction.	None

Returns:

Name	Type	Description
ndarray		Flattened marker measurements in world frame

Notes

• Excludes joint elasticity effects
• Requires base or end-effector parameters in param_name
• Validates all parameters are properly updated

Source code in src/figaroh/calibration/calibration_tools.py

def calc_updated_fkm(model, data, var, q, calib_config, verbose=0, backend=None):
    """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)

    Args:
        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.
        backend (DynamicsBackend, optional): If provided, routes forward kinematics
            calls through the backend abstraction.

    Returns:
        ndarray: Flattened marker measurements in world frame

    Notes:
        - Excludes joint elasticity effects
        - Requires base or end-effector parameters in param_name
        - Validates all parameters are properly updated
    """

    # name reference of calibration parameters
    if calib_config["calib_model"] == "full_params":
        axis_tpl = FULL_PARAMTPL

    elif calib_config["calib_model"] == "joint_offset":
        axis_tpl = JOINT_OFFSETTPL

    # order of joint in variables are arranged as in calib_config['actJoint_idx']
    assert len(var) == len(
        calib_config["param_name"]
    ), "Length of variables != length of params"
    param_dict = dict(zip(calib_config["param_name"], var))
    origin_model = model.copy()

    # store parameter updated to the model
    updated_params = []

    # check if baseframe and end--effector frame are known
    for key in param_dict.keys():
        if "base" in key:
            base_param_incl = True
            break
        else:
            base_param_incl = False
    for key in param_dict.keys():
        if "EE" in key:
            ee_param_incl = True
            break
        else:
            ee_param_incl = False

    # kinematic chain
    start_f = calib_config["start_frame"]
    end_f = calib_config["end_frame"]

    # if world frame (measurement ref frame) to the start frame is not known,
    # base_tpl needs to be used to define the first 6 parameters

    # 1/ calc transformation from the world frame to start frame: wMo
    if base_param_incl:
        base_tf = np.zeros(6)
        for key in param_dict.keys():
            for base_id, base_ax in enumerate(BASE_TPL):
                if base_ax in key:
                    base_tf[base_id] = param_dict[key]
                    updated_params.append(key)

        wMo = cartesian_to_SE3(base_tf)
    else:
        wMo = pin.SE3.Identity()

    # 2/ calculate transformation from the end frame to the end-effector frame,
    # if not known: eeMf
    if ee_param_incl and calib_config["NbMarkers"] == 1:
        for marker_idx in range(1, calib_config["NbMarkers"] + 1):
            pee = np.zeros(6)
            ee_name = "EE"
            for key in param_dict.keys():
                if ee_name in key and str(marker_idx) in key:
                    # update xyz_rpy with kinematic errors
                    for axis_pee_id, axis_pee in enumerate(EE_TPL):
                        if axis_pee in key:
                            if verbose == 1:
                                logger.debug(
                                    "Updating [{}_{}] joint placement at axis {} with [{}]".format(
                                        ee_name, str(marker_idx), axis_pee, key
                                    )
                                )
                            pee[axis_pee_id] += param_dict[key]
                            updated_params.append(key)

            eeMf = cartesian_to_SE3(pee)
    else:
        if calib_config["NbMarkers"] > 1:
            logger.warning("Multiple markers are not supported.")
        else:
            eeMf = pin.SE3.Identity()

    # 3/ calculate transformation from start frame to end frame of kinematic chain using updated model: oMee

    # update model.jointPlacements with kinematic error parameter
    for j_id in calib_config["actJoint_idx"]:
        xyz_rpy = np.zeros(6)
        j_name = model.names[j_id]

        # check joint name in param dict
        for key in param_dict.keys():
            if j_name in key:

                # update xyz_rpy with kinematic errors based on identifiable axis
                for axis_id, axis in enumerate(axis_tpl):
                    if axis in key:
                        if verbose == 1:
                            logger.debug(
                                "Updating [{}] joint placement at axis {} with [{}]".format(
                                    j_name, axis, key
                                )
                            )
                        xyz_rpy[axis_id] += param_dict[key]
                        updated_params.append(key)

        # updaet joint placement
        model = update_joint_placement(model, j_id, xyz_rpy)

    # check if all parameters are updated to the model
    assert len(updated_params) == len(
        list(param_dict.keys())
    ), "Not all parameters are updated {} and {}".format(
        updated_params, list(param_dict.keys())
    )

    # pose vector of the end-effector
    PEE = np.zeros((calib_config["calibration_index"], calib_config["NbSample"]))

    q_ = np.copy(q)
    for i in range(calib_config["NbSample"]):

        if backend is not None:
            backend.compute_forward_kinematics(q_[i, :])
        else:
            pin.framesForwardKinematics(model, data, q_[i, :])
            pin.updateFramePlacements(model, data)

        # NOTE: joint elastic error is not considered in this version

        oMee = get_rel_transform(model, data, start_f, end_f)

        # calculate transformation from world frame to end-effector frame
        wMee = wMo * oMee
        wMf = wMee * eeMf

        # final transform
        trans = wMf.translation.tolist()
        orient = pin.rpy.matrixToRpy(wMf.rotation).tolist()
        loc = trans + orient
        measure = []
        for mea_id, mea in enumerate(calib_config["measurability"]):
            if mea:
                measure.append(loc[mea_id])
        PEE[:, i] = np.array(measure)

    # final result of updated fkm
    PEE = PEE.flatten("C")

    # revert model back to original
    assert origin_model.jointPlacements != model.jointPlacements, "before revert"
    for j_id in calib_config["actJoint_idx"]:
        xyz_rpy = np.zeros(6)
        j_name = model.names[j_id]
        for key in param_dict.keys():
            if j_name in key:
                # update xyz_rpy
                for axis_id, axis in enumerate(axis_tpl):
                    if axis in key:
                        xyz_rpy[axis_id] = param_dict[key]
        model = update_joint_placement(model, j_id, -xyz_rpy)

    assert origin_model.jointPlacements != model.jointPlacements, "after revert"

    return PEE

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:

Name	Type	Description	Default
model	Model	Robot model to modify	required
joint_idx	int	Index of joint to update	required
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)	required

Returns:

Type	Description
	pin.Model: Updated robot model

Side Effects

Modifies model.jointPlacements[joint_idx] in place

Source code in src/figaroh/calibration/calibration_tools.py

def 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.

    Args:
        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:
        pin.Model: Updated robot model

    Side Effects:
        Modifies model.jointPlacements[joint_idx] in place
    """
    tpl_translation = model.jointPlacements[joint_idx].translation
    tpl_rotation = model.jointPlacements[joint_idx].rotation
    tpl_orientation = pin.rpy.matrixToRpy(tpl_rotation)
    # update axes
    updt_translation = tpl_translation + xyz_rpy[0:3]
    updt_orientation = tpl_orientation + xyz_rpy[3:6]
    updt_rotation = pin.rpy.rpyToMatrix(updt_orientation)
    # update placements
    model.jointPlacements[joint_idx].translation = updt_translation
    model.jointPlacements[joint_idx].rotation = updt_rotation
    return model

calculate_kinematics_model(q_i, model, data, calib_config, backend=None)

Calculate Jacobian and kinematic regressor for single configuration.

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

Parameters:

Name	Type	Description	Default
q_i	ndarray	Joint configuration vector	required
model	Model	Robot model	required
data	Data	Robot data	required
calib_config	dict	Parameters containing "IDX_TOOL" frame index	required
backend	DynamicsBackend	If provided, routes forward kinematics and Jacobian calls through the backend abstraction.	None

Returns:

Name	Type	Description
tuple		model (pin.Model): Updated model data (pin.Data): Updated data R (ndarray): (6,6n) Kinematic regressor matrix J (ndarray): (6,n) Frame Jacobian matrix

Source code in src/figaroh/calibration/calibration_tools.py

def calculate_kinematics_model(q_i, model, data, calib_config, backend=None):
    """Calculate Jacobian and kinematic regressor for single configuration.

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

    Args:
        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
        backend (DynamicsBackend, optional): If provided, routes forward kinematics
            and Jacobian calls through the backend abstraction.

    Returns:
        tuple:
            - model (pin.Model): Updated model
            - data (pin.Data): Updated data
            - R (ndarray): (6,6n) Kinematic regressor matrix
            - J (ndarray): (6,n) Frame Jacobian matrix
    """
    if backend is not None:
        # compute_forward_kinematics updates FK internally
        backend.compute_forward_kinematics(q_i)
        # Convert frame ID to name for backend Jacobian
        frame_id = calib_config["IDX_TOOL"]
        frame_name = model.frames[frame_id].name
        J = backend.compute_jacobian(q_i, frame_name)
    else:
        pin.forwardKinematics(model, data, q_i)
        pin.updateFramePlacements(model, data)
        J = pin.computeFrameJacobian(
            model, data, q_i, calib_config["IDX_TOOL"], pin.LOCAL
        )

    # computeFrameKinematicRegressor has no backend equivalent — use escape hatch
    R = pin.computeFrameKinematicRegressor(
        model, data, calib_config["IDX_TOOL"], pin.LOCAL
    )
    return model, data, R, J

calculate_identifiable_kinematics_model(q, model, data, calib_config, backend=None)

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:

Name	Type	Description	Default
q	ndarray	Joint configurations matrix. If empty, uses random configs.	required
model	Model	Robot model	required
data	Data	Robot data	required
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	required
backend	DynamicsBackend	If provided, routes random configuration and forwards backend to called functions.	None

Returns:

Name	Type	Description
ndarray		Either: - Joint offset case: Frame Jacobian matrix - Full params case: Kinematic regressor matrix

Note

Removes rows corresponding to inactive DOFs and zero elements

Source code in src/figaroh/calibration/calibration_tools.py

def calculate_identifiable_kinematics_model(q, model, data, calib_config, backend=None):
    """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

    Args:
        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
        backend (DynamicsBackend, optional): If provided, routes random configuration
            and forwards backend to called functions.

    Returns:
        ndarray: Either:
            - Joint offset case: Frame Jacobian matrix
            - Full params case: Kinematic regressor matrix

    Note:
        Removes rows corresponding to inactive DOFs and zero elements
    """
    q_temp = np.copy(q)
    # Note if no q id given then use random generation of q to determine the
    # minimal kinematics model
    if np.any(q):
        MIN_MODEL = 0
    else:
        MIN_MODEL = 1

    # obtain aggreated Jacobian matrix J and kinematic regressor R
    calib_idx = calib_config["calibration_index"]
    R = np.zeros([6 * calib_config["NbSample"], 6 * (model.njoints - 1)])
    J = np.zeros([6 * calib_config["NbSample"], model.njoints - 1])
    for i in range(calib_config["NbSample"]):
        if MIN_MODEL == 1:
            if backend is not None:
                q_rand = backend.random_configuration()
            else:
                q_rand = pin.randomConfiguration(model)
            q_i = calib_config["q0"]
            q_i[calib_config["config_idx"]] = q_rand[calib_config["config_idx"]]
        else:
            q_i = q_temp[i, :]
        if calib_config["start_frame"] == "universe":
            model, data, Ri, Ji = calculate_kinematics_model(
                q_i, model, data, calib_config, backend=backend
            )
        else:
            Ri = get_rel_kinreg(
                model,
                data,
                calib_config["start_frame"],
                calib_config["end_frame"],
                q_i,
                backend=backend,
            )
            # Ji = np.zeros([6, model.njoints-1]) ## TODO: get_rel_jac
            Ji = get_rel_jac(
                model,
                data,
                calib_config["start_frame"],
                calib_config["end_frame"],
                q_i,
                backend=backend,
            )
        for j, state in enumerate(calib_config["measurability"]):
            if state:
                R[calib_config["NbSample"] * j + i, :] = Ri[j, :]
                J[calib_config["NbSample"] * j + i, :] = Ji[j, :]
    # remove zero rows
    zero_rows = []
    for r_idx in range(R.shape[0]):
        if np.linalg.norm(R[r_idx, :]) < 1e-6:
            zero_rows.append(r_idx)
    R = np.delete(R, zero_rows, axis=0)
    zero_rows = []
    for r_idx in range(J.shape[0]):
        if np.linalg.norm(J[r_idx, :]) < 1e-6:
            zero_rows.append(r_idx)
    J = np.delete(J, zero_rows, axis=0)

    # select regressor matrix based on calibration model
    if calib_config["calib_model"] == "joint_offset":
        return J
    elif calib_config["calib_model"] == "full_params":
        return R

calculate_base_kinematics_regressor(q, model, data, calib_config, tol_qr=TOL_QR, backend=None)

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:

Name	Type	Description	Default
q	ndarray	Joint configurations matrix	required
model	Model	Robot model	required
data	Data	Robot data	required
calib_config	dict	Contains calibration settings: - free_flyer: Whether base is floating - calib_model: Either "joint_offset" or "full_params"	required
tol_qr	float	QR decomposition tolerance. Defaults to TOL_QR.	TOL_QR
backend	DynamicsBackend	If provided, forwards backend to called functions for backend-aware computation.	None

Returns:

Name	Type	Description
tuple		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

Side Effects

• Updates calib_config["param_name"] with identified base parameters
• Prints regressor matrix shapes

Source code in src/figaroh/calibration/calibration_tools.py

def calculate_base_kinematics_regressor(
    q, model, data, calib_config, tol_qr=TOL_QR, backend=None
):
    """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

    Args:
        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.
        backend (DynamicsBackend, optional): If provided, forwards backend to
            called functions for backend-aware computation.

    Returns:
        tuple:
            - 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

    Side Effects:
        - Updates calib_config["param_name"] with identified base parameters
        - Prints regressor matrix shapes
    """
    # obtain joint names
    joint_names = [name for i, name in enumerate(model.names[1:])]
    geo_params = get_fullparam_offset(joint_names)
    joint_offsets = get_joint_offset(model, joint_names)

    # calculate kinematic regressor with random configs
    if not calib_config["free_flyer"]:
        Rrand = calculate_identifiable_kinematics_model(
            [], model, data, calib_config, backend=backend
        )
    else:
        Rrand = calculate_identifiable_kinematics_model(
            q, model, data, calib_config, backend=backend
        )
    # calculate kinematic regressor with input configs
    if np.any(np.array(q)):
        R = calculate_identifiable_kinematics_model(
            q, model, data, calib_config, backend=backend
        )
    else:
        R = Rrand

    # only joint offset parameters
    if calib_config["calib_model"] == "joint_offset":
        geo_params_sel = joint_offsets

        # select columns corresponding to joint_idx
        Rrand_sel = Rrand

        # select columns corresponding to joint_idx
        R_sel = R

    # full 6 parameters
    elif calib_config["calib_model"] == "full_params":
        geo_params_sel = geo_params
        Rrand_sel = Rrand
        R_sel = R

    # remove non affect columns from random data => reduced regressor
    Rrand_e, paramsrand_e = eliminate_non_dynaffect(
        Rrand_sel, geo_params_sel, tol_e=1e-6
    )

    # indices of independent columns (base param) w.r.t to reduced regressor
    idx_base = get_baseIndex(Rrand_e, paramsrand_e, tol_qr=tol_qr)

    # get base regressor and base params from random data
    Rrand_b, paramsrand_base, _ = get_baseParams(Rrand_e, paramsrand_e, tol_qr=tol_qr)

    # remove non affect columns from GIVEN data
    R_e, params_e = eliminate_non_dynaffect(R_sel, geo_params_sel, tol_e=1e-6)

    # get base param from given data
    # idx_gbase = get_baseIndex(R_e, params_e, tol_qr=tol_qr)
    R_gb, params_gbase, _ = get_baseParams(R_e, params_e, tol_qr=tol_qr)

    # get base regressor from GIVEN data
    R_b = build_baseRegressor(R_e, idx_base)

    # update calibrating calib_config['param_name']/calibrating parameters
    for j in idx_base:
        calib_config["param_name"].append(paramsrand_e[j])

    return Rrand_b, R_b, R_e, paramsrand_base, paramsrand_e

config

Configuration parsing and parameter management for robot calibration.

This module handles all configuration-related functionality including: - YAML configuration file parsing - Unified to legacy config format conversion - Parameter extraction and validation - Frame and joint configuration management

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:

Name	Type	Description	Default
model	Model	Robot model	required
start_frame	str	Name of starting frame	required
end_frame	str	Name of ending frame	required

Returns:

Type	Description
	list[int]: 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

Raises:

Type	Description
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.

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        model (pin.Model): Robot model
        start_frame (str): Name of starting frame
        end_frame (str): Name of ending frame

    Returns:
        list[int]: 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

    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.
    """
    start_frameId = model.getFrameId(start_frame)
    end_frameId = model.getFrameId(end_frame)
    start_par = model.frames[start_frameId].parentJoint
    end_par = model.frames[end_frameId].parentJoint
    branch_s = model.supports[start_par].tolist()
    branch_e = model.supports[end_par].tolist()
    # remove 'universe' joint from branches
    if model.names[branch_s[0]] == "universe":
        branch_s.remove(branch_s[0])
    if model.names[branch_e[0]] == "universe":
        branch_e.remove(branch_e[0])

    # find over-lapping joints in two branches
    shared_joints = list(set(branch_s) & set(branch_e))
    # create a list of supporting joints between two frames
    list_1 = [x for x in branch_s if x not in branch_e]
    list_1.reverse()
    list_2 = [y for y in branch_e if y not in branch_s]
    # case 2: root_joint is fixed; branch_s and branch_e are separate
    if shared_joints == []:
        sup_joints = list_1 + list_2
    else:
        # case 1: branch_s is part of branch_e
        if shared_joints == branch_s:
            sup_joints = [branch_s[-1]] + list_2
        else:
            assert shared_joints != branch_e, "End frame should be before start frame."
            # case 3: there are overlapping joints between two branches
            sup_joints = list_1 + [shared_joints[-1]] + list_2
    return sup_joints

get_param_from_yaml(robot, calib_data)

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:

Name	Type	Description	Default
robot	RobotWrapper	Robot instance containing model and data	required
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	required

Returns:

Name	Type	Description
dict	dict	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

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

Source code in src/figaroh/calibration/config.py

def 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

    Args:
        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:
        dict: 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

    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
    """
    # NOTE: since joint 0 is universe and it is trivial,
    # indices of joints are different from indices of joint configuration,
    # different from indices of joint velocities
    calib_config = dict()
    robot_name = robot.model.name
    frames = [f.name for f in robot.model.frames]
    calib_config["robot_name"] = robot_name

    # End-effector sensing measurability:
    NbMarkers = len(calib_data["markers"])
    measurability = calib_data["markers"][0]["measure"]
    calib_idx = measurability.count(True)
    calib_config["NbMarkers"] = NbMarkers
    calib_config["measurability"] = measurability
    calib_config["calibration_index"] = calib_idx

    # Calibration model
    calib_config["calib_model"] = calib_data["calib_level"]

    # Get start and end frames
    start_frame = calib_data["base_frame"]
    end_frame = calib_data["tool_frame"]

    # Validate frames exist
    err_msg = "{}_frame {} does not exist"
    if start_frame not in frames:
        raise AssertionError(err_msg.format("Start", start_frame))
    if end_frame not in frames:
        raise AssertionError(err_msg.format("End", end_frame))

    calib_config["start_frame"] = start_frame
    calib_config["end_frame"] = end_frame

    # Handle eye-hand calibration frames
    try:
        base_to_ref_frame = calib_data["base_to_ref_frame"]
        ref_frame = calib_data["ref_frame"]
    except KeyError:
        base_to_ref_frame = None
        ref_frame = None
        logger.warning("base_to_ref_frame and ref_frame are not defined.")

    # Validate base-to-ref frame if provided
    if base_to_ref_frame is not None:
        if base_to_ref_frame not in frames:
            err_msg = "base_to_ref_frame {} does not exist"
            raise AssertionError(err_msg.format(base_to_ref_frame))

    # Validate ref frame if provided
    if ref_frame is not None:
        if ref_frame not in frames:
            err_msg = "ref_frame {} does not exist"
            raise AssertionError(err_msg.format(ref_frame))

    calib_config["base_to_ref_frame"] = base_to_ref_frame
    calib_config["ref_frame"] = ref_frame

    # Get initial poses
    try:
        base_pose = calib_data["base_pose"]
        tip_pose = calib_data["tip_pose"]
    except KeyError:
        base_pose = None
        tip_pose = None
        logger.warning("base_pose and tip_pose are not defined.")

    calib_config["base_pose"] = base_pose
    calib_config["tip_pose"] = tip_pose

    # q0: default zero configuration
    calib_config["q0"] = robot.q0
    calib_config["NbSample"] = calib_data["nb_sample"]

    # IDX_TOOL: frame ID of the tool
    IDX_TOOL = robot.model.getFrameId(end_frame)
    calib_config["IDX_TOOL"] = IDX_TOOL

    # tool_joint: ID of the joint right before the tool's frame (parent)
    tool_joint = robot.model.frames[IDX_TOOL].parentJoint
    calib_config["tool_joint"] = tool_joint

    # indices of active joints: from base to tool_joint
    actJoint_idx = get_sup_joints(robot.model, start_frame, end_frame)
    calib_config["actJoint_idx"] = actJoint_idx

    # indices of joint configuration corresponding to active joints
    config_idx = [robot.model.joints[i].idx_q for i in actJoint_idx]
    calib_config["config_idx"] = config_idx

    # number of active joints
    NbJoint = len(actJoint_idx)
    calib_config["NbJoint"] = NbJoint

    # initialize a list of calibrating parameters name
    param_name = []
    if calib_data["non_geom"]:
        # list of elastic gain parameter names
        elastic_gain = []
        joint_axes = ["PX", "PY", "PZ", "RX", "RY", "RZ"]
        for j_id, joint_name in enumerate(robot.model.names.tolist()):
            if joint_name == "universe":
                axis_motion = "null"
            else:
                # for ii, ax in enumerate(AXIS_MOTION[j_id]):
                #     if ax == 1:
                #         axis_motion = axis[ii]
                shortname = robot.model.joints[
                    j_id
                ].shortname()  # ONLY TAKE PRISMATIC AND REVOLUTE JOINT
                for ja in joint_axes:
                    if ja in shortname:
                        axis_motion = ja
                    elif "RevoluteUnaligned" in shortname:
                        axis_motion = "RZ"  # hard coded fix for canopies

            elastic_gain.append("k_" + axis_motion + "_" + joint_name)
        for i in actJoint_idx:
            param_name.append(elastic_gain[i])
    calib_config["param_name"] = param_name

    calib_config.update(
        {
            "free_flyer": calib_data["free_flyer"],
            "non_geom": calib_data["non_geom"],
            "eps": 1e-3,
            "PLOT": 0,
        }
    )
    try:
        calib_config.update(
            {
                "coeff_regularize": calib_data["coeff_regularize"],
                "data_file": calib_data["data_file"],
                "sample_configs_file": calib_data["sample_configs_file"],
                "outlier_eps": calib_data["outlier_eps"],
            }
        )
    except KeyError:
        calib_config.update(
            {
                "coeff_regularize": None,
                "data_file": None,
                "sample_configs_file": None,
                "outlier_eps": None,
            }
        )
    return calib_config

unified_to_legacy_config(robot, unified_calib_config)

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:

Name	Type	Description	Default
robot	RobotWrapper	Robot instance containing model and data	required
unified_calib_config	dict	Configuration from create_task_config	required

Returns:

Name	Type	Description
dict	dict	Legacy format calibration configuration matching get_param_from_yaml output

Raises:

Type	Description
KeyError	If required fields are missing from unified config
AssertionError	If frame validation fails

Example

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

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        robot (pin.RobotWrapper): Robot instance containing model and data
        unified_calib_config (dict): Configuration from create_task_config

    Returns:
        dict: Legacy format calibration configuration matching
              get_param_from_yaml output

    Raises:
        KeyError: If required fields are missing from unified config
        AssertionError: If frame validation fails

    Example:
        >>> unified_config = create_task_config(robot, parsed_config,
        ...                                    "calibration")
        >>> legacy_config = unified_to_legacy_config(robot, unified_config)
    """
    # Initialize output configuration
    calib_config = {}

    # Extract unified config sections
    joints = unified_calib_config.get("joints", {})
    kinematics = unified_calib_config.get("kinematics", {})
    parameters = unified_calib_config.get("parameters", {})
    measurements = unified_calib_config.get("measurements", {})
    data = unified_calib_config.get("data", {})

    # 1. Extract basic robot information
    calib_config["robot_name"] = robot.model.name
    calib_config["q0"] = robot.q0
    calib_config["instance"] = unified_calib_config.get("instance", {})

    # 2. Extract and validate markers/measurements
    _extract_marker_info(calib_config, measurements)

    # 3. Extract and validate kinematic frames
    _extract_frame_info(calib_config, robot, kinematics)

    # 4. Extract tool frame information
    _extract_tool_info(calib_config, robot, calib_config["end_frame"])

    # 5. Determine active joints
    _determine_active_joints(
        calib_config,
        robot,
        joints,
        calib_config["start_frame"],
        calib_config["end_frame"],
    )

    # 6. Extract poses
    _extract_poses(calib_config, measurements)

    # 7. Extract calibration parameters
    _extract_calibration_params(calib_config, robot, parameters)

    # 8. Extract data configuration
    calib_config["NbSample"] = data.get("number_of_samples", 500)
    calib_config["data_file"] = data.get("source_file")
    calib_config["sample_configs_file"] = data.get("sample_configurations_file")

    return calib_config

get_param_from_yaml_legacy(robot, calib_data)

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:

Name	Type	Description	Default
robot		Robot instance	required
calib_data		Calibration data dictionary	required

Returns:

Type	Description
dict	Calibration configuration dictionary

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        robot: Robot instance
        calib_data: Calibration data dictionary

    Returns:
        Calibration configuration dictionary
    """
    # Keep the original implementation here for compatibility
    return get_param_from_yaml(robot, calib_data)

get_param_from_yaml_unified(robot, calib_data)

Enhanced parameter parser using unified configuration system.

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

Parameters:

Name	Type	Description	Default
robot		Robot instance	required
calib_data		Configuration data (dict or file path)	required

Returns:

Type	Description
dict	Calibration configuration dictionary

Source code in src/figaroh/calibration/config.py

def 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.

    Args:
        robot: Robot instance
        calib_data: Configuration data (dict or file path)

    Returns:
        Calibration configuration dictionary
    """
    try:
        return unified_get_param_from_yaml(robot, calib_data, "calibration")
    except Exception as e:
        # Fall back to legacy parser if unified parser fails
        import warnings

        warnings.warn(
            f"Unified parser failed ({e}), falling back to legacy "
            "parser. Consider updating your configuration format.",
            UserWarning,
        )
        return get_param_from_yaml_legacy(robot, calib_data)

get_param_from_yaml_with_warning(robot, calib_data)

Original function with deprecation notice.

Source code in src/figaroh/calibration/config.py

def get_param_from_yaml_with_warning(robot, calib_data) -> dict:
    """Original function with deprecation notice."""
    import warnings

    warnings.warn(
        "Direct use of get_param_from_yaml is deprecated. "
        "Consider using the unified config parser from "
        "figaroh.utils.config_parser",
        DeprecationWarning,
        stacklevel=2,
    )
    return get_param_from_yaml_unified(robot, calib_data)

parameter

Parameter management utilities for robot calibration.

This module contains functions for creating and managing calibration parameter dictionaries, including: - Joint offset parameters - Geometric parameter offsets - Base frame parameters - End-effector marker parameters - Frame management utilities

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:

Name	Type	Description	Default
model		Pinocchio robot model	required
joint_names		List of joint names from model.names	required

Returns:

Name	Type	Description
dict		Mapping of joint offset parameter names to initial zero values. Keys have format: "{offset_type}_{joint_name}"

Example

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        model: Pinocchio robot model
        joint_names: List of joint names from model.names

    Returns:
        dict: Mapping of joint offset parameter names to initial zero values.
            Keys have format: "{offset_type}_{joint_name}"

    Example:
        >>> offsets = get_joint_offset(robot.model, robot.model.names[1:])
        >>> print(offsets["offsetRZ_joint1"])
        0.0
    """
    joint_off = []
    joint_names = list(model.names[1:])
    joints = list(model.joints[1:])
    assert len(joint_names) == len(
        joints
    ), "Number of jointnames does not match number of joints! Please check\
        imported model."
    for id, joint in enumerate(joints):
        name = joint_names[id]
        shortname = joint.shortname()
        if model.name == "canopies":
            if "RevoluteUnaligned" in shortname:
                shortname = shortname.replace("RevoluteUnaligned", "RZ")
        for i in range(joint.nv):
            if i > 0:
                offset_param = (
                    shortname.replace("JointModel", "offset")
                    + "{}".format(i + 1)
                    + "_"
                    + name
                )
            else:
                offset_param = shortname.replace("JointModel", "offset") + "_" + name
            joint_off.append(offset_param)

    phi_jo = [0] * len(joint_off)  # default zero values
    joint_off = dict(zip(joint_off, phi_jo))
    return joint_off

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:

Name	Type	Description	Default
joint_names		List of joint names from robot model	required

Returns:

Name	Type	Description
dict		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

Example

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

Source code in src/figaroh/calibration/parameter.py

def get_fullparam_offset(joint_names):
    """Get dictionary of geometric parameter variations.

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

    Args:
        joint_names: List of joint names from robot model

    Returns:
        dict: 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

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

    for i in range(len(joint_names)):
        for j in FULL_PARAMTPL:
            # geo_params.append(j + ("_%d" % i))
            geo_params.append(j + "_" + joint_names[i])

    phi_gp = [0] * len(geo_params)  # default zero values
    geo_params = dict(zip(geo_params, phi_gp))
    return geo_params

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:

Name	Type	Description	Default
calib_config		Parameter dictionary containing: - calib_model: "full_params" or "joint_offset" - param_name: List of parameter names to update	required

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        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
    """
    if calib_config["calib_model"] == "full_params":
        calib_config["param_name"][0:6] = BASE_TPL
    elif calib_config["calib_model"] == "joint_offset":
        calib_config["param_name"] = BASE_TPL + calib_config["param_name"]

add_pee_name(calib_config)

Add end-effector marker parameters to parameter list.

Adds parameters for each active measurement DOF of each marker.

Parameters:

Name	Type	Description	Default
calib_config		Parameter dictionary containing: - NbMarkers: Number of markers - measurability: List of booleans for active DOFs - param_name: List of parameter names to update	required

Side Effects

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

Source code in src/figaroh/calibration/parameter.py

def add_pee_name(calib_config):
    """Add end-effector marker parameters to parameter list.

    Adds parameters for each active measurement DOF of each marker.

    Args:
        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}"
    """
    PEE_names = []
    for i in range(calib_config["NbMarkers"]):
        for j, state in enumerate(calib_config["measurability"]):
            if state:
                PEE_names.extend(["{}_{}".format(EE_TPL[j], i + 1)])
    calib_config["param_name"] = calib_config["param_name"] + PEE_names

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:

Name	Type	Description	Default
frame_name	str	Name for the new frame	required
p	ndarray	3D position offset from parent frame	required
rpy	ndarray	Roll-pitch-yaw angles for frame orientation	required
model	Model	Robot model to add frame to	required
data	Data	Robot data structure	required

Returns:

Name	Type	Description
int		ID of newly created frame

Note

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

Source code in src/figaroh/calibration/parameter.py

def 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.

    Args:
        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:
        int: ID of newly created frame

    Note:
        Currently hardcoded to attach to "arm_7_joint". This should be made
        configurable in future versions.
    """
    p = np.array([0.1, 0.1, 0.1])
    R = pin.rpy.rpyToMatrix(rpy)
    frame_placement = pin.SE3(R, p)

    parent_jointId = model.getJointId("arm_7_joint")
    prev_frameId = model.getFrameId("arm_7_joint")
    ee_frame_id = model.addFrame(
        pin.Frame(
            frame_name,
            parent_jointId,
            prev_frameId,
            frame_placement,
            pin.FrameType(0),
            pin.Inertia.Zero(),
        ),
        False,
    )
    return ee_frame_id

data_loader

Data loading and processing utilities for robot calibration.

This module provides functions for loading and processing calibration data from various file formats, including: - CSV file reading for joint configurations - Marker position/orientation data loading - Data validation and cleanup - Configuration vector management

get_idxq_from_jname(model, joint_name)

Get index of joint in configuration vector.

Parameters:

Name	Type	Description	Default
model	Model	Robot model	required
joint_name	str	Name of joint to find index for	required

Returns:

Name	Type	Description
int		Index of joint in configuration vector q

Raises:

Type	Description
AssertionError	If joint name does not exist in model

Source code in src/figaroh/calibration/data_loader.py

def get_idxq_from_jname(model, joint_name):
    """Get index of joint in configuration vector.

    Args:
        model (pin.Model): Robot model
        joint_name (str): Name of joint to find index for

    Returns:
        int: Index of joint in configuration vector q

    Raises:
        AssertionError: If joint name does not exist in model
    """
    assert joint_name in model.names, "Given joint name does not exist."
    jointId = model.getJointId(joint_name)
    joint_idx = model.joints[jointId].idx_q
    return joint_idx

read_config_data(model, path_to_file)

Read joint configurations from CSV file.

Parameters:

Name	Type	Description	Default
model	Model	Robot model containing joint information	required
path_to_file	str	Path to CSV file containing joint configurations	required

Returns:

Name	Type	Description
ndarray		Matrix of shape (n_samples, n_joints-1) containing joint positions

Source code in src/figaroh/calibration/data_loader.py

def read_config_data(model, path_to_file):
    """Read joint configurations from CSV file.

    Args:
        model (pin.Model): Robot model containing joint information
        path_to_file (str): Path to CSV file containing joint configurations

    Returns:
        ndarray: Matrix of shape (n_samples, n_joints-1) containing joint
            positions
    """
    df = pd.read_csv(path_to_file)
    q = np.zeros([len(df), model.njoints - 1])
    for i in range(len(df)):
        for j, name in enumerate(model.names[1:].tolist()):
            jointidx = get_idxq_from_jname(model, name)
            q[i, jointidx] = df[name][i]
    return q

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:

Name	Type	Description	Default
path_to_file	str	Path to CSV file containing recorded data	required
model	Model	Robot model containing joint information	required
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	required
del_list	list	Indices of bad samples to remove. Defaults to [].	[]

Returns:

Name	Type	Description
tuple		PEEm_exp (ndarray): Flattened marker measurements of shape (n_active_dofs,) q_exp (ndarray): Joint configurations of shape (n_samples, n_joints)

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:

Type	Description
KeyError	If required columns are missing from CSV

Side Effects

• Prints joint headers
• Updates calib_config["NbSample"] with number of valid samples

Source code in src/figaroh/calibration/data_loader.py

def 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.

    Args:
        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:
        tuple:
            - PEEm_exp (ndarray): Flattened marker measurements of shape
                (n_active_dofs,)
            - q_exp (ndarray): Joint configurations of shape
                (n_samples, n_joints)

    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
    """
    # read_csv
    df = pd.read_csv(path_to_file)

    # create headers for marker position
    PEE_headers = []
    pee_tpl = ["x", "y", "z", "phix", "phiy", "phiz"]
    for i in range(calib_config["NbMarkers"]):
        for j, state in enumerate(calib_config["measurability"]):
            if state:
                PEE_headers.extend(["{}{}".format(pee_tpl[j], i + 1)])

    # create headers for joint configurations
    joint_headers = [model.names[i] for i in calib_config["actJoint_idx"]]

    # check if all created headers present in csv file
    csv_headers = list(df.columns)
    for header in PEE_headers + joint_headers:
        if header not in csv_headers:
            logger.warning("%s does not exist in the file.", header)
            break

    # Extract marker position/location
    pose_ee = df[PEE_headers].to_numpy()

    # Extract joint configurations
    q_act = df[joint_headers].to_numpy()

    # remove bad data
    if del_list:
        pose_ee = np.delete(pose_ee, del_list, axis=0)
        q_act = np.delete(q_act, del_list, axis=0)

    # update number of data points
    calib_config["NbSample"] = q_act.shape[0]

    PEEm_exp = pose_ee.T
    PEEm_exp = PEEm_exp.flatten("C")

    q_exp = np.empty((calib_config["NbSample"], calib_config["q0"].shape[0]))
    for i in range(calib_config["NbSample"]):
        config = calib_config["q0"]
        config[calib_config["config_idx"]] = q_act[i, :]
        q_exp[i, :] = config

    return PEEm_exp, q_exp