Utilities

Configuration Parser

Unified Configuration Parser for FIGAROH

This module provides a unified configuration parsing system that supports: - Template inheritance and extension - Variant configurations - Variable expansion - Backward compatibility with legacy formats - Schema validation

class figaroh.utils.config_parser.ConfigMetadata(schema_version: str = '2.0', config_type: str = 'robot_configuration', source_file: str | None = None, variant: str | None = None, resolved_at: str | None = None)

Bases: object

Configuration metadata container.

config_type: str = 'robot_configuration'

resolved_at: str | None = None

schema_version: str = '2.0'

source_file: str | None = None

variant: str | None = None

class figaroh.utils.config_parser.UnifiedConfigParser(config_path: str | Path, variant: str | None = None)

Bases: object

Unified configuration parser supporting inheritance, templates, and validation.

parse() → Dict[str, Any]

Parse configuration with inheritance and validation.

Returns:

Complete configuration dictionary with metadata

Raises:

ConfigurationError – If parsing or validation fails

figaroh.utils.config_parser.create_task_config(robot, unified_config: Dict[str, Any], task_name: str) → Dict[str, Any]

Create task-specific configuration from unified config.

Parameters:

• robot – Robot instance

• unified_config – Parsed unified configuration

• task_name – Name of task to extract configuration for

Returns:

Task-specific configuration dictionary

Raises:

ConfigurationError – If task not found or invalid

figaroh.utils.config_parser.get_calibration_param_from_yaml(robot, calib_data: Dict[str, Any]) → Dict[str, Any]

Legacy function for calibration parameter parsing - DEPRECATED.

figaroh.utils.config_parser.get_identification_param_from_yaml(robot, identif_data: Dict[str, Any]) → Dict[str, Any]

Legacy function for identification parameter parsing - DEPRECATED.

figaroh.utils.config_parser.get_param_from_yaml(robot, config_data: Dict[str, Any] | str | Path, task_type: str = 'auto', variant: str | None = None) → Dict[str, Any]

Unified parameter parser with backward compatibility.

Parameters:

• robot – Robot instance

• config_data – Configuration data (dict), or path to config file

• task_type – Type of task configuration to extract (“calibration”, “identification”, “auto”)

• variant – Optional variant to apply

Returns:

Task-specific parameter dictionary

Raises:

ConfigurationError – If parsing fails or task not found

figaroh.utils.config_parser.is_unified_config(config_file: str | Path) → bool

Check if a configuration file is using the unified format.

Parameters:

config_file – Path to configuration file

Returns:

True if unified format, False if legacy format

figaroh.utils.config_parser.parse_configuration(config_path: str | Path, variant: str | None = None) → Dict[str, Any]

Convenience function to parse a configuration file.

Parameters:

• config_path – Path to configuration file

• variant – Optional variant name to apply

Returns:

Parsed configuration dictionary

Cubic Spline

Cubic Spline Trajectory Generation for Robotics Applications.

This module provides comprehensive tools for generating smooth, continuous trajectories using cubic splines for robotic systems. It supports constraint handling, waypoint generation, and trajectory validation with joint limits.

The module contains two main classes: - CubicSpline: Core trajectory generation with cubic splines - WaypointsGeneration: Automated waypoint generation for trajectory planning

Key Features:

• C2 continuous cubic spline trajectories

• Joint position, velocity, and acceleration constraints

• Waypoint-based trajectory planning

• Collision and constraint checking

• Visualization capabilities

• Random and systematic waypoint generation

Dependencies:

• ndcurves: Curve generation and manipulation

• numpy: Numerical computations

• matplotlib: Plotting and visualization

• pinocchio: Robot kinematics and dynamics

Examples

Basic trajectory generation:

```python from figaroh.utils.cubic_spline import CubicSpline

# Initialize spline generator spline = CubicSpline(robot, num_waypoints=5,

active_joints=[‘joint1’, ‘joint2’])

# Generate trajectory time_points = np.array([[0], [1], [2], [3], [4]]) waypoints = np.random.rand(2, 5) # 2 joints, 5 waypoints

t, pos, vel, acc = spline.get_full_config(

freq=100, time_points=time_points, waypoints=waypoints

)

# Check constraints spline.check_cfg_constraints(pos, vel) ```

Automated waypoint generation:

