Identification

identification_tools

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, param, 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.calculate_standard_parameters(model, W, tau, COM_max, COM_min, params_standard_u, alpha)[source]

Calculate optimal standard inertial parameters via quadratic programming.

Finds standard parameters that: 1. Best fit measurement data (tau) 2. Stay close to reference values from URDF 3. Keep COM positions within physical bounds

Parameters:

  • model (pin.Model) – Robot model containing inertias

  • W (ndarray) – Regressor matrix mapping parameters to measurements

  • tau (ndarray) – Measured force/torque data

  • COM_max (ndarray) – Upper bounds on center of mass positions

  • COM_min (ndarray) – Lower bounds on center of mass positions

  • params_standard_u (dict) – Reference parameters from URDF

  • alpha (float) – Weight between fitting data (1) vs staying near refs (0)

Returns:

  • phi_standard (ndarray): Optimized standard parameters

  • phi_ref (ndarray): Reference parameters from URDF

Return type:

tuple

Note

Constrains parameters to stay within [0.7, 1.3] times reference values except for COM positions which use explicit bounds.

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.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, param, 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.quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None)[source]

Solve a Quadratic Program defined as:

minimize 1/2 x^T P x + q^T x subject to G x <= h

A x = b

Parameters:

  • P (ndarray) – Symmetric quadratic cost matrix (n x n)

  • q (ndarray) – Linear cost vector (n)

  • G (ndarray, optional) – Linear inequality constraint matrix (m x n).

  • h (ndarray, optional) – Linear inequality constraint vector (m).

  • A (ndarray, optional) – Linear equality constraint matrix (p x n).

  • b (ndarray, optional) – Linear equality constraint vector (p).

Returns:

Optimal solution vector x* (n)

Return type:

ndarray

Note

Ensures P is symmetric positive definite by adding small identity matrix

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.set_missing_params_setting(robot, params_settings)[source]

Set default values for missing robot parameters.

Fills in missing parameters in the robot model with default values from params_settings when URDF doesn’t specify them.

Parameters:

  • robot (pin.RobotWrapper) – Robot instance to update

  • params_settings (dict) – Default parameter values containing: - q_lim_def: Default joint position limits - dq_lim_def: Default joint velocity limits - tau_lim_def: Default joint torque limits - ddq_lim_def: Default acceleration limits - fv, fs: Default friction parameters

Returns:

Updated params_settings with all required parameters

Return type:

dict

Side Effects:

Modifies robot model in place: - Sets joint limits if undefined - Sets velocity limits if zero - Sets effort limits if zero - Adds acceleration limits

figaroh.identification.identification_tools.weigthed_least_squares(robot, phi_b, W_b, tau_meas, tau_est, param)[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