Identification

base_identification

Base class for robot dynamic parameter identification. This module provides a generalized framework for dynamic parameter identification that can be inherited by any robot type (TIAGo, UR10, MATE, etc.).

class figaroh.identification.base_identification.BaseIdentification(robot, config_file='config/robot_config.yaml')[source]

Bases: ABC

Base class for robot dynamic parameter identification.

Provides common functionality for all robots while allowing robot-specific implementations of key methods.

add_additional_parameters()[source]: Add additional parameters specific to the robot and recalculate dynamic regressor.

calculate_full_regressor()[source]: Build regressor matrix, compute pre-identified values of standard parameters, compute joint torques based on pre-identified standard parameters.

filter_kinematics_data(filter_config=None)[source]

Apply filtering to data with configurable parameters.

Parameters:: filter_config (dict, optional) – Filter configuration with keys: - differentiation_method: Method for derivative estimation - filter_params: Parameters for signal filtering
Raises:: ValueError – If required data is missing

initialize(truncate=None)[source]

load_param(config_file, setting_type='identification')[source]

Load the identification parameters from the yaml file.

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

Parameters:

config_file (str) – Path to configuration file (legacy or unified)
setting_type (str) – Configuration section to load

abstractmethod load_trajectory_data()[source]

Load and process CSV data.

This method must be implemented by robot-specific subclasses to handle their specific data formats and file structures.

Returns:: (timestamps, positions, velocities, torques) as numpy arrays
Return type:: tuple

plot_results()[source]: Plot identification results using unified results manager.

process_data(truncate=None)[source]: Load and process data

process_kinematics_data(filter_config=None)[source]: Process kinematics data (positions, velocities, accelerations) with filtering.

process_torque_data(**kwargs)[source]: Process torque data (generic implementation, should be overridden for robot-specific processing).

save_results(output_dir='results')[source]: Save identification results using unified results manager.

solve(decimate=True, decimation_factor=10, zero_tolerance=0.001, plotting=True, save_results=False)[source]

Main solving method for dynamic parameter identification.

This method implements the complete base parameter identification workflow including column elimination, optional decimation, QR decomposition, and quality metric computation.

Parameters:

decimate (bool) – Whether to apply decimation to reduce data size
decimation_factor (int) – Factor for signal decimation (default: 10)
zero_tolerance (float) – Tolerance for eliminating zero columns
plotting (bool) – Whether to generate plots
save_results (bool) – Whether to save parameters to file

Returns:

Base parameters phi_base

Return type:

ndarray

Raises:

AssertionError – If prerequisites not met (dynamic_regressor, standard_parameter)
ValueError – If data shapes are incompatible
np.linalg.LinAlgError – If QR decomposition fails

identification_tools

figaroh.identification.identification_tools.add_custom_parameters(phi, params, custom_params, model)[source]

Add custom user-defined parameters.

Parameters:

phi – Current parameter values list
params – Current parameter names list
custom_params – Custom parameter definitions
model – Robot model

Returns:

Updated (phi, params) lists

Return type:

tuple

figaroh.identification.identification_tools.add_standard_additional_parameters(phi, params, identif_config, model)[source]

Add standard additional parameters (actuator inertia, friction, offsets).

Parameters:

phi – Current parameter values list
params – Current parameter names list
identif_config – Configuration dictionary
model – Robot model

Returns:

Updated (phi, params) lists

Return type:

tuple

figaroh.identification.identification_tools.base_param_from_standard(phi_standard, params_base)[source]

Convert standard parameters to base parameters.

Takes standard dynamic parameters and calculates the corresponding base parameters using analytical relationships between them.

Parameters:

phi_standard (dict) – Standard parameters from model/URDF
params_base (list) – Analytical parameter relationships

Returns:

Base parameter values calculated from standard parameters

Return type:

list

figaroh.identification.identification_tools.calculate_first_second_order_differentiation(model, q, identif_config, dt=None)[source]

Calculate joint velocities and accelerations from positions.

Computes first and second order derivatives of joint positions using central differences. Handles both constant and variable timesteps.

Parameters:

model (pin.Model) – Robot model
q (ndarray) – Joint position matrix (n_samples, n_joints)
param (dict) – Parameters containing: - is_joint_torques: Whether using joint torques - is_external_wrench: Whether using external wrench - ts: Timestep if constant
dt (ndarray, optional) – Variable timesteps between samples.

Returns:

q (ndarray): Trimmed position matrix
dq (ndarray): Joint velocity matrix
ddq (ndarray): Joint acceleration matrix

Return type:

tuple

Note

Two samples are removed from start/end due to central differences

figaroh.identification.identification_tools.get_param_from_yaml(robot, identif_data)[source]

Parse identification parameters from YAML configuration file.

Extracts robot parameters, problem settings, signal processing options and total least squares parameters from a YAML config file.

Parameters:

robot (pin.RobotWrapper) – Robot instance containing model
identif_data (dict) – YAML configuration containing: - robot_params: Joint limits, friction, inertia settings - problem_params: External wrench, friction, actuator settings - processing_params: Sample rate, filter settings - tls_params: Load mass and location

Returns:

Parameter dictionary with unified settings

Return type:

dict

Example

>>> config = yaml.safe_load(config_file)
>>> params = get_param_from_yaml(robot, config)
>>> print(params["nb_samples"])

figaroh.identification.identification_tools.get_param_from_yaml_legacy(robot, identif_data) → dict[source]

Legacy identification parameter parser - kept for backward compatibility.

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

Parameters:

robot – Robot instance
identif_data – Identification data dictionary

Returns:

Identification configuration dictionary