```python from figaroh.utils.cubic_spline import WaypointsGeneration

# Initialize waypoint generator wp_gen = WaypointsGeneration(robot, num_waypoints=5,

active_joints=[‘joint1’, ‘joint2’])

# Generate waypoint pool wp_gen.gen_rand_pool()

# Generate random waypoints pos_wp, vel_wp, acc_wp = wp_gen.gen_rand_wp()

# Generate trajectory from waypoints t, pos, vel, acc = wp_gen.get_full_config(

freq=100, time_points=time_points, waypoints=pos_wp

References

• Spline theory: “A Practical Guide to Splines” by Carl de Boor

• Robot trajectory planning: “Introduction to Robotics” by John Craig

• ndcurves library: https://github.com/humanoid-path-planner/ndcurves

class figaroh.utils.cubic_spline.CubicSpline(robot, num_waypoints: int, active_joints: list, soft_lim=0)

Bases: object

Cubic spline trajectory generator for robotic systems.

This class generates smooth, C2-continuous trajectories using cubic splines for robotic systems. It supports both position-only and full kinematic constraint specification (position, velocity, acceleration) at waypoints.

The class handles active joint selection, joint limit constraints, and provides methods for trajectory validation and visualization.

robot

Robot model instance

rmodel

Robot kinematic model

num_waypoints

Number of waypoints in trajectory

Type:

int

act_Jid

Active joint IDs

Type:

List[int]

act_Jname

Active joint names

Type:

List[str]

act_J

Active joint objects

Type:

List

act_idxq

Position indices for active joints

Type:

List[int]

act_idxv

Velocity indices for active joints

Type:

List[int]

dim_q

Position vector dimensions

Type:

Tuple[int, int]

dim_v

Velocity vector dimensions

Type:

Tuple[int, int]

upper_q, lower_q

Position limits for active joints

Type:

np.ndarray

upper_dq, lower_dq

Velocity limits for active joints

Type:

np.ndarray

upper_effort, lower_effort

Effort limits for active joints

Type:

np.ndarray

Examples

Basic usage:

```python # Initialize for 2 joints, 5 waypoints spline = CubicSpline(robot, num_waypoints=5,

active_joints=[‘joint1’, ‘joint2’])

# Define waypoints and time stamps waypoints = np.array([[0, 1, 2, 1, 0], # joint1 positions

[0, 0.5, 1, 0.5, 0]]) # joint2 positions

time_points = np.array([[0], [1], [2], [3], [4]])

# Generate trajectory at 100 Hz t, pos, vel, acc = spline.get_full_config(

freq=100, time_points=time_points, waypoints=waypoints

)

# Validate constraints is_violated = spline.check_cfg_constraints(pos, vel) ```

With velocity and acceleration constraints:

```python # Define waypoint constraints vel_waypoints = np.zeros((2, 5)) # Zero velocity at waypoints acc_waypoints = np.zeros((2, 5)) # Zero acceleration at waypoints

# Generate constrained trajectory t, pos, vel, acc = spline.get_full_config(

freq=100, time_points=time_points, waypoints=waypoints, vel_waypoints=vel_waypoints, acc_waypoints=acc_waypoints

)

# Visualize results spline.plot_spline(t, pos, vel, acc) ```

Note

The class uses the ndcurves library for cubic spline generation. Joint limits are automatically extracted from the robot model and can be modified with soft limits for safety margins.

check_cfg_constraints(q, v=None, tau=None, soft_lim=0)

Check joint constraints violation for trajectory configurations.

Validates whether the generated trajectory respects robot joint limits including position, velocity, and effort constraints. Provides detailed violation reporting for debugging.

Parameters:

• q (np.ndarray) – Position trajectory to check, shape (N, robot_nq)

• v (np.ndarray, optional) – Velocity trajectory to check, shape (N, robot_nv). If None, velocity checks are skipped.

• tau (np.ndarray, optional) – Effort trajectory to check, shape (N, robot_nv). If None, effort checks are skipped.

• soft_lim (float, optional) – Additional safety margin factor (0-1). Adds this fraction of joint range as safety buffer. Defaults to 0.

Returns:

True if any constraint is violated, False if all

constraints are satisfied

Return type:

bool

Examples

Basic position check:

```python t, q, dq, ddq = spline.get_full_config(…) is_violated = spline.check_cfg_constraints(q) if is_violated:

print(“Trajectory violates position limits!”)

```

Full constraint check with safety margin:

```python is_violated = spline.check_cfg_constraints(

q, v=dq, tau=torques, soft_lim=0.1

Note

Constraint violations are printed to console with specific joint indices and violation types for debugging purposes.

check_self_collision()

get_active_config(freq: int, time_points: ndarray, waypoints: ndarray, vel_waypoints=None, acc_waypoints=None)

Generate cubic splines on active joints

get_full_config(freq: int, time_points: ndarray, waypoints: ndarray, vel_waypoints=None, acc_waypoints=None)

Generate complete robot configuration trajectory with cubic splines.

This method creates smooth trajectories for all robot joints by: 1. Generating cubic splines for active joints between waypoints 2. Filling inactive joints with zero values 3. Ensuring C2 continuity at waypoints

Parameters:

• freq (int) – Sampling frequency for trajectory generation (Hz)

• time_points (np.ndarray) – Time stamps for waypoints, shape (num_waypoints, 1)

• waypoints (np.ndarray) – Position waypoints for active joints, shape (num_active_joints, num_waypoints)

• vel_waypoints (np.ndarray, optional) – Velocity constraints at waypoints, same shape as waypoints. If provided, enforces specific velocities at waypoints.

• acc_waypoints (np.ndarray, optional) – Acceleration constraints at waypoints, same shape as waypoints. If provided, enforces specific accelerations at waypoints.

Returns:

Four-element tuple containing:

• t (np.ndarray): Time stamps, shape (N, 1)

• q_full (np.ndarray): Position trajectory for all joints, shape (N, robot_nq)

• dq_full (np.ndarray): Velocity trajectory for all joints, shape (N, robot_nv)

• ddq_full (np.ndarray): Acceleration trajectory for all joints, shape (N, robot_nv)

Where N = int(total_time * freq) + 1

Return type:

tuple

Raises:

AssertionError – If waypoint dimensions don’t match active joints

Examples

Position-only trajectory:

```python time_pts = np.array([[0], [1], [2], [3]]) waypts = np.array([[0, 1, 2, 1], # joint1

[0, 0.5, 1, 0.5]]) # joint2