figaroh.identification.identification_tools.get_param_from_yaml_unified(robot, identif_data) → dict[source]

Enhanced parameter parser using unified configuration system.

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

Parameters:

robot – Robot instance
identif_data – Configuration data (dict or file path)

Returns:

Identification configuration dictionary

figaroh.identification.identification_tools.get_param_from_yaml_with_warning(robot, identif_data) → dict[source]: Original function with deprecation notice.

figaroh.identification.identification_tools.get_parameter_info()[source]

Get information about available parameter types.

Returns:: Information about standard and custom parameter types
Return type:: dict

figaroh.identification.identification_tools.get_standard_parameters(model, identif_config=None, include_additional=True, custom_params=None)[source]

Get standard inertial parameters from robot model with extensible parameter support.

Parameters:

model – Robot model (Pinocchio model)
param (dict, optional) – Dictionary of parameter settings for additional parameters. Expected keys: - has_actuator_inertia (bool): Include actuator inertia parameters - has_friction (bool): Include friction parameters - has_joint_offset (bool): Include joint offset parameters - Ia (list): Actuator inertia values - fv (list): Viscous friction coefficients - fs (list): Static friction coefficients - off (list): Joint offset values
include_additional (bool) – Whether to include additional parameters beyond inertial
custom_params (dict, optional) – Custom parameter definitions Format: {param_name: {values: list, per_joint: bool, default: float}}

Returns:

Parameter names mapped to their values

Return type:

dict

Examples

# Basic usage - only inertial parameters params = get_standard_parameters(robot.model)

# Include standard additional parameters identif_config = {

‘has_actuator_inertia’: True, ‘has_friction’: True, ‘Ia’: [0.1, 0.2, 0.3], ‘fv’: [0.01, 0.02, 0.03], ‘fs’: [0.001, 0.002, 0.003]

} params = get_standard_parameters(robot.model, identif_config)

# Add custom parameters custom = {

‘gear_ratio’: {‘values’: [100, 50, 25], ‘per_joint’: True,
‘default’: 1.0},

‘temperature’: {‘values’: [20.0], ‘per_joint’: False,
‘default’: 25.0}

} params = get_standard_parameters(robot.model, identif_config,

custom_params=custom)

figaroh.identification.identification_tools.index_in_base_params(params, id_segments)[source]

Map segment IDs to their base parameters.

For each segment ID, finds which base parameters contain inertial parameters from that segment.

Parameters:

params (list) – Base parameter expressions
id_segments (list) – Segment IDs to map

Returns:

Maps segment IDs to lists of base parameter indices

Return type:

dict

figaroh.identification.identification_tools.low_pass_filter_data(data, identif_config, nbutter=5)[source]

Apply zero-phase Butterworth low-pass filter to measurement data.

Uses scipy’s filtfilt for zero-phase digital filtering. Removes high frequency noise while preserving signal phase. Handles border effects by trimming filtered data.

Parameters:

data (ndarray) – Raw measurement data to filter
param (dict) – Filter parameters containing: - ts: Sample time - cut_off_frequency_butterworth: Cutoff frequency in Hz
nbutter (int, optional) – Filter order. Higher order gives sharper frequency cutoff. Defaults to 5.

Returns:

Filtered data with border regions removed

Return type:

ndarray

Note

Border effects are handled by removing nborder = 5*nbutter samples from start and end of filtered signal.

figaroh.identification.identification_tools.relative_stdev(W_b, phi_b, tau)[source]

Calculate relative standard deviation of identified parameters.

Implements the residual error method from [Pressé & Gautier 1991] to estimate parameter uncertainty.

Parameters:

W_b (ndarray) – Base regressor matrix
phi_b (list) – Base parameter values
tau (ndarray) – Measured joint torques/forces

Returns:

Relative standard deviation (%) for each base parameter

Return type:

ndarray

figaroh.identification.identification_tools.reorder_inertial_parameters(pinocchio_params)[source]

Reorder inertial parameters from Pinocchio format to desired format.

Parameters:

pinocchio_params – Parameters in Pinocchio order [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz]

Returns:

Parameters in desired order: [Ixx, Ixy, Ixz, Iyy, Iyz, Izz, mx, my, mz, m]

Return type:

list

figaroh.identification.identification_tools.unified_to_legacy_identif_config(robot, unified_identif_config) → dict[source]

Convert unified identification format to legacy identif_config format.

Maps the new unified identification configuration structure to produce the exact same output as get_param_from_yaml. This ensures backward compatibility while using the new unified parser.

Parameters:

robot (pin.RobotWrapper) – Robot instance containing model and data
unified_identif_config (dict) – Configuration from create_task_config

Returns:

Identification configuration matching get_param_from_yaml output

Return type:

dict

Example

>>> unified_config = create_task_config(robot, parsed_config,
...                                    "identification")
>>> legacy_config = unified_to_legacy_identif_config(robot,
...                                                  unified_config)
>>> # legacy_config has same keys as get_param_from_yaml output

figaroh.identification.identification_tools.weigthed_least_squares(robot, phi_b, W_b, tau_meas, tau_est, identif_config)[source]

Compute weighted least squares solution for parameter identification.

Implements iteratively reweighted least squares method from [Gautier, 1997]. Accounts for heteroscedastic noise.

Parameters:

robot (pin.Robot) – Robot model
phi_b (ndarray) – Initial base parameters
W_b (ndarray) – Base regressor matrix
tau_meas (ndarray) – Measured joint torques
tau_est (ndarray) – Estimated joint torques
param (dict) – Settings including idx_tau_stop

Returns:

Identified base parameters

Return type:

ndarray