t, q, dq, ddq = spline.get_full_config(

freq=100, time_points=time_pts, waypoints=waypts

With velocity/acceleration constraints:

```python vel_waypts = np.zeros((2, 4)) # Zero velocity at waypoints acc_waypts = np.zeros((2, 4)) # Zero acceleration at waypoints

t, q, dq, ddq = spline.get_full_config(

freq=100, time_points=time_pts, waypoints=waypts, vel_waypoints=vel_waypts, acc_waypoints=acc_waypts

Note

The trajectory uses piecewise cubic curves connected at waypoints. When velocity and acceleration constraints are provided, the resulting trajectory enforces exact values at waypoints.

plot_spline(t, p, v, a)

class figaroh.utils.cubic_spline.WaypointsGeneration(robot, num_waypoints: int, active_joints: list, soft_lim=0)

Bases: CubicSpline

Automated waypoint generation for cubic spline trajectories.

This class extends CubicSpline to provide automated generation of feasible waypoints that respect robot joint constraints. It creates pools of valid configurations and uses random sampling to generate diverse trajectory waypoints.

The class generates waypoints for position, velocity, and acceleration that can be used as initial guesses for trajectory optimization or as standalone feasible trajectories.

n_set

Size of waypoint pools (default: 10)

Type:

int

pool_q

Pool of valid position configurations

Type:

np.ndarray

pool_dq

Pool of valid velocity configurations

Type:

np.ndarray

pool_ddq

Pool of valid acceleration configurations

Type:

np.ndarray

soft_limit_pool_default

Default soft limit values

Type:

np.ndarray

Examples

Basic waypoint generation:

```python wp_gen = WaypointsGeneration(robot, num_waypoints=5,

active_joints=[‘joint1’, ‘joint2’])

# Generate random feasible waypoints pos_wp, vel_wp, acc_wp = wp_gen.random_feasible_waypoints()

# Use with cubic spline time_pts = np.linspace(0, 4, 5).reshape(-1, 1) t, q, dq, ddq = wp_gen.get_full_config(

freq=100, time_points=time_pts, waypoints=pos_wp.T, vel_waypoints=vel_wp.T, acc_waypoints=acc_wp.T

With custom soft limits:

```python # Different safety margins for position/velocity/acceleration soft_lim_custom = np.array([

[0.1, 0.15], # Position limits (10%, 15% for joints) [0.2, 0.2], # Velocity limits (20% for both joints) [0.3, 0.25] # Acceleration limits (30%, 25%)

])

wp_gen.set_soft_limit_pool(soft_lim_custom) pos_wp, vel_wp, acc_wp = wp_gen.random_feasible_waypoints() ```

Note

The class automatically ensures waypoints don’t violate joint constraints and avoids repeated waypoint values that could cause numerical issues in spline generation.

check_repeat_wp(wp_list: list)

gen_equal_wp(wp_init=None, vel_wp_init=None, acc_wp_init=None)

Generate equal waypoints everywhere same as first waypoints with default: zero vel and zero acc

gen_rand_pool(soft_limit_pool=None)

Generate a uniformly distributed waypoint pool of pos/vel/acc over a specific range

gen_rand_wp(wp_init=None, vel_wp_init=None, acc_wp_init=None, vel_set_zero=True, acc_set_zero=True)

Generate waypoint pos/vel/acc which randomly pick from waypoint pool Or, set vel and/or acc at waypoints to be zero

figaroh.utils.cubic_spline.calc_torque(N, robot, q, v, a)

figaroh.utils.cubic_spline.init_robot(robot)

Results Manager

Unified results management for FIGAROH examples.

This module provides standardized plotting and saving functionality across all task types: Calibration, Identification, OptimalCalibration, OptimalTrajectory.

class figaroh.utils.results_manager.ResultsManager(task_type: str, robot_name: str = 'robot', results_data: Dict[str, Any] | None = None)

Bases: object

Unified results management for all FIGAROH task types.

This class provides standardized plotting and saving functionality with consistent styling and formats across different analysis types.

COLORS = {'calibrated': '#2ca02c', 'identified': '#ff7f0e', 'measured': '#1f77b4', 'optimal': '#d62728', 'reference': '#9467bd', 'residual': '#8c564b'}

PLOT_STYLES = {'calibration': {'dpi': 100, 'figsize': (14, 10)}, 'identification': {'dpi': 100, 'figsize': (12, 8)}, 'optimal_calibration': {'dpi': 100, 'figsize': (10, 8)}, 'optimal_trajectory': {'dpi': 100, 'figsize': (15, 10)}}

plot_calibration_results(measured_poses: ndarray | None = None, estimated_poses: ndarray | None = None, residuals: ndarray | None = None, parameter_values: ndarray | None = None, parameter_names: List[str] | None = None, outlier_indices: List[int] | None = None, title: str = 'Calibration Results') → None

Plot calibration results with pose comparison and residual analysis.

Parameters:

• measured_poses – Measured poses (optional, uses self.result)

• estimated_poses – Estimated poses (optional, uses self.result)

• residuals – Position/orientation residuals (optional, uses self.result)

• parameter_values – Calibrated parameters (optional, uses self.result)

• parameter_names – Parameter names (optional, uses self.result)

• outlier_indices – Outlier indices (optional, uses self.result)

• title – Plot title

plot_identification_results(tau_measured: ndarray | None = None, tau_identified: ndarray | None = None, parameter_values: ndarray | None = None, parameter_names: List[str] | None = None, time_vector: ndarray | None = None, joint_names: List[str] | None = None, title: str = 'Identification Results') → None

Plot identification results with torque comparison and analysis.

Parameters:

• tau_measured – Measured joint torques (optional, uses self.result)

• tau_identified – Estimated torques (optional, uses self.result)

• parameter_values – Parameter values (optional, uses self.result)

• parameter_names – Parameter names (optional, uses self.result)

• time_vector – Time vector for plots

• joint_names – Names of robot joints

• title – Plot title

plot_optimal_calibration_results(configurations: Dict[str, ndarray], weights: ndarray, condition_numbers: ndarray | None = None, information_matrix: ndarray | None = None, title: str = 'Optimal Calibration Results') → None

Plot optimal calibration configuration results.

Parameters:

• configurations – Dictionary of configuration data

• weights – Weights assigned to each configuration

• condition_numbers – Condition numbers for analysis

• information_matrix – Fisher information matrix

• title – Plot title

plot_optimal_trajectory_results(trajectories: Dict[str, Any], condition_number: float, joint_names: List[str] | None = None, title: str = 'Optimal Trajectory Results') → None

Plot optimal trajectory generation results.

Parameters:

• trajectories – Dictionary containing trajectory data

• condition_number – Final condition number achieved

• joint_names – Names of robot joints

• title – Plot title

save_results(results: Dict[str, Any] | None = None, output_dir: str = 'results', file_prefix: str | None = None, save_formats: List[str] = ['yaml', 'csv']) → Dict[str, str]

Save results to files with standardized format.

Parameters:

• results – Dictionary containing results to save (uses self.result if None)

• output_dir – Output directory path

• file_prefix – Prefix for output files (default: robot_name_task_type)

• save_formats – List of formats to save (‘yaml’, ‘csv’, ‘json’, ‘npz’)

Returns:

Dictionary mapping format to file path

figaroh.utils.results_manager.plot_calibration_results(measured_poses, estimated_poses, residuals, **kwargs)

Plot calibration results (convenience function).

figaroh.utils.results_manager.plot_identification_results(tau_measured, tau_identified, parameters, **kwargs)

Plot identification results (convenience function).

figaroh.utils.results_manager.save_results(results, task_type, output_dir='results', **kwargs)

Save results (convenience function).

Error Handling

Custom exceptions and error handling for FIGAROH examples.

This module provides standardized error handling and validation utilities for robot identification and calibration workflows.

exception figaroh.utils.error_handling.CalibrationError

Bases: FigarohExampleError

Exception raised during calibration procedures.

exception figaroh.utils.error_handling.ConfigurationError

Bases: FigarohExampleError

Exception raised for configuration-related issues.

exception figaroh.utils.error_handling.DataProcessingError

Bases: FigarohExampleError

Exception raised during data processing operations.

class figaroh.utils.error_handling.ErrorContext(operation_name: str, raise_on_error: bool = True)

Bases: object

Context manager for structured error handling.

exception figaroh.utils.error_handling.FigarohExampleError

Bases: Exception

Base exception for FIGAROH examples.

exception figaroh.utils.error_handling.IdentificationError

Bases: FigarohExampleError

Exception raised during identification procedures.

exception figaroh.utils.error_handling.RobotInitializationError

Bases: FigarohExampleError

Exception raised when robot initialization fails.

exception figaroh.utils.error_handling.ValidationError

Bases: FigarohExampleError

Exception raised when validation fails.

figaroh.utils.error_handling.handle_calibration_errors(func: F) → F

Decorator to handle calibration-specific errors.

Parameters:

func – Function to decorate

Returns:

Decorated function with calibration error handling

figaroh.utils.error_handling.handle_identification_errors(func: F) → F

Decorator to handle identification-specific errors.

Parameters:

func – Function to decorate

Returns:

Decorated function with identification error handling

figaroh.utils.error_handling.safe_execute(func: F, *args, **kwargs) → tuple

Safely execute a function and return (success, result_or_error).

Parameters:

• func – Function to execute

• *args – Positional arguments for the function

• **kwargs – Keyword arguments for the function

Returns:

Tuple of (success_flag, result_or_exception)

figaroh.utils.error_handling.setup_example_logging(log_level: str = 'INFO', log_file: str | None = None) → Logger

Setup logging for FIGAROH examples.

Parameters:

• log_level – Logging level (DEBUG, INFO, WARNING, ERROR)

• log_file – Optional log file path

Returns:

Configured logger instance

figaroh.utils.error_handling.validate_input_data(func: F) → F

Decorator to validate input data for processing functions.

Parameters:

func – Function to decorate

Returns:

Decorated function with input validation

figaroh.utils.error_handling.validate_numeric_range(value: float | int | ndarray, min_val: float | None = None, max_val: float | None = None, name: str = 'value') → None

Validate that numeric value(s) are within specified range.

Parameters:

• value – Value or array to validate

• min_val – Minimum allowed value

• max_val – Maximum allowed value

• name – Name of the value for error messages

Raises:

ValidationError – If value is out of range

figaroh.utils.error_handling.validate_robot_config(config: Dict[str, Any]) → None

Validate robot configuration dictionary.

Parameters:

config – Configuration dictionary to validate

Raises:

ValidationError – If configuration is invalid

figaroh.utils.error_handling.validate_robot_initialization(func: F) → F

Decorator to validate robot initialization.

Parameters:

func – Function to decorate

Returns:

Decorated function with robot validation

figaroh.utils.error_handling.validate_trajectory_data(q: ndarray, qd: ndarray | None = None, qdd: ndarray | None = None, tau: ndarray | None = None) → None

Validate trajectory data arrays.

Parameters:

• q – Joint positions

• qd – Joint velocities (optional)

• qdd – Joint accelerations (optional)

• tau – Joint torques (optional)

Raises:

ValidationError – If data is invalid