# FIGAROH

> A Python toolbox for dynamic parameter identification and geometric calibration of robots and humans.

FIGAROH is a Python toolbox for geometric calibration, dynamic parameter identification and optimal experiment design of robots. Runnable robot examples live in the figaroh-examples repository. Agent skills ship in the figaroh repository under skills/.

# Start

# Quickstart

Install FIGAROH, run a real calibration on a bundled robot, and read its report. About five minutes.

## 1. Install

```
pip install figaroh
```

Enough for calibration and identification. Trajectory design needs cyipopt, which installs from conda only.

```
git clone https://github.com/thanhndv212/figaroh-plus figaroh
cd figaroh
conda env create -f environment.yml
conda activate figaroh-dev
pip install -e .
```

Includes cyipopt for trajectory design and the tools to build these docs.

## 2. Get the examples

Robots, data and runnable scripts live in a separate repository:

```
git clone https://github.com/thanhndv212/figaroh-examples
cd figaroh-examples/examples/ur10
```

## 3. Run a calibration

```
python calibration.py --calibrate-only --no-plot
```

FIGAROH loads the UR10 URDF and 29 measured poses, fits joint offsets and link geometry, and prints a quality report: condition number, RMSE before and after, and outliers.

## 4. Read the result

| Output          | Where                                                    | What to look for                                           |
| --------------- | -------------------------------------------------------- | ---------------------------------------------------------- |
| Terminal report | your shell                                               | RMSE should drop; few or no outliers                       |
| Saved results   | `data/calibration/calibration_results_<timestamp>.npz`   | input for `--update-model`, which exports a corrected URDF |
| HTML report     | `results/calibration_report.html` (with `--html-report`) | residual plots and parameter table, shareable as one file  |

**Done when** the terminal shows post-calibration RMSE below the pre-calibration value.

## Where next

- **Calibrate your robot**

  ______________________________________________________________________

  Plan, design, configure, fit and verify, with the choices at each step.

  [Calibration guide](https://thanhndv212.github.io/figaroh-plus/workflows/calibration/index.md)

- **Identify dynamics**

  ______________________________________________________________________

  Masses, inertias and friction from motion and torque logs.

  [Identification guide](https://thanhndv212.github.io/figaroh-plus/workflows/identification/index.md)

- **Add your robot**

  ______________________________________________________________________

  Scaffold a folder with URDF, config and a tools subclass.

  [New-robot guide](https://thanhndv212.github.io/figaroh-plus/workflows/new-robot/index.md)

- **Use an AI agent**

  ______________________________________________________________________

  Let a coding agent set up and run the task with FIGAROH's skills.

  [Agent setup](https://thanhndv212.github.io/figaroh-plus/agents/index.md)

# Getting Started

## Installation

Install FIGAROH from PyPI:

```
pip install figaroh
```

For development with all dependencies:

```
conda env create -f environment.yml
conda activate figaroh-dev
pip install -e .
```

FIGAROH's configuration system (the unified YAML format, template inheritance) is covered separately in [Configuration System](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) — read that before writing your first robot config.

## Quick Start Examples

`BaseCalibration` and `BaseIdentification` are abstract base classes — each robot example provides a concrete subclass (e.g. `TiagoCalibration`, `UR10Identification`) that implements the robot-specific cost function. The [examples repository](https://github.com/thanhndv212/figaroh-examples) has a complete, runnable subclass per supported robot; the snippets below show the shape every subclass follows.

### Basic Calibration

```
from examples.tiago.utils.tiago_tools import TiagoCalibration
from figaroh.tools.robot import load_robot

robot = load_robot("path/to/robot.urdf", load_by_urdf=True)
calibrator = TiagoCalibration(robot, "config/calibration_config.yaml")
calibrator.initialize()
result = calibrator.solve(plotting=False, html_report=True)
```

`solve(html_report=True)` also writes a self-contained HTML diagnostic report alongside the terminal quality report that's always printed — see [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) for the full report/verdict/compare-page suite.

### Basic Identification

```
from examples.tiago.utils.tiago_tools import TiagoIdentification
from figaroh.tools.robot import load_robot

robot = load_robot("path/to/robot.urdf", load_by_urdf=True)
identifier = TiagoIdentification(robot, "config/identification_config.yaml")
identifier.initialize()
result = identifier.solve(decimate=True, html_report=True)

verdict = identifier.verify()
print("PASS" if verdict.passed else "FAIL")
```

### Advanced Regressor Building

```
from figaroh.tools.regressor import RegressorBuilder, RegressorConfig

# Configure regressor
config = RegressorConfig(
    has_friction=True,
    has_actuator_inertia=True,
    is_joint_torques=True
)

# Build regressor matrix
builder = RegressorBuilder(robot, config)
W = builder.build_basic_regressor(q, dq, ddq)
```

### Advanced Linear Solver

```
from figaroh.tools.solver import LinearSolver, solve_linear_system

# Basic usage with convenience function
result = solve_linear_system(
    A, b,
    method='ridge',
    alpha=0.01
)
x = result['solution']

# Advanced usage with constraints
solver = LinearSolver()
result = solver.solve(
    A, b,
    method='constrained',
    bounds=(0, None),  # Positive constraints
    A_eq=A_eq, b_eq=b_eq,  # Equality constraints
    alpha=0.1  # Regularization
)

# Access quality metrics
print(f"RMSE: {result['rmse']:.4f}")
print(f"R²: {result['r_squared']:.4f}")
print(f"Condition number: {result['condition_number']:.2e}")

# Use in identification workflow
params = identifier.solve_with_custom_solver(
    method='elastic_net',
    alpha=0.01,
    l1_ratio=0.5,
    bounds=(0, None)  # Physical constraints
)
```

## Next Steps

- Follow the [Tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md) for an end-to-end walkthrough of calibration, identification, and optimal experiment design.
- Read [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) to turn a solved calibration/identification into a shareable report and a CI-gateable pass/fail check.
- Browse the [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) for complete, runnable workflows per robot (UR10, TIAGo, TALOS, Staubli TX40).
- Check the API Reference for detailed module information.
- Set up your own robot config from a [template](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md).
# Workflows

# Workflows

Calibration and identification follow the same five steps. Each guide explains the choices you make at every step and how you know it worked.

| Step             | You decide                                          | You check                                    |
| ---------------- | --------------------------------------------------- | -------------------------------------------- |
| **1. Plan**      | What to estimate and which data you have            | You have a separate validation set           |
| **2. Design**    | Which postures or trajectories to record            | The design is well conditioned               |
| **3. Configure** | Which template to extend and which keys to override | The config loads and matches your data       |
| **4. Fit**       | Which estimator to run                              | Conditioning, residuals and parameter values |
| **5. Verify**    | Which limits your application needs                 | `verify()` passes on held-out data           |

- **Calibrate a robot**

  ______________________________________________________________________

  Joint offsets, link geometry, base and tool frames from measured poses. Covers the five estimation methods and when to use each.

  [Calibration guide](https://thanhndv212.github.io/figaroh-plus/workflows/calibration/index.md)

- **Identify dynamics**

  ______________________________________________________________________

  Base parameters, friction and actuator inertia from motion and torque. Covers filtering, estimators and reconstruction.

  [Identification guide](https://thanhndv212.github.io/figaroh-plus/workflows/identification/index.md)

- **Add your robot**

  ______________________________________________________________________

  Scaffold a robot folder before either guide applies to your hardware.

  [New-robot guide](https://thanhndv212.github.io/figaroh-plus/workflows/new-robot/index.md)

- **Reports & verdicts**

  ______________________________________________________________________

  The HTML report, the JSON verdict and the two-run compare page.

  [Reporting & verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md)

For a single narrative walkthrough on TIAGo, read the [tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md). For a full plan-to-verdict example in code, read [Plan, fit & verify](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md).

# Calibrate a robot

Correct joint offsets and frame transforms from measured end-effector poses. Five steps, each with the choices you make and how you know it worked.

Use the toggle below to read the steps for the bundled UR10 example or for your own robot. Each choice card lists the options FIGAROH supports, and the badge marks the usual starting point.

## 1. Plan

Decide what you want to correct and check that you have the data for it.

Example robot

The UR10 example ships everything: a URDF in `urdf/`, measured poses in `data/calibration.csv`, and a config that extends the manipulator template. Clone the examples repository and skip to [Configure](#configure) if you only want to see a run.

```
git clone https://github.com/thanhndv212/figaroh-examples
cd figaroh-examples/examples/ur10
```

Your own robot

You need:

- a URDF whose joint names match your logs;
- a CSV with one row per measured pose: the marker position (and orientation, if your sensor measures it) followed by one column per active joint;
- a second CSV, recorded separately, to validate on.

If your robot has no folder under `figaroh-examples/examples/` yet, do [Add your robot](https://thanhndv212.github.io/figaroh-plus/workflows/new-robot/index.md) first.

**What to estimate** tasks.calibration.parameters.calibration_level

Estimates joint offsets and the full geometric parameters of each link, plus base and tool frames. Use it when link lengths or orientations may differ from the CAD values. It needs more poses than `joint_offset`.

Estimates only the encoder zero of each joint, plus base and tool frames. Use it when the geometry is trusted and only the zeros drift, for example after re-mounting motors. It works with fewer poses.

**How to weigh prior knowledge** tasks.calibration.parameters.estimation.method

Plain least squares on the identifiable parameters. It reproduces earlier FIGAROH results and needs no extra information. Start here.

Maximum a posteriori: pulls each parameter towards its nominal value, weighted by `priors`. Use it when you know manufacturing tolerances. It gives the best prediction and reports an uncertainty for every parameter.

Like `map`, but picks the prior strength by cross-validation (`cv_folds`, `cv_multipliers`, `cv_rule: min | one_se`). Use it when you have no tolerance information.

Keeps only the parameters your poses excite, judged against `priors` and `excitation_k`. Use it when you know tolerances and want a short, readable parameter list.

Fits a named subset of parameters sized by cross-validation (`cv_sizes`). Use it when you want to compare a few candidate parameter sets.

Default priors are 1 mm for translations, 2 mrad for rotations, 20 mrad for joint offsets and 2 mm for prismatic offsets. Priors that are wrong by a factor of 10 lose most of the gain, so set them from your hardware. `noise_std: null` estimates the sensor noise. See [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md) for the maths and [Choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md) for a worked comparison.

**Done when** you can name the parameters you will estimate, the method, and the file you will validate on.

## 2. Design

Choose the poses to measure. Poses that move every joint through its range make every parameter identifiable. Poses clustered in one region leave some parameters undetermined.

Example robot

The bundled data is already well designed, so you can skip this step. To see how the poses were chosen, run:

```
MPLBACKEND=Agg python optimal_config.py
```

Your own robot

Generate a candidate pool within joint limits, then let FIGAROH select the subset that maximises information. Write your robot's `optimal_config.py` by copying the UR10 one, and record the selected poses on the robot.

**How many poses to keep** calculate_optimal_configurations(selection_method=…)

Keeps every pose whose optimal weight is above a threshold. It is simple and predictable.

Adds poses until the information gain stops improving: the relative change must stay below `iroc_rel_tol` (1e-3) for `iroc_patience` (3) steps. Use it to measure as few poses as possible.

Measure more poses than you need and keep a separate session aside for validation. See [Experiment design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md).

**Done when** the design report shows a condition number you accept and you have recorded both a calibration set and a validation set.

## 3. Configure

Write one YAML file that extends a shared template and only overrides what differs.

**Which template to extend** extends:

For UR10, Stäubli TX40, SO-101 and similar arms. It assumes a 6-DOF marker on the wrist.

For TIAGo, TALOS and other robots with a mobile or floating base. It turns external forces on, uses 1000 Hz sampling and a 3-DOF hand marker.

The root of both templates. Extend it only when neither fits.

Example robot

Open `config/ur10_unified_config.yaml`. The calibration block is short:

```
extends: "../../templates/manipulator_robot.yaml"
tasks:
  calibration:
    parameters:
      calibration_level: "full_params"
      outlier_threshold: 0.02          # metres
    kinematics:
      base_frame: "universe"
      tool_frame: "wrist_3_link"
    measurements:
      markers:
        - reference_joint: "wrist_3_joint"
          measurable_dof: [true, true, true, true, true, true]
    data:
      source_file: "data/calibration.csv"
      validation_data_file: ""         # set this to validate on held-out data
      number_of_samples: 29
```

Your own robot

Start from the block above and change, in order:

1. `kinematics.tool_frame` (required): the frame your marker is attached to. Set `base_frame` if your measurements are not in the `universe` frame, and `free_flying_base: true` for a floating base.
1. `measurements.markers[0].measurable_dof`: which of x, y, z, rx, ry, rz your sensor measures. The default is position only, `[true, true, true, false, false, false]`. Only the first marker is read.
1. `poses.base_pose` and `poses.tool_pose`: rough initial guesses for the sensor base and marker offset. Base and tool are always estimated freely.
1. `data.source_file`, `data.validation_data_file` and `data.number_of_samples`.

**Options to know before the first run**

Samples whose worst marker position error exceeds this many metres are dropped (default 0.05). With several markers the whole sample goes if one is an outlier. Lower it once the fit is stable.

Adds non-geometric terms. Default `false`; not supported with `map`, `map_cv` or `cv_subset`.

Set `eye_hand.enabled: true` with `camera_frame` and `reference_frame` when the sensor is a camera with an unknown 6-DOF mount. Use it with `map` and designed poses.

Must stay `0`. Use `map` or `map_cv` instead.

All keys: [Config keys](https://thanhndv212.github.io/figaroh-plus/concepts/config_parameters/index.md). How inheritance works: [Configuration system](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md).

**Done when** the config loads without errors and `number_of_samples` matches the rows in your CSV.

## 4. Fit

Run the solver and read the terminal report before trusting any number.

Example robot

```
python calibration.py --calibrate-only --no-plot
```

Without `--calibrate-only`, the script also exports an updated URDF and opens a viewer.

Your own robot

```
from figaroh.tools.robot import load_robot
from examples.my_robot.utils.my_robot_tools import MyRobotCalibration

robot = load_robot("urdf/my_robot.urdf", load_by_urdf=True)
cal = MyRobotCalibration(robot, "config/my_robot_config.yaml")
cal.initialize()
cal.solve(html_report=True, save_results=True)
```

`solve()` runs Levenberg–Marquardt (`method="lm"`) for up to `max_iterations=3` outer passes, removing outliers between passes. Pass `outlier_threshold=` to override the config for one run.

Read the report in this order:

1. **Condition number.** A very large value means some parameters are not identifiable from your poses. Go back to [Design](#design) or estimate fewer parameters.
1. **RMSE before and after.** It should drop substantially. If it does not, check the frames and the column order in your CSV.
1. **Outliers.** More than a few percent usually means a units or frame mistake, not bad data.
1. **Parameter values.** Offsets of several degrees or several centimetres are suspicious. With `map` methods, compare each value with its reported uncertainty.

**Done when** the RMSE drops, few samples are dropped as outliers, and every parameter value is physically plausible.

## 5. Verify

A good fit on the training data is not proof. Check prediction on poses the solver never saw.

**What the verdict covers** verify(scope=…)

Requires a held-out set (`data.validation_data_file`) and explicit limits that you set. It checks `position_rmse_mm`, plus `orientation_rmse_deg` when rotation is measured. FIGAROH sets no default limit, because acceptable error depends on your application.

Only checks that the fit produced finite numbers. Use it in CI or as a smoke test. It says nothing about accuracy.

```
verdict = cal.verify(
    thresholds={"position_rmse_mm": {"threshold": 1.0, "comparison": "max"}},
)
print(verdict.passed)
cal.export_verification_report()   # results/calibration_verification.json
```

Without validation data, FIGAROH falls back to the calibration data and prefixes the metrics with `training_`, labelling them `calibration_data_fallback`. A fallback can never pass a prediction check. A missing required metric reads `not_evaluated`, not pass.

Example robot

The UR10 config has no validation file, so its prediction verdict reads `not_evaluated`. Run `python calibration.py` without flags to export the updated URDF and check its forward kinematics.

Your own robot

Export the corrected URDF once the verdict passes, then reload it and compare predicted poses on a fresh session. The [Reports & verdicts](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) page covers the HTML report and the two-run compare page.

**Done when** `verify()` passes in prediction scope on held-out data with limits you chose, and the exported URDF reproduces the validation poses.

# Identify dynamics

Fit masses, inertias, friction and actuator inertia from recorded motion and joint torques. Five steps, each with the choices you make and how you know it worked.

Use the toggle below to read the steps for the bundled UR10 example or for your own robot. Each choice card lists the options FIGAROH supports, and the badge marks the usual starting point.

## 1. Plan

FIGAROH writes joint torque as a linear function of the inertial parameters and estimates the **base parameters**: the combinations of masses and inertias your data can actually determine. Decide which extra effects to model and check that you have the data for them.

Example robot

The UR10 example ships a training trajectory and a separate validation trajectory (`data/truth/`), recorded at 100 Hz.

```
git clone https://github.com/thanhndv212/figaroh-examples
cd figaroh-examples/examples/ur10
```

Your own robot

You need:

- a URDF with reasonable nominal inertias (used as the starting point and for reconstruction);
- joint positions and joint torques (or motor currents with a known torque constant) from an exciting motion;
- a **separate** recording, in the same layout, to validate on;
- the true sampling rate, read from the timestamps.

If your robot has no folder under `figaroh-examples/examples/` yet, do [Add your robot](https://thanhndv212.github.io/figaroh-plus/workflows/new-robot/index.md) first.

**What to model** tasks.identification.problem.model_components

All components `false`. Fits masses, first moments and inertias. Get this working before adding terms.

Adds viscous and Coulomb friction per joint. Almost always needed on geared robots. Turn it on when torque residuals change sign with velocity.

Adds rotor inertia reflected through the gearbox. Use it on high-ratio drives when residuals track acceleration.

Adds a constant torque offset per joint, for sensor bias.

**Which torque signal** tasks.identification.problem

Fits measured joint torques. `torque_fit_joints` restricts the fit to a list of joints.

Fits base wrenches from force/torque sensors (`force_torque_sensors`, `external_wrench_offsets`). Use it for floating-base robots. The humanoid template turns it on.

**Done when** you can name the model components, the torque source, and the file you will validate on.

## 2. Design

A slow or repetitive motion leaves most parameters undetermined. An exciting trajectory moves every joint with varied velocity and acceleration within its limits.

Example robot

The bundled trajectory is already exciting, so you can skip this step. To see how it was generated:

```
MPLBACKEND=Agg python optimal_trajectory.py
```

Your own robot

Copy the UR10 `optimal_trajectory.py` and its `tasks.optimal_trajectory` block (waypoints, segment duration, joint box, velocity and acceleration scaling). Execute the result on the robot and record a second, different motion for validation.

`OptimalTrajectory.solve(stack_reps=2)` minimises the condition number of the base regressor. It needs cyipopt, which installs from conda only (`conda env create -f environment.yml`). See [Experiment design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md).

**Done when** the trajectory stays within limits, its condition number is acceptable, and you have recorded both a training and a validation motion.

## 3. Configure

Write one YAML file that extends a shared template. Identification settings sit under `tasks.identification`.

Example robot

```
extends: "../../templates/manipulator_robot.yaml"
tasks:
  identification:
    problem:
      use_joint_torques: true
      model_components:
        friction: false
        actuator_inertia: false
        joint_offset: false
    signal_processing:
      sampling_frequency: 100.0   # Hz, matches the recorded clock
      cutoff_frequency: 10.0      # Hz, below Nyquist (50 Hz)
      filter_type: "butterworth"
      filter_order: 4
    data:
      validation_data_file: "data/truth/validation.csv"
```

Your own robot

Pick the template (`manipulator_robot.yaml` for fixed-base arms, `humanoid_robot.yaml` for mobile or floating bases), then set:

1. `signal_processing.sampling_frequency` from your timestamps. The template defaults (500 Hz, 1000 Hz for humanoids) are rarely right for your logger.
1. `cutoff_frequency` above the motion content and below half the sampling rate.
1. `data.validation_data_file` to the separate recording.
1. `coupling.has_coupled_wrist` to `false` unless your wrist joints are mechanically coupled.

**Velocity and acceleration from positions** signal_processing.differentiation_method

Second-order central differences in the interior, one-sided at the ends. Good for most logs.

Central differences. Similar to `gradient`.

One-sided differences. Use them only to match another tool's processing.

**Beyond base parameters** tasks.identification

No extra keys. You get the identifiable combinations, which predict torque but are not individual link masses.

`reconstruction: {enabled: true, method: nullspace}` recovers a full per-link parameter set closest to the URDF. `method: sdp` enforces physical consistency (needs picos); `auto` tries `sdp` and falls back to `nullspace`.

`physical_consistency: {enabled: true}` checks or enforces positive masses and positive-definite inertias (`mass_min`, `psd_eig_tol`, `solver: cvxopt`).

Chooses which result is reported and exported: `fit`, `reconstruction` or `physical_fit`. `physical_fit` is a research path.

If the log warns that the regressor is ill-conditioned, set `problem.qr_relative_tolerance: 1e-4`. All keys: [Config keys](https://thanhndv212.github.io/figaroh-plus/concepts/config_parameters/index.md).

**Done when** the config loads, the sampling rate matches your timestamps, and the cutoff is below Nyquist.

## 4. Fit

Example robot

```
MPLBACKEND=Agg python identification.py
```

The script writes an HTML report and runs `verify()` by default. Add `--wls` to refine with weighted least squares.

Your own robot

```
from figaroh.tools.robot import load_robot
from examples.my_robot.utils.my_robot_tools import MyRobotIdentification

robot = load_robot("urdf/my_robot.urdf", load_by_urdf=True)
ident = MyRobotIdentification(robot, "config/my_robot_config.yaml")
ident.initialize()
ident.solve(plotting=False, html_report=True, save_results=True)
```

**Which estimator** solve(…)

`solve()` reduces the regressor to base parameters by QR decomposition and solves ordinary least squares. It decimates by `decimation_factor=10` first to cut noise and size.

`solve(wls=True)` reweights each joint by its residual variance (iteratively reweighted least squares). Use it when joints have very different torque noise. The `problem.wls` config key is not read; pass the argument.

`solve_with_custom_solver(method=…)` takes one of `lstsq`, `qr`, `svd`, `ridge`, `lasso`, `elastic_net`, `tikhonov`, `constrained`, `robust`, `weighted`. Use it for regularisation or bounds.

Read the report in this order:

1. **Condition number of the base regressor.** A very large value means your motion does not excite some parameters. Go back to [Design](#design).
1. **Torque fit per joint.** Residuals should look like noise. Structure that follows velocity points to friction, and structure that follows acceleration points to actuator inertia.
1. **Relative standard deviation per parameter.** Large values mark parameters the data barely determines.

**Done when** the conditioning is acceptable, residuals look like noise on every joint, and well-determined parameters have small relative deviation.

## 5. Verify

Check torque prediction on the separate recording.

**What the verdict covers** verify(scope=…)

Requires the validation recording and a `validation_rmse:<joint>` limit in N·m for **every** active joint. Optional checks: `validation_abs_bias`, `validation_peak_error`. FIGAROH sets no default limits.

Only checks that the fit produced finite, consistent outputs. Use it in CI or as a smoke test.

```
limits = {f"validation_rmse:{j}": {"threshold": 2.0, "comparison": "max"}
          for j in ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                    "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}
verdict = ident.verify(thresholds=limits)
print(verdict.passed)
ident.export_verification_report()   # results/identification_verification.json
```

From the example scripts, pass the same limits on the command line:

```
python identification.py --verification-scope prediction \
  --acceptance-profile '{"validation_rmse:elbow_joint": {"threshold": 2.0, "comparison": "max"}, ...}'
```

Without validation data, metrics fall back to the training data, are labelled `identification_data_fallback`, and cannot pass a prediction check.

Your own robot

To compare two runs, for example with and without friction, build a static compare page with `figaroh.tools.compare_report.generate_compare_page`. To write identified inertias back to a URDF, every exported link needs all ten inertial values, and physically infeasible sets are refused. Reload the written URDF and check its predicted torques before using it. [Reports & verdicts](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) covers the HTML report and the stage records.

**Done when** `verify()` passes in prediction scope on the separate recording, with a limit you chose for every joint.

# Add your robot

Give your robot a folder in figaroh-examples with a URDF, a config and a tools subclass, so the calibration and identification guides work for it.

Only do this when `figaroh-examples/examples/` has no folder for your robot yet. Otherwise go straight to [Calibrate a robot](https://thanhndv212.github.io/figaroh-plus/workflows/calibration/index.md) or [Identify dynamics](https://thanhndv212.github.io/figaroh-plus/workflows/identification/index.md).

**Which example to copy from**

`examples/ur10/` with `templates/manipulator_robot.yaml`. It is the most complete reference: calibration, identification, both design scripts and verification flags. Use it for UR, KUKA, Stäubli and similar arms.

`examples/tiago/` or `examples/talos/` with `templates/humanoid_robot.yaml`. Use them for mobile manipulators, humanoids, floating bases and coupled wrists.

## 1. Place the description package

If your URDF references meshes with `package://<robot>_description/...`, put that package at `figaroh-examples/models/<robot>_description/`. Add to `models/`; do not edit what is already there. Skip this if the URDF has no mesh references.

## 2. Scaffold the folder

```
cd figaroh-examples/examples
./create_example.sh my_robot
```

This creates `examples/my_robot/` with `config/`, `data/`, `urdf/`, `utils/` and `docs/`. The generated `config/my_robot_unified_config.yaml` is a real config with TODO markers; edit it in place. The four generated scripts (`calibration.py`, `identification.py`, `optimal_config.py`, `optimal_trajectory.py`) are placeholders that point to their UR10 counterparts.

## 3. Add the URDF and prove it loads

Put the file at `examples/my_robot/urdf/my_robot.urdf`, then print its joints and frames:

```
cd figaroh-examples/examples/my_robot
python -c "
from figaroh.tools.robot import load_robot
r = load_robot('urdf/my_robot.urdf', package_dirs='../../models', load_by_urdf=True)
print('joints', list(r.model.names))
print('frames', [f.name for f in r.model.frames][:40])
"
```

Copy names from this output. They are the exact strings your config's `active_joints`, your CSV headers, and `base_frame` / `tool_frame` must use.

## 4. Fill in the config

```
extends: "../../templates/manipulator_robot.yaml"   # or humanoid_robot.yaml
robot:
  name: "my_robot"
  properties:
    joints:
      active_joints: ["joint_1", "joint_2"]   # exact names from step 3
      joint_limits:
        position: [...]       # rad, min/max per joint, flattened
        velocity: [...]       # rad/s
        acceleration: [...]   # rad/s²
        torque: [...]         # N·m
tasks:
  calibration:    { enabled: true }
  identification: { enabled: true }
```

- Use `extends:`. The key `inherit_from` is not read.
- Robot-wide facts go under `robot.properties`, task settings under `tasks.<task>`. Task settings placed under `robot.properties` are silently ignored.
- Enable only the tasks you have scripts for.

## 5. Write the tools subclass and scripts

Replace `utils/my_robot_tools.py` with a copy of `examples/ur10/utils/ur10_tools.py` and adapt it:

| Task           | Base class                                        | You implement                            |
| -------------- | ------------------------------------------------- | ---------------------------------------- |
| Calibration    | `BaseCalibration`                                 | data and marker handling for your sensor |
| Identification | `BaseIdentification`                              | `load_trajectory_data()`                 |
| Design         | `BaseOptimalCalibration`, `BaseOptimalTrajectory` | your constraints                         |

Then copy the entry scripts from `examples/ur10/` and change the class import, the `--config` and `--urdf` defaults, and the data paths. Keep the command-line flags the same as UR10's so `validate.py` can drive your robot.

## 6. Add data and run

Put your CSVs under `data/` and follow the Configure step of the [calibration](https://thanhndv212.github.io/figaroh-plus/workflows/calibration/#configure) or [identification](https://thanhndv212.github.io/figaroh-plus/workflows/identification/#configure) guide.

**Done when** `python validate.py --robot my_robot` exits 0 from the `figaroh-examples` root, and a run archive appears under `examples/my_robot/results/runs/`.

Let your agent do it

The `figaroh-setup-new-robot` skill walks a coding agent through these steps, including registering the robot in `validate.py`. See [AI agents](https://thanhndv212.github.io/figaroh-plus/agents/index.md).

# Plan, fit and validate a robot example

**Draft guideline for review — 2026-10-02.** This is a decision process for using FIGAROH with a new robot or dataset. It complements the runnable [tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md); it does not introduce a new pipeline API. Record each decision in the examples repository's [experiment brief](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/experiment-brief-template.md). Reference workflows demonstrate the process; a new example must still audit its own sensors, model, assumptions and validation evidence.

## 1. Define the outcome and inventory available evidence

Start by answering these questions before selecting a solver:

| Question                          | Record                                                                                                                    | Consequence                                          |
| --------------------------------- | ------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------- |
| What should improve?              | Effort prediction, geometric accuracy, or an exported model for a declared application                                    | Determines residuals and acceptance metrics          |
| Which model is available?         | URDF/source revision, geometry packages, joint names/order, active chain, fixed/free base, payload and nominal parameters | Defines coordinates, frames and model assumptions    |
| What measurements exist?          | Raw files, timestamps, units, sensors, effort source, measured pose/contact components and missing channels               | Determines which estimation problem is supported     |
| What can be collected?            | Accessible joints/poses, instrumentation, controller, logging rate, operating limits and recording time                   | Determines feasible excitation and validation design |
| What independent evidence exists? | Separate recording, unused trajectory/postures, synthetic truth or none                                                   | Limits the validation claim                          |
| What must remain fixed?           | Known payload, unobserved joints, sensor transforms, geometry, friction or other nuisance parameters                      | Reduces ambiguity and defines the estimation scope   |

Classify every input as measured, commanded, numerically derived, inferred or simulated. Encoder position and commanded position are different signals. Current-derived effort needs a documented conversion, gear ratio, sign and joint-side convention; it is not automatically a calibrated torque measurement. Keep unknown quantities unknown rather than filling them with undocumented constants. Missing dynamic measurements may support a reduced gravity/friction study, but do not establish full inertial identification.

**Output:** a task statement, input inventory, missing-data list and intended validation level. If critical data are absent, plan acquisition or narrow the model before fitting.

## 2. Choose the model and parameter scope

For **dynamic identification**, declare the rigid-body inertial blocks and any friction, actuator inertia, coupling or external-force terms. State which are fitted, fixed or excluded. Motion/effort data identify parameter combinations; an apparently precise full inertial vector may simply be one choice in an unobservable null space. Distinguish base parameters from a physical full model and from nuisance parameters.

For **geometric calibration**, declare the chain, measured frame, sensor/world frame, joint offsets/link corrections and any base/tool/marker transforms. State the gauge: which frame or transform anchors otherwise interchangeable corrections. A position-only marker supports position residuals; plane contact constrains the observed contact component. Neither supplies full 6D pose truth. Use measured joint configurations where available and document substitutes.

For calibration, which identifiable corrections to estimate, and whether with priors, is a separate choice from the calibration level: see [Calibration: choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md).

Start with the smallest model that explains the measured phenomenon. Add a parameter block only with a physical reason, observability evidence and a validation test. Record payload, temperature/contact regime and other operating conditions that could invalidate a single-model assumption.

**Output:** parameter/block table, observation equation, units, frame conventions, fixed quantities, gauge and unsupported effects.

## 3. Select methods with explicit objectives

| Method                              | Appropriate question                                                      | Required interpretation                                                                                                                                                                                                                                                        |
| ----------------------------------- | ------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| Base-parameter OLS                  | Which observable dynamic combinations explain effort?                     | Prediction baseline; does not guarantee physical individual inertias                                                                                                                                                                                                           |
| WLS or regularized estimation       | Is there a justified noise model or prior?                                | Estimate weights/scales from training evidence; disclose prior and its influence                                                                                                                                                                                               |
| Exact SDP/LMI reconstruction        | Can a physical full model preserve the chosen base vector?                | Check solver status, physical constraints and base residual; a returned fallback is not success                                                                                                                                                                                |
| Per-link physical projection        | Can an estimated link vector be made physically consistent?               | Recompute effort/base fit afterward; projection may alter prediction                                                                                                                                                                                                           |
| Direct physical constrained fit     | Which physical model minimizes the declared effort objective?             | Separate experiment from exact reconstruction; confirm the selected implementation is supported                                                                                                                                                                                |
| Log-Cholesky fit                    | Can positive pseudo-inertia coordinates produce an adequate physical fit? | Feasibility and optimizer convergence are separate; currently a research path pending convergence review                                                                                                                                                                       |
| Geometric least-squares calibration | Which identifiable corrections reduce the measured pose/contact residual? | Declare residual components, scaling, gauge, priors and redistribution semantics; choose the [estimation method](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md) (`structural`, `excitation`, `map`, `map_cv`, `cv_subset`) and record it |

This table is a selection guide, not a promise that every method is exposed by every robot entry point. Check the [identification](https://thanhndv212.github.io/figaroh-plus/api/identification/index.md), [calibration](https://thanhndv212.github.io/figaroh-plus/api/calibration/index.md) and [tools](https://thanhndv212.github.io/figaroh-plus/api/tools/index.md) interfaces and the robot adapter. Link a missing capability to a scoped issue instead of silently substituting a different objective.

Compare methods with the same training/validation samples, conventions, residual scaling and extra-parameter policy. Freeze extras to isolate inertial methods, or compare joint estimation as a separately declared protocol. Record initialization, priors, bounds, tolerances, solver/version, evaluation budget and timeout. Select hyperparameters on training/development data; reserve the final validation set for assessment.

**Output:** baseline, candidate methods, objective/constraint definitions and a comparison protocol frozen before inspecting final validation results.

## 4. Design acquisition and validation together

If data already exist, first assess what they excite and where coverage is missing. Do not assume a long recording supplies identifiable parameters. For a new experiment, define both fitting and validation recordings up front.

For dynamics, vary configurations and motion to expose the selected gravity, inertial and friction effects. Inspect the relevant regressor's rank and singular spectrum under declared column scaling. Include direction/speed coverage when estimating friction. For calibration, vary chain configurations and marker observations to expose the chosen corrections; inspect the measurement Jacobian and gauge dependencies. Repeated similar samples cannot replace missing directions of information.

Use [optimal experiment design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md) where applicable, with the same parameter scope and constraints as the intended task. Compare against a feasible baseline; matrix conditioning alone does not establish better real measurements or validation accuracy. Check generated motion/poses against the actual robot's position, velocity, acceleration, effort, workspace, collision and acquisition constraints before execution. An offline optimizer result is a candidate experiment, not evidence of executed motion.

Prefer an independent validation recording with different unused excitation or postures in the intended operating range. A temporal block from one recording is useful but weaker evidence. For filtered time series, separate blocks by a guard interval sufficient for the documented filter/derivative support. Random neighboring-sample splits can leak strongly correlated information.

**Output:** acquisition plan, excitation rationale, coverage diagnostics, training/development/final-validation assignments and predeclared thresholds.

## 5. Collect and preserve measurements

Preserve immutable raw recordings and nominal models. Record acquisition date, robot/controller state, payload, sensor configuration, calibration/conversion, clock sources, actual sample intervals and missing/saturated/dropped samples. Document whether streams share a clock and how offset/drift is established. Store enough metadata to reconstruct joint order, units and all frame transforms.

Run a small acquisition check before the full experiment: inspect channel signs, ranges, timestamps, synchronization and observation availability. Keep recorded executed motion separate from the planned trajectory. Synthetic runs need known model parameters, independently checked states/derivatives and saved noise seeds; using the same code to generate and fit data alone can conceal a shared convention bug.

**Output:** raw-data manifest, acquisition notes and sanity-check figures.

## 6. Process data through an auditable adapter

Keep robot-specific import/conversion in `examples/<robot>/utils/`; reusable numerical processing belongs in the core library. Preserve a mapping from raw samples to accepted, trimmed and decimated samples.

1. Check timestamps, missing values, joint order, dimensions, units and frames.
1. Align streams with an explicit policy; record interpolation and exclusions.
1. Establish the actual rate from timestamps. Set filter cutoffs in the correct units and document assumptions if timestamps are unavailable.
1. For dynamics, check velocity/acceleration provenance for **every active coordinate**. Handle `nq != nv` deliberately; do not blindly differentiate configuration columns as if all were scalar joint angles.
1. Document filter type/order, phase behavior, derivative method and edge trim. Apply consistent index changes to all corresponding channels.
1. Process split partitions independently so smoothing, learned noise weights, outlier rules or normalization do not borrow final-validation information.
1. Inspect raw/processed overlays and derived signals before constructing the regressor or calibration residual.

For geometric observations, check pose representation, rotation conventions and marker validity; do not average or subtract rotation coordinates without considering their representation. Label offline noncausal filtering if relevant to the eventual deployment.

**Output:** processing recipe, accepted indices, split provenance and processed signal diagnostics. Current rate/derivative limitations in the delivery plan must be checked for the chosen adapter before trusting a fit.

## 7. Fit and read the result in layers

Fit the nominal/baseline and candidates under the frozen protocol. Preserve raw estimates, reconstructed/projected estimates and failed candidates as separate stages. Read the result in this order:

| Layer                    | Inspect                                                                                                                                                                                                                                                        | A useful conclusion                                      |
| ------------------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------- |
| Input correctness        | Rates, signs, frames, synchronized channels and coverage                                                                                                                                                                                                       | Whether the fitted problem matches the observations      |
| Numerical termination    | Success flag, residual/objective, budget, bounds and fallback                                                                                                                                                                                                  | Whether the intended solve completed                     |
| Training fit             | Per-joint/component RMSE, bias, residual traces and nominal comparison                                                                                                                                                                                         | What the model explains in the fitted data               |
| Parameter interpretation | Rank, observable combinations, physical checks, gauge and prior sensitivity (calibration: `estimation_report`, posterior vs prior; see the [guide](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/#5-read-the-diagnostics)) | Which parameter claims are justified                     |
| Held-out prediction      | Same metrics on unused data, coverage and leakage checks                                                                                                                                                                                                       | Whether the improvement transfers                        |
| Export parity            | Reloaded-model effort or FK vs selected fitted stage                                                                                                                                                                                                           | Whether the delivered model represents the fitted result |

Plot residuals against time and relevant configuration/velocity/acceleration, as applicable. Persistent bias, phase lag or structured errors warrant checking synchronization, conversion or missing effects before adding model complexity. Correlation alone can conceal amplitude/bias error. Report each effort in its proper unit (N or Nm), and translation/rotation separately unless justified whitening defines a dimensionless residual. Do not combine mixed units into an unlabeled aggregate error. Define relative errors and zero-signal behavior.

Good effort prediction does not prove each link parameter is correct. Synthetic truth comparisons should use identifiable coordinates where appropriate; real-data parameter analysis needs physical plausibility and sensitivity, not an invented ground-truth vector. A positive pseudo-inertia candidate can still have an unsuccessful optimizer termination or poor validation fit.

**Output:** methodology, procedure, model fitting results, parameter analysis, held-out validation and limitations, with every candidate's status retained.

## 8. Validate, export and make the example reusable

Evaluate final held-out data with the selected model and frozen processing. Label evidence as training-only, temporal holdout, separate simulation or independent real recording. If no genuine held-out data exist, state that validation is unavailable; a training fallback does not fill this gap.

Use [reporting and verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md), but inspect which checks were computed, skipped and passed. A passing verdict with absent validation checks does not establish generalization. Choose task thresholds from the application and measurement limits before measuring; preserve existing regression gates unless a separate reviewed change justifies them.

Export only an explicitly selected, accepted result stage to a new output. Reload the model and compare FK for calibration or inverse dynamics for identification on declared states. Check inertial frames/CoM/tensors and physical consistency where relevant. Existing inertial-export limitations require explicit checks; a written URDF alone is insufficient evidence.

Archive paired core/examples revisions, dependency versions, input/model/config hashes, commands, processing/splits, solver settings/status, metrics, reports, selected stage and export/reload evidence. Specify which fields the existing archive captures and attach the remaining evidence explicitly. Hardware control or rollout claims require their own executed experiment.

A reusable example includes a README explaining purpose and limitations, data and model provenance, unified config, a small supported adapter, reproducible commands, an evidence report and regression tests for its essential behavior. See the examples repository's [new-example guide](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/new-example-guide.md) for the folder layout and review checklist.

# Reporting & Verification (V&V)

Every `BaseCalibration`/`BaseIdentification` run produces the same underlying metrics — condition number, per-DOF/per-joint residuals, parameter uncertainty, held-out validation. This guide covers the four ways those metrics get surfaced, and when to reach for each one.

| Artifact                        | Call                                                   | Audience                                     |
| ------------------------------- | ------------------------------------------------------ | -------------------------------------------- |
| Terminal quality report         | automatic, after `solve()`                             | you, right now, in the terminal              |
| Self-contained HTML report      | `solve(html_report=True)` / `export_html_report()`     | you, or anyone you send the file to          |
| Machine-readable verdict (JSON) | `verify()` / `export_verification_report()`            | CI, scripts, anything that needs a pass/fail |
| Static two-run compare page     | `figaroh.tools.compare_report.generate_compare_page()` | comparing two exported verdicts, offline     |

All four read from data `solve()` already computed — none of them re-run the calibration/identification, and none of them require network access or a backend.

## Interpreting a verdict

Use [Plan, Fit and Validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) to establish input correctness, parameter scope and independent validation before interpreting these reports. Inspect computed and skipped checks: a passing verdict with unavailable validation metrics does not establish held-out accuracy. Keep solver termination, physical feasibility, prediction quality and export/reload parity as separate conclusions. The compare page's compatibility checks do not establish matching raw inputs, processing, objectives or absence of leakage; retain that provenance.

### Stages, splits and revisions

Each run records what each step did (`figaroh.tools.stages`, #55/#63). The terminal report prints them on a `Stages:` line, and the HTML report shows a **Stages and data** section.

Each step gets its own verdict in `verdict.stages`:

- the steps are `data`, `fit`, `validation`, `physical` and `export`;
- each is `pass`, `fail`, `fallback` or `not_run`, or `not_evaluated` when the run recorded nothing for it;
- the existing entries (`numerical_execution`, `prediction`, `solver`) are kept beside them.

A `validation` **fallback** means the metrics were computed on the training data. It is never held-out evidence, and prediction acceptance cannot pass on it.

| Field                                  | Meaning                                                                                                    |
| -------------------------------------- | ---------------------------------------------------------------------------------------------------------- |
| `verdict.stage_records`                | every step's status, reason and metrics with units                                                         |
| `verdict.selected_stage`               | the step whose parameters the result reports (`fit`; `none` if the fit failed)                             |
| `verdict.splits`                       | which files, sessions and sample counts trained and validated the run, and whether validation was held out |
| provenance `software.git_commit`       | the working directory's commit (an example run: the examples repository)                                   |
| provenance `software.figaroh_revision` | the figaroh checkout's own commit and dirty state                                                          |

Together, the two commits name the paired core/examples revisions. The exported JSON carries the records under `stage_records` and a `schema_version`. The run archive writes `stages.json` and adds each run's stage verdicts to `index.jsonl`.

## Terminal quality reports

`print_quality_report()` runs automatically at the end of `solve()`; call it again any time after a successful solve to reprint it:

```
calibrator.solve()
calibrator.print_quality_report()   # same output, printed again
```

For calibration this prints convergence status, outlier count, condition number, per-DOF residual statistics (mean/std/RMSE/max/R²), held-out validation (if `validation_data_file` is configured), and any parameter pairs with `|ρ| > 0.8`. For identification it prints base-parameter count, condition number, RMSE, correlation, per-joint torque residuals, the top-5 worst-identified base parameters, and (if enabled) physical-consistency / reconstruction status.

## Self-contained HTML reports

```
calibrator.solve(html_report=True)
# or, after the fact:
calibrator.export_html_report(output_path="results/calibration_report.html")
```

The identification side is the same shape:

```
identifier.solve(html_report=True)
identifier.export_html_report(output_path="results/identification_report.html")
```

Each report is a single self-contained HTML file — no external requests, no CDN dependency, inline CSS/JS only, light/dark theme aware — so it opens correctly straight from disk and is safe to attach to an email or a ticket. It contains:

- **Summary** — the same headline numbers as the terminal report.
- **Insights** — auto-flagged issues (ill-conditioning, poorly-identified parameters, weak validation improvement, no validation data configured).
- **Per-DOF / per-joint residuals** and **parameter uncertainty** tables, each parameter's relative uncertainty rendered as a confidence-tier bar.
- **Validation** — nominal vs. fitted vs. measured, when a genuinely separate held-out dataset was configured.
- **Before / after** — an interactive overlay chart of the same nominal/fitted/measured series, with wheel-to-zoom and hover-to-inspect, so you don't have to read the numbers out of a table to see the fit improve. This needs no extra call — it's populated automatically whenever validation data is available.

## Machine-readable verification

`verify()` checks the run's metrics against a set of pass/fail thresholds and returns a `VerificationVerdict` — this is the piece that turns a report (for a human to read) into something a script can branch on:

```
verdict = identifier.verify(scope="execution")
print(verdict.passed)          # bool
for check in verdict.checks:
    print(check.name, check.value, check.comparison, check.threshold, check.passed)
```

A bare library `verify()` defaults to **prediction acceptance**, which stays incomplete without a validation set and explicit limits. Routine example CLIs explicitly request **numerical execution**:

```
verdict = identifier.verify(scope="execution")
print(verdict.scope, verdict.status, verdict.stages)
```

This checks finite, nonempty fitted parameters, predictions, measurements and RMSE, matching effort dimensions, and whether explicitly requested validation loaded. Calibration additionally checks its reported optimization success and finite parameter/residual outputs. It does not certify data provenance, a full physical model or export/reload parity. Those stages remain `not_evaluated`.

There are **no universal improvement, correlation, raw condition-number or calibration-error gates**. These remain report diagnostics. A good nominal model may have little improvement; high correlation can coexist with biased torque; raw conditioning changes with parameter units and basis scaling.

For prediction acceptance, provide independent validation data and explicit application limits. For identification, supply an absolute maximum RMSE in Nm for **every active validation joint**:

```
profile = {
    f"validation_rmse:{joint}": {
        "threshold": allowed_error_nm[joint],
        "comparison": "max",
        "rationale": "measurement uncertainty and the application requirement",
    }
    for joint in active_joints
}
verdict = identifier.verify(scope="prediction", thresholds=profile)
```

Identification also exposes `validation_abs_bias:<joint>` and `validation_peak_error:<joint>` in Nm (or `training_…` for fallback). Add explicit limits where the application needs them; the per-joint RMSE limits remain required.

For calibration, use `position_rmse_mm` and, when rotational DOFs are measured, `orientation_rmse_deg`, with application-specific maximum limits. No replacement universal error value is supplied. Record units, the requirement behind each limit and the validation split before evaluating acceptance. Loading a separate file is necessary but does not prove experimental independence: split integrity, coverage and acquisition provenance remain the experiment owner's responsibility.

`passed` remains available for existing consumers and is true only if all required checks **in the named scope** pass. Additive `status`, `scope`, `stages` and applied `policy` fields make the meaning explicit:

- `pass`: all required checks in scope succeeded.
- `fail`: a required evaluated check failed, including computed NaN/Inf evidence.
- `not_evaluated`: required evidence or a prediction profile is unavailable.

Missing checks are retained with their reason, rather than silently skipped. An empty threshold-only policy is `not_evaluated`, never PASS. A spec may set `required=False` for an advisory check; it is still reported. A failed explicitly requested validation load prevents execution acceptance. Training fallback can support diagnostics but cannot pass prediction acceptance. Physical and export acceptance require their own evidence; a scoped prediction PASS is not a full physical-model approval. Compare historical verdicts with care: their unscoped boolean used the earlier universal gates, not this policy.

Write the verdict to disk as JSON (numpy-safe, includes git commit / config hash / timestamp provenance):

```
path = identifier.export_verification_report(
    output_path=None,       # defaults to {output_dir}/identification_verification.json
    output_dir="results",
    thresholds=None,        # explicit limits, when evaluating prediction
    scope="execution",
)
```

The same call exists on `BaseCalibration` (writes `{output_dir}/calibration_verification.json`).

### Wiring `--verify` into a CLI script

This is the pattern already used by the `identification.py` entry points in [figaroh-examples](https://github.com/thanhndv212/figaroh-examples):

```
verdict = identifier.verify(scope="execution")
identifier.export_verification_report(output_path=str(run_dir / "verdict.json"), scope="execution")

for check in verdict.checks:
    status = check.status.upper()
    print(f"  [{status}] {check.name}: {check.value} ({check.comparison} {check.threshold:.4g})")

if not verdict.passed:
    sys.exit(1)
```

Run it as `python identification.py --verify`. The scope must be printed with the verdict: zero means numerical execution passed by default, not that an independent prediction or physical model was accepted. Prediction-scoped incomplete evidence produces a nonzero exit.

## Comparing two runs

`figaroh.tools.compare_report.generate_compare_page()` renders a static, self-contained HTML page that loads **two** `export_verification_report()` JSON files client-side (drag-and-drop or a file picker) and, once they pass a compatibility check, shows a per-metric diff table and an overlaid before/after series chart with a per-run visibility toggle:

```
from figaroh.tools.compare_report import generate_compare_page

generate_compare_page(output_path="results/compare.html")
```

Open `results/compare.html` in a browser and load two verdict JSON files — everything after that runs entirely client-side; there's no server, no network request, and nothing is uploaded anywhere.

**The compatibility check runs before anything renders.** Two verdicts are compared only if they agree on:

- domain (calibration vs. identification — inferred from `compat.dof_names` vs. `compat.active_joints`)
- the same DOF/joint names
- the same `decimate` setting (identification only)
- comparable sample counts (within ~20%)

On a mismatch the page blocks the comparison and explains why (e.g. "Different decimate setting: false vs. true") rather than silently overlaying two runs that aren't actually comparable — you can still force the comparison via an explicit "compare anyway" checkbox, with a persistent warning banner while forced.

This is deliberately a static, no-backend artifact, not a run library or dashboard — see the design rationale in [`docs/decisions/external-tool-comparisons.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/external-tool-comparisons.md) Part C if you're curious why (short version: no validated need yet for a run history/backend, and comparing incompatible runs silently was judged actively dangerous).

## CI integration example

A minimal build-gate recipe combining the pieces above:

```
identifier.solve(html_report=True)
verdict = identifier.verify()
identifier.export_verification_report(output_dir="results")

if not verdict.passed:
    failed = [c.name for c in verdict.checks if not c.passed]
    print(f"Verification failed: {', '.join(failed)}")
    sys.exit(1)
```

Point your CI at `results/*_verification.json` as a build artifact, and at `results/*_report.html` if you want a human-readable report attached to the same run.

# Tutorials

These walkthroughs explain the *why* and *how* behind FIGAROH's four core workflows, using TIAGo (a mobile manipulator) as the running example. Each one is backed by a real, runnable script in [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) — see the [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) for the complete per-robot reference implementations (UR10, TIAGo, TALOS, Staubli TX40) once you've read the walkthrough for the workflow you need.

| Tutorial                                                                                                                | Workflow                              | Answers                                                                        |
| ----------------------------------------------------------------------------------------------------------------------- | ------------------------------------- | ------------------------------------------------------------------------------ |
| [Calibration Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md)        | Kinematic calibration                 | Why do robots need calibrating, and how does FIGAROH solve for the correction? |
| [Choosing What to Estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md) | Kinematic calibration                 | Which corrections should I estimate, and how, for my robot and postures?       |
| [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md)  | Dynamic parameter identification      | How does FIGAROH turn a torque/motion log into a validated dynamic model?      |
| [Optimal Experiment Design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md)               | Optimal configurations & trajectories | How does FIGAROH decide *which* poses/motions to measure, instead of guessing? |

## Starting with a new robot or dataset

Begin with [Plan, Fit and Validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) to inventory data, choose the model/methods, design acquisition, process measurements and establish independent validation. These task walkthroughs start after those decisions; the examples repository supplies a reusable experiment brief and integration guide.

## Prerequisites

- FIGAROH installed (see [Getting Started](https://thanhndv212.github.io/figaroh-plus/getting_started/index.md))
- A robot URDF and a [unified config](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) for it — or clone [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) and use one of the shipped robot folders directly
- Basic familiarity with the linear-in-parameters formulation `τ = W(q, q̇, q̈) · φ` for dynamics; geometric calibration instead fits pose/contact residuals and analyzes their parameter Jacobian

## The four workflows, at a glance

```
Optimal Configuration Generation ──▶ (collect calibration data) ──▶ Kinematic Calibration
Optimal Trajectory Generation    ──▶ (collect identification data) ──▶ Dynamic Identification
```

The two design steps are optional tools for choosing informative, feasible experiments (see [Optimal Experiment Design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md)). Acquisition coverage, synchronization and sensor quality still limit the resulting fit.

Use verification and HTML reports where supported by the selected workflow — see [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md). Inspect skipped checks and actual held-out data; a passing verdict alone is not proof of a validated model.

# Choosing what to estimate in a calibration

A calibration's result depends on **which parameters you estimate and how** as much as on the measurements. FIGAROH does not make that choice for you: it provides several methods, diagnostics to compare them, and this guide. The methods themselves (what each does, what it needs, pros and cons) are described in the reference page [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md).

This guide is the workflow: start from a baseline, decide what you know, configure, read the diagnostics, and check the choice on data the fit did not see.

## 1. Know why the choice matters

With an estimated base frame and tool point, some joint errors are not identifiable at all (the frames absorb them), and others are identifiable but barely excited by your postures. Estimating the latter freely fits noise: the training residual drops, prediction on new postures gets worse.

On the TIAGo mocap reference (37 training postures, position only), a free `full_params` fit with 31 parameters overfits even a synthetic robot whose true errors are exactly of that form; at realistic noise it predicts new postures two to three times worse than the methods that account for excitation. See the table in the [reference page](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/#what-a-known-truth-shows).

## 2. Run the baseline first

Keep the default (`structural`) and fit once. It is the behaviour of earlier FIGAROH versions and the reference every other choice is compared with.

```
calibrator = TiagoCalibration(robot, "config/tiago_unified_config.yaml")
calibrator.initialize()
calibrator.solve(plotting=False)
print(len(calibrator.calib_config["param_name"]), "parameters")
print(calibrator.calib_config.get("absorbed_param_name"))  # dropped: frames absorb them
```

Note the number of parameters against the number of measurements (`NbSample` × measured components). Few measurements per parameter is the first sign that the method matters.

## 3. Decide what you know

| You know…                                                                             | Consider                  | Because                                                                            |
| ------------------------------------------------------------------------------------- | ------------------------- | ---------------------------------------------------------------------------------- |
| Nothing beyond the URDF                                                               | `map_cv`, `cv_subset`     | They choose the prior scale or the set size by cross-validation on your postures   |
| Typical error sizes (vendor tolerances, earlier calibrations of the same model)       | `map`, `excitation`       | They use those sizes directly; `map` for prediction, `excitation` for a short list |
| You need a short, named list of corrections (for review, a URDF diff, a runtime file) | `excitation`, `cv_subset` | They drop parameters instead of shrinking them                                     |
| You need results comparable with an earlier run                                       | `structural`              | Unchanged behaviour                                                                |

None of these is right in general. If unsure, run several (step 6).

## 4. Configure

In the calibration task of the unified config:

```
parameters:
  calibration_level: full_params
  estimation:
    method: map_cv
```

Optional keys and their defaults are listed in the [reference page](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md). The ones you are most likely to set:

- `priors` — expected error sizes per group (`translation` m, `rotation` rad, `joint_offset` rad, `prismatic_offset` m). Used by `map` and `excitation`; `map_cv` only uses their ratios.
- `noise_std` — measurement noise in the units of your cost function's residuals. Leave `null` to estimate it from the data (recommended unless you have a reason to trust a sensor specification more than your residuals).
- `cv_folds`, `cv_seed` — cross-validation over your training postures.
- `cv_rule: one_se` — with `cv_subset`, prefer the smallest set within one standard error of the best.

The same settings can be set in Python before `initialize()`:

```
calibrator.calib_config["estimation"] = {"method": "excitation", "excitation_k": 1.0}
```

## 5. Read the diagnostics

Selection happens in `initialize()`; its record is `calibrator.estimation_report` (also `calib_config["estimation_report"]`):

```
r = calibrator.estimation_report
print(r["method"], r["noise_std"], r["noise_source"])
print("not identifiable here:", r["dependent"])
for name, ratio in r.get("removal_order", []):   # excitation, cv_subset
    print(f"removed {name}: predicted SE {ratio:.1f} x expected size")
for point in r.get("cv_curve", []):              # map_cv, cv_subset
    print(point)
print(r.get("prior_scale"), r.get("n_joint_chosen"))
```

After `solve()`, for `map` and `map_cv`, compare each parameter's posterior standard deviation with its prior: close to the prior means the data did not inform it, and its value is essentially the nominal one.

```
from figaroh.calibration import estimation

names = calibrator.calib_config["param_name"]
prior = estimation.prior_std(names, calibrator.model, estimation.settings(calibrator.calib_config)["priors"])
for n, s, p in zip(names, calibrator.std_dev, prior):
    if p != float("inf"):
        print(f"{n:28s} posterior/prior {s / p:.2f}")
```

Warning signs:

- the cross-validation curve is flat, or its minimum is at the edge of `cv_multipliers` / `cv_sizes` (extend the grid);
- `cv_subset` chooses very different sizes for different `cv_seed` values (too few postures for a stable choice);
- the estimated noise is much larger than the sensor's (unmodelled effects such as backlash or deflection dominate; no method fixes that).

## 6. Compare candidates on data the fit did not see

Cross-validation inside `map_cv` and `cv_subset` uses training postures only. Judge the final choice on postures from another session (`validation_data_file`) or a held-out part of the data, with the same frames and processing.

```
results = {}
for method in ("structural", "excitation", "map_cv", "cv_subset"):
    c = TiagoCalibration(robot, config_path)
    c.calib_config["estimation"] = {"method": method}
    c.initialize()
    c.solve(plotting=False)
    results[method] = c.evaluation_metrics  # includes validation metrics when configured
```

Decide which comparison counts *before* looking at it, and look at the held-out set once. Repeatedly tuning the method on the same held-out set turns it into training data. figaroh-examples' [TIAGo held-out protocol](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/development/tiago-mocap-heldout-protocol.md) shows one way to freeze sets and rules.

## 7. Check against a known truth when you can

A synthetic truth at **your** postures answers questions real data cannot: do these postures support this many parameters, and does the method recover what it should? figaroh-examples' [TIAGo truth fixture](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/development/tiago-calibration-synthetic-truth.md) (`examples/tiago/calibration_truth.py`) draws errors of a chosen size, simulates your postures with noise, fits, and compares with the truth. Adapt it to your robot's postures and expected error sizes. It has no model mismatch, so it complements real held-out data rather than replacing it.

## 8. Export and report

- Write the same joint corrections to the URDF and to PAL's `geometric_calibration`:

```
from figaroh.tools.urdf_exporter import export_urdf

export_urdf("urdf/robot.urdf", calibrator.joint_corrections(), output_path="urdf/robot_calibrated.urdf")
```

For `structural`, `joint_corrections()` lifts the fitted base parameters onto every joint with your expected error sizes, so a joint whose correction was represented by another parameter in the fit gets its share (`joint_corrections(lift=False)` returns the representatives only). For the other methods it returns the fitted joint parameters; those left out stay at nominal. Reload the written URDF and compare its forward kinematics with the fit before deploying it.

- Frames (base, tool point) are measurement-setup quantities and are not written into the robot URDF.
- Record in your report: the method and every non-default setting, the estimated or given noise, the priors, the selection record (removal order or cross-validation curve and choice), the parameter count, and the held-out result with its evidence type (training only, temporal block, other session, synthetic truth).

## See also

- [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md) — reference
- [Calibration walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md)
- [Plan, fit and validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) — the general decision process
- [Optimal experiment design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md) — postures that excite more parameters
- [Reporting & verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md)

# Calibration Walkthrough

## The problem

Manufacturing tolerances and assembly errors give every physical robot systematic geometric errors — joint zero-offsets, link length/orientation deviations from CAD, tool-center-point misalignment — typically 5-10mm at the end-effector. That's enough to make precision tasks (contact-rich manipulation, vision-guided grasping, multi-robot coordination) unreliable.

Kinematic calibration corrects this using external measurements (motion capture, a tracked marker, a camera) of the end-effector at a set of known joint configurations.

## The model

```
P_measured = forward_kinematics(q, θ_nominal + Δθ) + ε
```

- `q` — measured joint configurations (document any use of commanded substitutes)
- `θ_nominal` — nominal kinematic parameters from the URDF
- `Δθ` — parameter corrections FIGAROH solves for
- `P_measured` — external sensor measurement of the end-effector (or another tracked frame) at each `q`
- `ε` — measurement noise

FIGAROH minimizes the residual between `P_measured` and the predicted pose over `Δθ` using Levenberg-Marquardt, then reports which corrections are well- vs. poorly-observed given your measurement set.

## Running it

Every robot in [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) provides a `BaseCalibration` subclass (e.g. `TiagoCalibration`) that supplies the robot-specific frame/marker configuration; the shape is the same for every robot:

```
from examples.tiago.utils.tiago_tools import TiagoCalibration
from figaroh.tools.robot import load_robot

robot = load_robot("path/to/robot.urdf", load_by_urdf=True)
calibrator = TiagoCalibration(robot, "config/tiago_unified_config.yaml")
calibrator.initialize()
result = calibrator.solve(plotting=False, html_report=True)
```

`solve()` always prints a terminal quality report (condition number, per-DOF residual RMSE, outlier count). `html_report=True` additionally writes a self-contained diagnostic page — see [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md).

From the command line, the TIAGo example wraps this in one script:

```
cd examples/tiago
python calibration.py                 # full pipeline: calibrate → plot → save → export → viser viz
python calibration.py --calibrate-only  # just solve, skip URDF export
python calibration.py --update-model    # export a corrected URDF from a saved result
```

## What you configure

The `calibration:` section of your [unified config](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) declares:

- `base_frame` / `tool_frame` — the kinematic chain endpoints
- `markers` — which frame(s) are measured, and which of their 6 DOF are observable (`measure: [x, y, z, roll, pitch, yaw]`) — a wrist-mounted 6-DOF tracker measures all six; a hand-mounted marker seen by a fixed camera might only give 3-DOF position
- `nb_sample` — number of configurations in your measurement set (more isn't always better — see [Optimal Experiment Design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md) for how to pick the *best* set instead of the largest)
- `calibration_level` and `estimation` — which corrections exist (`joint_offset` or `full_params`) and which of them are estimated, and how (`structural` by default; `excitation`, `map`, `map_cv`, `cv_subset`). With few postures this choice matters as much as the data: see [Choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md)

## Interpreting results

Compare nominal and calibrated prediction on unused configurations. Report translation and rotation components in explicit units; position-only or contact measurements cannot establish full pose accuracy. Analyze gauge, identifiable corrections and prior sensitivity before interpreting individual parameters.

Verification defaults are task checks, not guaranteed accuracy. Inspect which validation metrics were available, and reload any exported model to check FK against the selected calibrated stage. See [Plan, Fit and Validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) for acquisition, split design, residual interpretation and acceptance evidence.

## Next steps

- [Choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md) — pick and check the estimation method for your robot and postures.
- [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) — once kinematics are corrected, identify the *dynamic* parameters (mass, inertia, friction) for control and simulation.
- [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) — turn this run into a shareable report and a CI-gateable pass/fail check.
- [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) — complete calibration scripts for UR10, TIAGo, and TALOS.

# Identification Walkthrough

## The problem

Nominal models can omit cables, connectors, payloads, friction and assembly differences. The resulting parameter and prediction errors depend on the robot and the operating conditions. Accurate dynamic parameters matter for feedforward torque control, energy-efficient trajectory planning, collision detection, and any digital-twin/simulation use of the model.

Dynamic identification estimates observable parameter combinations from motion and effort data. Individual physical link parameters require additional constraints and are not necessarily uniquely recoverable.

## The model

The robot's equation of motion:

```
τ = M(q) q̈ + C(q, q̇) q̇ + G(q) + F(q̇)
```

is linear in the *dynamic parameters* `φ` (masses, first mass moments, inertia tensor entries, friction coefficients), so it can be rewritten as:

```
τ = W(q, q̇, q̈) · φ
```

`W` is the **regressor matrix** — computed purely from motion (`q`, `q̇`, `q̈`), independent of the unknown `φ`. Given enough logged `(q, q̇, q̈, τ)` samples from a sufficiently "exciting" trajectory, `φ` is recovered by linear least squares.

## The pipeline

1. **Partition and signal processing** — declare training/validation data, check rates and synchronization, then process each partition independently; estimate velocity/acceleration if not measured directly.
1. **Regressor construction** — build `W` from checked motion and effort data.
1. **Base parameter analysis** — analyze identifiable combinations and dataset excitation; distinguish structural dependencies from weak sample coverage.
1. **Parameter estimation** — solve the linear least-squares problem for the base parameters (FIGAROH's [solver](https://thanhndv212.github.io/figaroh-plus/api/tools/index.md) supports OLS, WLS, ridge, and several constrained/robust variants).
1. **Validation** — compare predicted vs. measured torques on held-out data.

## Running it

```
from examples.tiago.utils.tiago_tools import TiagoIdentification
from figaroh.tools.robot import load_robot

robot = load_robot("path/to/robot.urdf", load_by_urdf=True)
identifier = TiagoIdentification(robot, "config/tiago_unified_config.yaml")
identifier.initialize()
result = identifier.solve(decimate=True, html_report=True)

verdict = identifier.verify()
print("PASS" if verdict.passed else "FAIL")
```

`decimate=True` downsamples the regressor to reduce redundant, highly correlated rows before the solve — cheaper and often better-conditioned than fitting on every raw sample.

From the command line:

```
cd examples/tiago
python identification.py                          # html-report + verify on by default
python identification.py --no-html-report --no-verify
```

For a one-line version without a robot-specific subclass, see [Integration API](https://thanhndv212.github.io/figaroh-plus/concepts/integration/index.md).

## What you configure

The `identification:` section of your [unified config](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) sets `has_friction`, `has_actuator_inertia`, `has_external_forces`, signal-processing (`sampling_frequency`, `cutoff_frequency`), and joint/velocity/torque limits used to sanity-check the logged data.

## Interpreting results

Report per-joint training and held-out effort errors against the nominal model, rank/conditioning under declared scaling, and uncertainty assumptions. Default verification thresholds are configurable checks, not guarantees of percentage accuracy or full-parameter recovery. Record which checks were skipped and whether the adapter used genuine held-out data.

If reconstructing or projecting full inertias, report solver and physical verdicts separately and recompute prediction for the selected stage. The private log-Cholesky benchmark remains experimental pending convergence review. Follow [Plan, Fit and Validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) for method selection, comparison policy, parameter interpretation and export checks.

## Next steps

- [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) — the full report/verdict/compare-page suite, and how to wire `--verify` into CI.
- [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) — complete identification scripts for UR10, TIAGo, and Staubli TX40.

# Optimal Experiment Design

Both [calibration](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md) and [identification](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) need measurement data — but *which* poses to measure and *which* trajectory to execute has a huge effect on how much data you need and how good the resulting parameters are. Measurement count alone does not establish observability. FIGAROH provides optimization-based configuration and trajectory design tools; their benefit must be assessed for the selected parameter scope, constraints and sensor model. Use [Plan, Fit and Validate](https://thanhndv212.github.io/figaroh-plus/example_workflow/index.md) to plan acquisition and independent validation together.

## Optimal configuration generation (for calibration)

**Goal:** pick the smallest set of robot poses that maximizes observability of the kinematic parameters.

Uses **D-optimal design**: select configurations that maximize the determinant of the Fisher information matrix built from the kinematic regressor `R`:

```
maximize:   det(Σ w_i · Rᵢᵀ Rᵢ)^(1/n)
subject to: Σ w_i ≤ 1,  w_i ≥ 0,  kinematic feasibility
```

Solved as a Second-Order Cone Program (SOCP) over candidate weights `w_i` for a pool of feasible candidate poses. Inspect the selected set's rank and information spectrum under the chosen model. Numerical design quality does not guarantee real measurement accuracy or recovery of parameters outside the observable subspace.

```
cd examples/tiago
python optimal_config.py --end-effector hey5
# → results/tiago_optimal_configurations_hey5.yaml
```

## Optimal trajectory generation (for identification)

**Goal:** generate a single smooth motion that "excites" all the dynamic coupling effects (inertial, Coriolis, gravitational, friction) needed to observe the dynamic parameters, while respecting every physical limit.

Formulated as a constrained optimization over cubic-spline waypoints:

```
minimize:   condition_number(W_base(trajectory))
subject to: joint position/velocity/acceleration limits
            joint torque/power limits
            self-collision avoidance
            C² trajectory smoothness
```

Solved with IPOPT (interior-point). Record convergence and verify which constraints the selected implementation enforces. Compare the candidate with a feasible baseline under the same scaling and assess the acquired data's held-out prediction; improved conditioning alone is insufficient evidence.

```
cd examples/tiago
python optimal_trajectory.py
# → candidate trajectory; check actual robot/acquisition constraints before execution
```

## Why this matters

Both problems reduce to the same idea: the [regressor](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) or parameter Jacobian determines which directions the sampled model observes, under the declared measurement assumptions. A poorly chosen measurement set can remain rank-deficient despite many samples. Inspect conditioning alongside rank, scaling and the chosen design objective; D-optimal information and condition number are different criteria. The condition-number check in [`verify()`](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) is one diagnostic, not a complete experiment-quality or accuracy certificate.

## Next steps

- Run the [Calibration](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md) or [Identification](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) walkthrough using the configurations/trajectory generated here.
- [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) — `optimal_config.py` / `optimal_trajectory.py` are shipped for UR10 and TIAGo.
# AI agents

# Use FIGAROH with an AI agent

FIGAROH ships agent skills: step-by-step instructions a coding agent follows to check your environment, pick a config template, run a task and read the verdict back to you.

## 1. Set up your agent

Pick the agent you use. Each tab is a complete setup.

Clone both repositories side by side, copy the skills into your workspace, and tell the skills where the workspace is:

```
git clone https://github.com/thanhndv212/figaroh-plus figaroh
git clone https://github.com/thanhndv212/figaroh-examples
mkdir -p .claude/skills && cp -r figaroh/skills/* .claude/skills/
export FIGAROH_WS="$PWD"
```

Claude Code discovers the skills automatically. Start a session in the workspace and describe your task.

Clone both repositories as above, then add one line to your workspace's `AGENTS.md` (or the instructions file your agent reads):

```
For FIGAROH tasks, read figaroh/skills/figaroh-start/SKILL.md first.
```

`figaroh-start` routes the agent to the right task skill. Set `FIGAROH_WS` to the workspace root, as for Claude Code.

Without a coding agent, give the assistant the whole documentation as one file:

```
https://thanhndv212.github.io/figaroh-plus/llms-full.txt
```

Paste the link, or download the file and attach it. Every docs page also has a **Copy as Markdown** button at the top right.

## 2. Check the environment

Ask the agent to run the read-only doctor, or run it yourself. It names the exact fix for each failure (missing cyipopt, an import from the wrong checkout, meshes that do not load).

```
bash figaroh/skills/figaroh-setup-env/scripts/doctor.sh
```

**Done when** the doctor reports no failures and `python -c "import figaroh"` works in the environment the agent uses.

## 3. Ask for a task

Good prompts name the robot, the data, and what "done" means. Copy one and edit the paths.

All Setup Calibration Identification Design New robot

figaroh-setup-envCopy

Check that my FIGAROH environment works, fix anything the doctor reports, then run the UR10 calibration example as a smoke test.

Ends with a run that proves the fix.

figaroh-setup-calibrationCopy

Calibrate my UR10 with the mocap poses in data/poses.csv, validating on data/poses_validation.csv. Use a 1 mm position RMSE limit and tell me whether the verdict passes and which parameters changed most.

Names the robot, the data, the held-out set and the limit.

figaroh-setup-calibrationCopy

I know my arm's tolerances: about 0.5 mm on link lengths and 0.1° on joint zeros. Recalibrate with the map method using those priors and compare held-out error with the structural method.

Gives the priors the method needs and asks for a comparison.

figaroh-setup-identificationCopy

Identify the dynamics of TIAGo's arm from q and τ in data/arm_log.csv. Validate on the separate recording data/arm_log_validation.csv and report the held-out torque RMSE per joint, with and without friction.

Asks for held-out validation, not just a fit.

figaroh-setup-optimalCopy

My identification is badly conditioned. Generate an exciting trajectory for the UR10 within its joint limits and compare its condition number with my current motion.

States the problem the design should fix.

figaroh-setup-new-robotCopy

Add my robot to figaroh-examples. The URDF is in ~/robots/my_arm/urdf/my_arm.urdf and the joints I move are joint_1 to joint_6. Start with calibration.

Gives the URDF path, the joints and the first task.

## Tips

- **Say what done means.** "Passes prediction verification with a 2 N·m limit per joint" is checkable; "looks good" is not.
- **Ask for held-out validation.** A fit on training data always looks better than it is. Give the agent a separate recording.
- **Read the verdict yourself.** The agent writes `results/*_verification.json`. Check `passed` and any `not_evaluated` entries before you trust a summary.
- **Keep runs headless.** Plot windows block agents. Set `MPLBACKEND=Agg`, or pass `--no-plot` where a script supports it.

## llms.txt

The site publishes machine-readable copies for assistants and tools:

| File                                                                        | Contains                                                        |
| --------------------------------------------------------------------------- | --------------------------------------------------------------- |
| [`llms.txt`](https://thanhndv212.github.io/figaroh-plus/llms.txt)           | An index of the documentation with one link per page            |
| [`llms-full.txt`](https://thanhndv212.github.io/figaroh-plus/llms-full.txt) | Every page in one Markdown file                                 |
| `<page>/index.md`                                                           | The Markdown of a single page (the **Copy as Markdown** button) |

Next: [what each skill does](https://thanhndv212.github.io/figaroh-plus/agents/skills/index.md).

# Skills

Each skill is a Markdown file under `figaroh/skills/` that tells an agent how to do one job, which files to touch and how to know it worked.

`figaroh-start` is the entry point. It maps the two repositories and hands the task to one of the setup skills.

| Skill                          | Use it when                                                                                                               | It ends with                                                           |
| ------------------------------ | ------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------- |
| `figaroh-start`                | Any FIGAROH request. Routes to the skills below.                                                                          | The right skill loaded, with the robot folder and task named           |
| `figaroh-setup-env`            | A fresh machine, `import figaroh` fails, cyipopt or picos is missing, meshes do not load, or library edits have no effect | `doctor.sh` passes and the UR10 example runs                           |
| `figaroh-setup-calibration`    | Correcting joint offsets and frame transforms from measured poses                                                         | A calibration run, an HTML report and a verdict                        |
| `figaroh-setup-identification` | Fitting masses, inertias, friction and actuator inertia from q/τ logs                                                     | An identification run with held-out torque error per joint             |
| `figaroh-setup-optimal`        | Badly conditioned results, or before collecting data on a new robot                                                       | Designed postures or an exciting trajectory, with its condition number |
| `figaroh-setup-new-robot`      | A robot with no folder under `figaroh-examples/examples/`                                                                 | A scaffolded robot folder that `validate.py --robot <robot>` runs      |

## How routing works

```
you ──► figaroh-start ─┬─► figaroh-setup-env            (always first on a new machine)
                       ├─► figaroh-setup-calibration
                       ├─► figaroh-setup-identification
                       ├─► figaroh-setup-optimal
                       └─► figaroh-setup-new-robot ──► calibration / identification
```

The skills follow the same five steps as the [workflow guides](https://thanhndv212.github.io/figaroh-plus/workflows/index.md): Plan, Design, Configure, Fit, Verify. If the agent proposes something the guides warn against, such as validating on a split of the training file, point it at the relevant step.

## Read a skill

The skills are plain Markdown. Reading the one your agent loaded shows you what it will do:

- [`skills/figaroh-start/SKILL.md`](https://github.com/thanhndv212/figaroh-plus/tree/main/skills/figaroh-start)
- [`skills/`](https://github.com/thanhndv212/figaroh-plus/tree/main/skills) for the full set
# Concepts

# Architecture

The canonical architecture document lives at the repository root ([`ARCHITECTURE.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/ARCHITECTURE.md)) so it stays next to the source tree it describes. This page embeds it directly so the same content is browsable as part of the docs site, without a second copy to keep in sync.

## FIGAROH Architecture

**Source audit: 2026-09-28.** This document describes the current source tree. Future capabilities belong in the [roadmap](https://github.com/thanhndv212/figaroh-plus/blob/main/ROADMAP.md) and [design decisions](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/README.md). The docs site embeds this file; API details are generated from docstrings under `docs/source/api/`.

### System boundaries

FIGAROH is a Python library for dynamic identification, geometric calibration and experiment design. The installable package is `src/figaroh/`; robot-specific scripts, datasets and models live in the separate [figaroh-examples repository](https://github.com/thanhndv212/figaroh-examples).

The principal user entry points are subclasses of `BaseIdentification`, `BaseCalibration`, `BaseOptimalTrajectory` and `BaseOptimalCalibration`. `RobotIdentificationSystem` provides a smaller CSV/URDF identification wrapper. The source currently imports several subpackages eagerly, so installing only a single numerical dependency is not sufficient to import the package.

```
Robot-specific script + YAML + measurements + URDF
                     |
     Workflow classes / integration wrapper
                     |
  Regressors, solvers, robot model, result processing
                     |
     Backend-aware operations + direct Pinocchio use
                     |
       Identified/calibrated results
                     |
   Verification / reports / archives / explicit export
```

### Module responsibilities

| Module                            | Owns                                                                                                        | Reference                                                                                                                                                                                                                                                                                      |
| --------------------------------- | ----------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `calibration/`                    | Config, measured poses, kinematic residuals, fitting and validation                                         | [BaseCalibration](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/calibration/base_calibration.py)                                                                                                                                                                           |
| `identification/`                 | Signal processing, dynamic regressors, identifiable parameters, optional physical projection/reconstruction | [BaseIdentification](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/identification/base_identification.py)                                                                                                                                                                  |
| `optimal/`                        | Excitation trajectories and calibration posture selection                                                   | [trajectory workflow](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/optimal/base_optimal_trajectory.py), [configuration workflow](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/optimal/base_optimal_calibration.py)                                   |
| `backends/`                       | Common dynamics operations and Pinocchio/MuJoCo implementations                                             | [DynamicsBackend](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/backends/base.py)                                                                                                                                                                                          |
| `tools/`                          | Robot loading, QR/linear solvers, collisions, export, reports and archives                                  | [Robot](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/robot.py), [regressor](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/regressor.py), [QR](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/qrdecomposition.py) |
| `utils/`                          | Config inheritance/migration, result handling and error types                                               | [config parser](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/utils/config_parser.py), [results](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/utils/results_manager.py)                                                                               |
| `integration/`                    | Convenience wrapper around the identification workflow                                                      | [API](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/integration/api.py)                                                                                                                                                                                                    |
| `measurements/`, `visualisation/` | Measurement and viewer utilities                                                                            | Generated API reference                                                                                                                                                                                                                                                                        |

Generic algorithms stay in core; robot paths, data conventions and experiment recipes stay in examples. A change spanning both repos records the tested revision pair and expected core dependency in the examples PR.

### Backend boundary

- [PinocchioBackend](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/backends/pinocchio.py) exists and implements dynamics/kinematics operations. `Robot.backend` lazily wraps the robot's current Pinocchio model and data through `PinocchioBackend.from_model()`.
- [MuJoCoBackend](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/backends/mujoco.py) exists. Its analytical regressor delegates to a Pinocchio model loaded from the model path; its Coriolis matrix uses a finite-difference construction. A MuJoCo operation being available does not imply the complete identification/calibration workflow is portable.
- Regressor construction and several calibration helpers use a backend when provided. Direct Pinocchio model/frame/inertia operations remain, especially model mutation during calibration and visualization/collision integration.
- Genesis and IsaacSim implementations are absent. They are roadmap proposals.
- `RobotIdentificationSystem.from_mjcf()` raises `NotImplementedError`. `from_urdf(backend=...)` loads the usual Robot and stores the requested name; that name alone does not switch the robot's executing backend. Treat the convenience identification path as Pinocchio/URDF-based until selection is implemented and verified.

Backend and cross-backend tests live in [`test_backends.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/tests/unit/test_backends.py) and [`test_cross_backend.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/tests/integration/test_cross_backend.py). Optional dependency skips are reported as missing coverage.

### Identification data flow

1. Load the robot and YAML configuration. A subclass supplies trajectory data; the convenience wrapper reads position, velocity, acceleration and torque CSVs.
1. Process signals and build configurations, then construct the dynamic regressor and the nominal CAD reference torque.
1. Eliminate zero columns, optionally decimate, and use QR to obtain an identifiable parameter basis. Solve and compute fitting metrics.
1. Apply configured weighting, physical-consistency projection and reconstruction where enabled. These are distinct result stages.
1. Evaluate validation data when available, generate verification/report output, and explicitly choose the result stage to export or archive.

Important contracts:

- Joint order, units and the provenance of torque measurements belong to the data boundary. The convenience CSV loader creates sample-index timestamps; it is not a universal timestamped log-ingestion API.
- Regressor rows are joint-major. Enabled friction, actuator-inertia and offset blocks must match parameter key order; see the [identification regressions](https://github.com/thanhndv212/figaroh-plus/blob/main/tests/unit/test_identification_regressions.py).
- Pinocchio's dynamic parameter order is `[m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz]`; FIGAROH's standard ordering differs. Use the conversion helpers in [`identification/parameter.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/identification/parameter.py).
- Projection is default-off. `result["physical consistency"]` records status, feasibility information, and, after projection, `raw_parameters` and `projected_parameters`. Reconstruction has its own `result["reconstruction"]`. Callers must not equate a fitted base vector with a complete physical model.
- Unified signal-processing settings feed the filter parameters; explicit filter overrides take precedence. `qr_relative_tolerance` is optional, preserving the default rank-selection behavior when unset.

### Calibration and experiment design

Calibration fits geometric parameters through the workflow and helpers in `calibration/`. Model edits still use Pinocchio types. A held-out measurement comparison assesses fit quality; an exported-model FK consistency check assesses whether a file represents the intended calibrated model. They answer different questions and are reported separately.

Optimal trajectory generation uses the IPOPT wrapper in [`tools/robotipopt.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/robotipopt.py). Calibration posture selection lives in `BaseOptimalCalibration`. Both provide result saving; a common report format across every optimal workflow remains roadmap work. Use the `figaroh-dev` conda environment for development, including `cyipopt`. Physical projection is opt-in at runtime, although `picos` is currently a package dependency in `pyproject.toml`.

### Export and reporting

[`tools/urdf_exporter.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/urdf_exporter.py) maps parameter names to update handlers. Joint placement/offset, mass, friction, armature and elasticity handlers exist. Identified inertial parameters are written as complete per-link sets (#60): inputs follow Pinocchio `toDynamicParameters()` order, with first moments and inertia about the link-frame origin. The exporter writes the CoM origin and the CoM-frame tensor, keeping an existing inertial rotation. A link target or a moving joint (resolved to its child link) is accepted. Targets with massive links attached by fixed joints (merged by Pinocchio), partial sets and `m <= 0` are refused; physically infeasible sets are refused unless `allow_infeasible=True`. Tests check reloaded-model RNEA parity against known feasible inputs; round-trip parity shows file consistency, not hardware accuracy.

Metrology base/measurement-frame parameters are described for the caller instead of automatically applied to robot geometry. PAL runtime correction YAML uses [`geometric_calibration_export.py`](https://github.com/thanhndv212/figaroh-plus/blob/main/src/figaroh/tools/geometric_calibration_export.py).

Calibration and identification HTML reports, two-run comparison, provenance and run archives live in `tools/`; workflow verification methods attach verdicts to results. Example CLIs explicitly verify numerical execution; library verification defaults to prediction acceptance. Independent prediction acceptance requires explicit per-output error limits and separate validation. Physical/export and general acquisition-provenance certification remain separate; missing required evidence is not a successful check. Use the [reporting and verification guide](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) for user-facing behavior. Store metrics with the model/config/data provenance; report whether validation is held-out, training-only, simulated or physical.

### Configuration and extension rules

`UnifiedConfigParser` supports YAML inheritance. The workflow-specific config adapters translate unified task config into internal dictionaries; the legacy flat config was removed in 1.0 and raises an error naming the migration command. The [configuration reference](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) owns the schema.

For a new interface, dependency, result schema or module boundary, add a decision record first or in the same PR. Proposed `ResidualTerm`/`LinearRegressorTerm`, full MJCF workflows and deployment/control layers are not current API contracts. For a new backend, implement the supported operations, document unsupported ones, and add numerical comparisons before claiming workflow parity.

The [contribution workflow](https://github.com/thanhndv212/figaroh-plus/blob/main/CONTRIBUTING.md) owns branch/release policy; the [validation guide](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/development/validation.md) owns commands and evidence requirements. Keep numerical claims linked to tests or archived runs and update this overview when boundaries or result contracts change.

# Backends

FIGAROH's algorithms (regressor construction, calibration, identification) are written once against an abstract `DynamicsBackend` interface, not against a specific simulator. Swapping the simulator underneath a workflow is a config change, not a rewrite.

## Available backends

| Backend       | Formats         | Status                                 | Best for                                      |
| ------------- | --------------- | -------------------------------------- | --------------------------------------------- |
| **Pinocchio** | URDF            | Always available (core dependency)     | Research & identification, CPU                |
| **MuJoCo**    | URDF, MJCF      | Optional (`pip install mujoco`)        | Contact dynamics, optimal control, sparse ops |
| **Genesis**   | URDF, MJCF, USD | Optional (`pip install genesis-world`) | GPU, large-scale parallel/multi-robot         |
| **Isaac Sim** | USD, URDF       | Optional, requires NVIDIA Omniverse    | Sim-to-real, RL, synthetic data               |

```
from figaroh.backends import get_backend, list_backends

print(list_backends())
# {'pinocchio': True, 'mujoco': True, 'genesis': False, 'isaacsim': False}

backend = get_backend("mujoco", model_path="robot.urdf")
```

## The `DynamicsBackend` interface

Every backend implements the same set of rigid-body dynamics primitives, so identification/calibration code never branches on which simulator is active:

- `compute_mass_matrix(q)` — `M(q)` in the equation of motion
- `compute_coriolis_matrix(q, v)` — `C(q, v̇)`
- `compute_gravity_vector(q)` — `g(q)`
- `compute_forward_kinematics(q)` — per-frame position/orientation
- `compute_jacobian(q, frame)` — stacked `[linear; angular]` geometric Jacobian
- `compute_regressor(q, v, a)` — the linear-in-parameters regressor `W` such that `τ = W(q, v̇, a) · θ`; this is the core computation identification builds on
- `compute_inverse_dynamics(q, v, a)` / `compute_forward_dynamics(q, v, τ)` — RNEA / ABA
- `nq`, `nv`, `model_format` properties

A handful of methods are optional (default to `NotImplementedError`) because not every backend can implement them efficiently: `compute_dynamics_derivatives`, `get_joint_names`, `get_frame_names`, `get_inertias`, `compute_difference`/ `compute_integrate` (Lie-group operations for free-flyer bases), `random_configuration`, and `get_model_object()` — an escape hatch that returns the underlying `pin.Model` or `mj.MjModel` for advanced features not yet abstracted (e.g. `PseudoInertia`, collision models).

See [`figaroh.backends.base.DynamicsBackend`](https://thanhndv212.github.io/figaroh-plus/api/backends/index.md) for the full signatures.

## Choosing a backend

- **Research & identification** → Pinocchio or MuJoCo
- **Contact-rich tasks** → MuJoCo or Genesis
- **Large-scale / parallel** → Genesis (GPU)
- **Sim-to-real** → Isaac Sim

## Next steps

- [Integration API](https://thanhndv212.github.io/figaroh-plus/concepts/integration/index.md) — the high-level, backend-selecting entry point (`RobotIdentificationSystem`) built on top of this interface.
- [API Reference → Backends](https://thanhndv212.github.io/figaroh-plus/api/backends/index.md) — full class docs.

# Calibration estimation methods

A geometric calibration estimates joint errors together with the unknown base frame (where the measurement system sees the robot) and tool point (where the marker sits). Two questions decide the result as much as the data does:

1. **Which parameters to estimate.** Some are not identifiable at all from the measurements (they move nothing, or exactly what another parameter or a frame moves). Others are identifiable in principle but barely excited by the recorded postures: estimating them freely fits noise and extrapolates badly.
1. **How to estimate them.** Freely, or with prior knowledge of how large they are expected to be.

This page is the reference for the available methods. For the step-by-step workflow (baseline, choosing, reading the diagnostics, checking on held-out data) see the guide [Calibration: choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md); for the rationale, the design record [Selectable calibration estimation methods](https://github.com/thanhndv212/figaroh-plus/blob/devel/docs/decisions/calibration-estimation-methods.md).

FIGAROH offers several methods. None is right for every robot and dataset; this page lists what each one does, what it needs from you, and when it tends to work. The choice is yours, in the configuration.

```
tasks:
  calibration:
    parameters:
      calibration_level: full_params      # or joint_offset
      estimation:
        method: map_cv                    # structural | excitation | map | map_cv | cv_subset
        priors:                           # expected error sizes (1 sigma)
          translation: 1.0e-3             # m
          rotation: 2.0e-3                # rad
          joint_offset: 2.0e-2            # rad, revolute joint-angle offset
          prismatic_offset: 2.0e-3        # m
        noise_std: null                   # residual units; null = estimated
        excitation_k: 1.0                 # excitation only
        cv_folds: 5                       # map_cv, cv_subset
        cv_seed: 0
        cv_multipliers: [0.1, 0.3, 1.0, 3.0, 10.0]   # map_cv
        cv_per_group: false               # map_cv: one factor per group
        cv_sizes: null                    # cv_subset: sizes to try (null = all)
        cv_rule: min                      # cv_subset: min | one_se
```

The base frame and tool point are always estimated freely, with no prior. Parameter groups: `joint_offset` is the rotation about a revolute joint's own axis (`d_phiz` for a z-axis joint at `full_params`, `offsetRZ` at `joint_offset`); `prismatic_offset` the translation along a prismatic joint's axis; `rotation` and `translation` every other placement error.

## The methods

### `structural` (default)

Selects base parameters by a QR decomposition on random configurations, then drops those the estimated frames absorb on the measured postures (figaroh-plus#102). Selected parameters are estimated freely.

- **Needs:** nothing.
- **Pros:** unchanged behaviour; results comparable with earlier versions.
- **Cons:** the structural step does not see the measured marker point, so it can drop parameters the data identifies; ties between equal columns are broken by floating-point details, so the selected set can differ between platforms (figaroh-plus#113); weakly excited parameters are estimated freely.
- **Use when:** you need results comparable with an earlier run.

### `excitation`

Starts from every joint parameter of the calibration level and drops exact dependencies on the measured postures. Then, one at a time, removes the parameter whose predicted standard error is largest relative to its expected size (`priors`), while that ratio exceeds `excitation_k`. Kept parameters are estimated freely.

- **Needs:** expected sizes (defaults above), `excitation_k`, noise (given or estimated).
- **Pros:** a named, reduced parameter set that is easy to read and export; kept parameters are not biased toward nominal; deterministic.
- **Cons:** still a scale assumption (the expected sizes and `k`); a hard cut, so a parameter close to the limit can flip with small data changes; dropped parameters are set exactly to nominal even if the data says something about them.
- **Use when:** you know roughly how large the errors are and want a short, interpretable list of corrections.

### `map`

Estimates every joint parameter with a zero-mean Gaussian prior of the expected sizes. Directions the data excites well are set by the data; weakly excited ones stay near nominal. Nothing is dropped.

- **Needs:** prior sizes per group, noise (given or estimated).
- **Pros:** no hard cut, so results change smoothly with the data and do not depend on the platform; uses all the information; every parameter gets a posterior standard deviation, and comparing it with the prior shows whether the data or the prior set it.
- **Cons:** results depend on the prior sizes. Too small: real errors are shrunk toward nominal (bias). Too large: behaves like a free fit and overfits. The priors are assumptions you must state.
- **Use when:** you know the robot's tolerances (vendor data, previous calibrations of the same model).

### `map_cv`

`map` with the prior sizes multiplied by a factor chosen by k-fold cross-validation over the training postures: each fold is fitted on the others and judged on the postures it left out. With `cv_per_group`, one factor per group is chosen by coordinate search.

- **Needs:** nothing robot-specific; the default sizes only set the shape.
- **Pros:** adapts to the robot and the data; cross-validation also pushes the priors down when a parameter group only fits noise or unmodelled effects; uses the training data only.
- **Cons:** slower (one fit per fold and candidate factor); with few postures the folds are small and the choice is noisy; one more layer to explain.
- **Use when:** you have no reliable information on the robot's tolerances.

### `cv_subset`

Orders the joint parameters by the `excitation` removal order (least excited first) and chooses how many to keep by k-fold cross-validation. With `cv_rule: one_se`, it takes the smallest set within one standard error of the best.

- **Needs:** nothing robot-specific.
- **Pros:** a named, reduced set like `excitation`, sized by prediction on left-out postures rather than by a threshold.
- **Cons:** slowest (one fit per fold and size); the chosen size can change with the fold split when there are few postures; a hard cut.
- **Use when:** you want a named parameter set and have no basis for a threshold.

## Choosing

| Situation                                                                                                              | Methods to consider                                    |
| ---------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------ |
| Reproduce or compare with earlier FIGAROH results                                                                      | `structural`                                           |
| Tolerances known, want a short list of corrections                                                                     | `excitation`                                           |
| Tolerances known, want the best prediction                                                                             | `map`                                                  |
| No information on the robot                                                                                            | `map_cv`, `cv_subset`                                  |
| Few postures for many parameters (TIAGo mocap: 111 measurements for 31 free parameters already overfits a known truth) | `map`, `map_cv`, `cv_subset`; avoid free `full_params` |
| Postures designed for this model (`figaroh.optimal`)                                                                   | any; differences shrink                                |

Whatever you choose, judge the result on postures that were not used for fitting, ideally from another session. Cross-validation inside `map_cv`/`cv_subset` uses training postures only and is not a substitute. figaroh-examples has a fixture with a known truth (`docs/development/tiago-calibration-synthetic-truth.md`) and a held-out protocol (`docs/development/tiago-mocap-heldout-protocol.md`) showing how.

### What a known truth shows

figaroh-examples' TIAGo fixture with a known truth (real 37 training postures, 184 held-out postures, 5 seeds; `calibration_truth.py`) gives the held-out prediction error against the truth, in mm, at `full_params` unless noted:

| Truth class / noise     | `joint_offset` level | `structural` | `excitation` | `map`, correct priors | `map`, priors ×0.1 | `map`, priors ×10 | `map_cv` | `cv_subset` |
| ----------------------- | -------------------- | ------------ | ------------ | --------------------- | ------------------ | ----------------- | -------- | ----------- |
| `full_params` / 0.5 mm  | 1.16                 | 1.09         | 0.65         | **0.49**              | 1.21               | 0.78              | 0.56     | 0.69        |
| `full_params` / 2.0 mm  | 1.50                 | 3.23         | 1.50         | **1.21**              | 3.05               | 2.54              | 1.54     | 1.59        |
| `joint_offset` / 0.5 mm | **0.25**             | 1.17         | 0.64         | 0.47                  | 0.71               | 0.79              | 0.42     | 0.33        |
| `joint_offset` / 2.0 mm | **0.98**             | 3.45         | **0.98**     | 1.12                  | 3.44               | 2.55              | 1.32     | 1.27        |

- Every method other than `structural` improves on it, by up to 3×.
- `map` is best when its priors are right; priors wrong by 10× in either direction lose most of that.
- `map_cv` and `cv_subset` need no robot sizes and stay close to `map` with correct priors (25–45 s instead of 1–3 s on this problem).
- A simpler model wins when the truth is simple; the data-driven methods come close to it without being told.

This is one robot, one posture plan and no model mismatch (no backlash or deflection); repeat the comparison on your own setup before relying on it.

## Noise

`noise_std` is in the units of your cost function's measurement residuals: metres for positions, unless the robot's `cost_function` weights them. When it is `null`, FIGAROH fits the identifiable parameters freely and uses √(RSS / (m − n)). That estimate includes unmodelled effects (backlash, deflection), which is usually what you want: it keeps the data from looking more precise than it is.

## Outputs

- `calib_config["estimation_report"]` (also `calibrator.estimation_report`): the method, candidates, exactly dependent parameters, the noise and its source, the priors, and per method the removal order with ratios, the cross-validation curve and the chosen factor or size.
- `calibrator.std_dev`: for `map`/`map_cv`, posterior standard deviations; a value close to the prior means the data did not inform that parameter.
- `redistribute_parameters()` / `joint_corrections()` (used by the PAL export, and for the URDF via `export_urdf(nominal, calibrator.joint_corrections())`):
- `structural`: a weighted minimum-norm lift of the fitted base parameters onto every joint parameter, with the expected sizes (`priors`) as weights and base-frame and dropped rows held at 0; refined so the lifted model reproduces the fit at the measured postures (figaroh-plus#111). It does not depend on which representative the QR chose.
- other methods: the fitted joint parameters directly (candidates left out of the fit at 0).

`joint_corrections()` raises if the fit has parameters a URDF cannot carry (elastic, contact planes); `drop_unsupported=True` exports the kinematic corrections only.

- `metrology_frames()`: the fitted base frame and tool point. They describe the measurement setup, so they are not written into the URDF; applied outside a reloaded URDF they reproduce the calibrated forward kinematics, for every method and both levels (figaroh-plus#62).

## Limits

- Not supported with `include_non_geometric` (elastic parameters); use `structural`.
- The robot's `cost_function` must keep the parameter layout: base frame first, joint parameters, tool point last (as the TIAGo, UR10 and TALOS examples do). Classes with extra parameters (e.g. table-contact planes) are not supported.
- `regularization_coefficient` was removed in 1.0 (#120): it weighted metres and radians alike. A non-zero value is a `ConfigurationError`; leave it at 0; `map`/`map_cv` are the unit-consistent replacement.

# Configuration Parameter Reference

[Configuration System](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) explains the two config *formats* (unified vs. legacy) and how they're parsed. This page is the field-by-field glossary for what each parameter in the `calibration:` and `identification:` sections actually *means* — units, which pipeline step consumes it, and its unified-format equivalent where one exists.

Field names below match the **legacy flat format**, since that's what you see verbatim in an existing config file (the legacy format was removed in 1.0; these are the keys `config_migration` reads). The **Unified path** column gives the equivalent key under `tasks.calibration.*` / `tasks.identification.*` in the unified format — `unified_to_legacy_config` and `unified_to_legacy_identif_config` (both in `figaroh/src/figaroh/*/config.py`) convert one to the other, so the two formats agree on meaning even though the layout differs.

## `calibration:` section

| Field                 | Meaning                                                                                                                                                                                                                                                                                                                                                                                                                                                                 | Unified path                                                                                                                                                                            |
| --------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `calib_level`         | Kinematic-error model: `joint_offset` (one offset per joint, about or along its own axis) or `full_params` (six placement errors per joint, translation and rotation vector in the joint frame)                                                                                                                                                                                                                                                                         | `parameters.calibration_level`                                                                                                                                                          |
| `estimation`          | Which parameters to estimate and how: `method` (`structural` default, `excitation`, `map`, `map_cv`, `cv_subset`), `priors`, `noise_std`, cross-validation settings. See [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md)                                                                                                                                                                          | `parameters.estimation`                                                                                                                                                                 |
| `non_geom`            | Also identify joint elasticity (gravity-torque-driven compliance, one term per active joint) on top of the geometric model                                                                                                                                                                                                                                                                                                                                              | `parameters.include_non_geometric`                                                                                                                                                      |
| `base_frame`          | URDF frame name at the **start** of the kinematic chain                                                                                                                                                                                                                                                                                                                                                                                                                 | `kinematics.base_frame`                                                                                                                                                                 |
| `tool_frame`          | URDF frame name at the **end** of the chain (usually the gripper/tool mount)                                                                                                                                                                                                                                                                                                                                                                                            | `kinematics.tool_frame`                                                                                                                                                                 |
| `base_to_ref_frame`   | *(optional, eye-hand/camera mode only)* A frame reachable from `base_frame` via a **known** transform (e.g. a camera housing anchor)                                                                                                                                                                                                                                                                                                                                    | `eye_hand.camera_frame` (requires `eye_hand.enabled: true`)                                                                                                                             |
| `ref_frame`           | *(optional, eye-hand/camera mode only)* The frame the chain restarts from after the known `base_to_ref_frame` segment; the *unknown* camera/anchor pose being estimated sits between `ref_frame` and the true world frame                                                                                                                                                                                                                                               | `eye_hand.reference_frame` (requires `eye_hand.enabled: true`)                                                                                                                          |
| `markers[].ref_joint` | Joint the external marker is rigidly mounted near; its offset from `tool_frame` is what gets estimated (`pEEx/y/z`, `phiEEx/y/z`; `pEEx_k` ... for marker `k`). Several markers are points on the same `tool_frame`, see [Several points on the tool](#several-points-on-the-tool)                                                                                                                                                                                      | `measurements.markers[].reference_joint` — written by convention, but **not currently read back** by `_extract_marker_info` (only `measurable_dof`, from the first marker, is consumed) |
| `markers[].measure`   | 6-DOF boolean mask `[x, y, z, roll, pitch, yaw]` — which axes the external sensor (mocap/vision/contact) actually reports for that marker. Every marker uses the first marker's mask                                                                                                                                                                                                                                                                                    | `measurements.markers[].measurable_dof`                                                                                                                                                 |
| `free_flyer`          | `True` if the robot's base itself is unconstrained/floating in the model (mobile-base robots with an unknown per-sample base pose)                                                                                                                                                                                                                                                                                                                                      | `kinematics.free_flying_base`                                                                                                                                                           |
| `base_pose`           | Initial guess `[x, y, z, roll, pitch, yaw]` (m, rad) for the `base_frame` → world (or anchor → camera, in eye-hand mode) transform being estimated                                                                                                                                                                                                                                                                                                                      | `measurements.poses.base_pose`                                                                                                                                                          |
| `tip_pose`            | Initial guess `[x, y, z, roll, pitch, yaw]` (m, rad) for the `tool_frame` → marker transform being estimated                                                                                                                                                                                                                                                                                                                                                            | `measurements.poses.tool_pose`                                                                                                                                                          |
| `coeff_regularize`    | **Removed in 1.0 (#120, #185), default 0.** One L2 weight that robot cost functions applied to every joint parameter, penalising metres and radians alike. Core never applied it; a non-zero value is now a `ConfigurationError` (the migration tool writes 0 and warns). Use `estimation.method: map` instead (priors of physical size per parameter group, [estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md)) | `parameters.regularization_coefficient`                                                                                                                                                 |
| `outlier_eps`         | Position error (**meters**, norm of the measured x/y/z residuals of a sample, worst marker) above which a sample is excluded and the rest refitted, for up to `max_iterations` fits. Rotational components do not take part; with no measured position component nothing is excluded. Excluded samples and their errors are in the evaluation (`excluded_samples`, `excluded_sample_errors`) and the report                                                             | `parameters.outlier_threshold`                                                                                                                                                          |
| `random_seed`         | Seed for the random configurations that select the identifiable parameter set (default 0). The selection, hence the result, does not depend on any global random state; the seed is recorded in the run provenance                                                                                                                                                                                                                                                      | `parameters.random_seed`                                                                                                                                                                |
| `data_file`           | Path to the CSV of recorded `(joint configuration, measured marker pose)` samples                                                                                                                                                                                                                                                                                                                                                                                       | `data.source_file`                                                                                                                                                                      |
| `sample_configs_file` | Optional: path cross-referencing which planned/optimal configuration each `data_file` row corresponds to. Traceability only — not required for the fit itself                                                                                                                                                                                                                                                                                                           | `data.sample_configurations_file`                                                                                                                                                       |
| `nb_sample`           | Number of samples expected from `data_file`                                                                                                                                                                                                                                                                                                                                                                                                                             | `data.number_of_samples`                                                                                                                                                                |

Why the camera pose is a free parameter, not a small offset

Everything else in `full_params`/`joint_offset` calibration treats the URDF as *approximately* right — each joint gets a small correction around its known nominal placement. An add-on eye-in-hand/eye-to-hand camera is different: its housing-to-robot mounting pose usually isn't known precisely from CAD/assembly at all, so it can't be treated as a small perturbation. `base_to_ref_frame`/`ref_frame` handle this by inserting the camera as a genuine 6-DOF unknown *in the middle of the chain* (`base_pose` is a real initial guess, not a near-zero one) rather than folding it into the same small-offset parameters as every other joint.

That's also what makes eye-hand calibration a specialized, harder case: this unknown camera-pose block is estimated *simultaneously*, from the same measurements, as the per-joint offsets of every joint the camera observes through — "camera is off by X" and "joint N is off by Y" can produce very similar marker-pose residuals, which can leave the problem under-determined or poorly conditioned. Priors (`estimation.method: map`) and deliberately exciting poses that vary joint loading/configuration widely (`sample_configs_file`, D-optimal design a la `tiago_pro/generate_optimal_configs.py`) are the two practical levers against this. A plain single-anchor unknown base frame — `base_frame` set to `universe`/`base_footprint` with no `base_to_ref_frame`, as in TiagoPro's `tiago_pro_calibration_config.yaml` (`known_baseframe=False` in `run_calibration.py`) — doesn't have this coupling, because nothing downstream of that single anchor is *itself* another unknown sensor mount; it's one 6-DOF unknown at the root, not one inserted mid-chain and re-estimated jointly with everything past it.

### Several points on the tool

Each entry of `measurements.markers` is one measured point (or pose) on the same `tool_frame`, for example the points of one mocap rigid body. The CSV has columns `x1, y1, z1, x2, ...` per marker, and each marker `k` gets its own offset `pEEx_k, pEEy_k, pEEz_k` (and `phiEE*_k` when orientation is measured); the base frame and the joint parameters are shared. Residual rows are ordered marker, then component, then sample.

What several points do and do not give (figaroh-plus#119):

- **The same identifiable joint parameters as one point.** Free point offsets absorb any rotation of the tool frame itself, so a joint offset about the tool axis stays unidentifiable. On a TIAGo arm, four points keep exactly the joint parameters one point keeps.
- **Smaller standard errors**, about `1/sqrt(n_points)` when the points' noise is independent. Points of one rigid body are not fully independent, so expect less.
- **Not necessarily better prediction.** On the TIAGo mocap reference, a four-point fit did not predict held-out postures better than marker 1 alone (`full_params`: 3.39 against 2.80 mm on marker 1); the extra points' rows bring their own errors (reflections, rigid-body tracking, unmodelled wrist effects) into every parameter.
- **One bad point affects the whole sample.** Outlier exclusion (`outlier_eps`) uses each sample's worst point, so a misreflected point removes that sample. A missing (occluded) point, i.e. an empty cell, is refused by the loader with its CSV rows; exclude them with `del_list`.

Decide per robot and dataset: compare held-out errors per point (the validation report lists them) with a one-point fit before adopting more points.

Eye-hand / camera calibration now has a unified-format equivalent

`base_to_ref_frame`/`ref_frame` map to `tasks.calibration.eye_hand.{camera_frame,reference_frame}` (with `eye_hand.enabled: true`) — the unified schema template (`templates/base_robot_config.yaml`) already reserved this section; `unified_to_legacy_config` just didn't read it. Fixed, so eye-hand configs like `examples/tiago/config/tiago_config_hey5.yaml` are no longer stuck in the legacy format — see [Migrating from legacy to unified format](#migrating-from-legacy-to-unified-format) below. (`free_flyer`/`kinematics.free_flying_base` had a similar wrong-key bug — the converter read `parameters.free_flyer`, a key the template never defines — fixed the same way.)

## `identification:` section

### `robot_params`

| Field                                                   | Meaning                                                                                                                                                                       | Unified path                                                                                                                                                               |
| ------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `q_lim_def`, `dq_lim_def`, `ddq_lim_def`, `tau_lim_def` | Joint position/velocity/acceleration/torque limit margins                                                                                                                     | `joints.joint_limits.{position,velocity,acceleration,torque}` — **parsed but not currently consumed anywhere in the identification pipeline** (kept for schema/future use) |
| `fv`                                                    | Viscous friction coefficient per joint (N·m·s/rad)                                                                                                                            | `mechanics.friction_coefficients.viscous` — used only if `has_friction`                                                                                                    |
| `fs`                                                    | Coulomb/static friction coefficient per joint (N·m)                                                                                                                           | `mechanics.friction_coefficients.static` — used only if `has_friction`                                                                                                     |
| `Ia`                                                    | Reflected actuator (motor + gearbox) inertia per joint (kg·m²)                                                                                                                | `mechanics.actuator_inertias` — used only if `has_actuator_inertia`                                                                                                        |
| `offset`                                                | Constant joint torque offset per joint (N·m) — electrical/sensor bias term                                                                                                    | `mechanics.joint_offsets` — used only if `has_joint_offset`                                                                                                                |
| `Iam6`, `fvm6`, `fsm6`                                  | Same three quantities as `Ia`/`fv`/`fs`, but for the shared "6th motor" of a differential-coupled wrist (two motors jointly drive the last two wrist axes, e.g. Staubli TX40) | `coupling.{Iam6,fvm6,fsm6}` — used only if `has_coupled_wrist`                                                                                                             |
| `reduction_ratio`                                       | Per-joint gear reduction ratio (motor turns per joint turn); sign encodes actuation direction                                                                                 | `mechanics.reduction_ratios`                                                                                                                                               |
| `ratio_essential`                                       | Threshold (%) for the "essential parameters" selection technique (Pham et al., 1995)                                                                                          | `mechanics.ratio_essential` — **documented intent only; not currently wired into any solver in this codebase**                                                             |

### `problem_params`

| Field                     | Meaning                                                                                                                                                                                                                                                                                                                                                                                                                                      | Unified path                                     |
| ------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------ |
| `is_external_wrench`      | Include a measured force/torque sensor wrench as a regressor input                                                                                                                                                                                                                                                                                                                                                                           | `problem.include_external_forces`                |
| `is_joint_torques`        | Regress against joint torque signals (standard inverse-dynamics identification; almost always `True`)                                                                                                                                                                                                                                                                                                                                        | `problem.use_joint_torques`                      |
| `force_torque`            | Which F/T channels to use (e.g. `['Fx','Fy','Fz']` or `['All']`) — only applied if `is_external_wrench` is `True`                                                                                                                                                                                                                                                                                                                            | `problem.force_torque_sensors`                   |
| `external_wrench_offsets` | Also identify a constant bias offset on the F/T sensor reading                                                                                                                                                                                                                                                                                                                                                                               | `problem.external_wrench_offsets`                |
| `has_friction`            | Include the `fv`/`fs` columns in the regressor                                                                                                                                                                                                                                                                                                                                                                                               | `problem.model_components.friction`              |
| `has_actuator_inertia`    | Include the `Ia` column in the regressor                                                                                                                                                                                                                                                                                                                                                                                                     | `problem.model_components.actuator_inertia`      |
| `has_joint_offset`        | Include the `offset` column in the regressor                                                                                                                                                                                                                                                                                                                                                                                                 | `problem.model_components.joint_offset`          |
| `is_static_regressor`     | Include the friction + offset ("static") regressor block                                                                                                                                                                                                                                                                                                                                                                                     | *(no unified equivalent found — always derived)* |
| `is_inertia_regressor`    | Include the rigid-body inertial-parameter regressor block                                                                                                                                                                                                                                                                                                                                                                                    | *(no unified equivalent found — always derived)* |
| `has_coupled_wrist`       | Last two wrist joints share differential actuation → adds the `Iam6`/`fvm6`/`fsm6` terms and a coupling transform to the regressor                                                                                                                                                                                                                                                                                                           | `coupling.has_coupled_wrist`                     |
| `embedded_forces`         | Express the external wrench in a body-embedded (tool) frame rather than a fixed/world frame                                                                                                                                                                                                                                                                                                                                                  | *(no unified equivalent found)*                  |
| `qr_relative_tolerance`   | *(optional)* QR rank threshold as a fraction of the largest pivot. Columns whose pivot falls below it are treated as unidentifiable instead of fitting noise. Unset: only the absolute `1e-6` threshold applies. `1e-4` is a reasonable start when the log warns about an ill-conditioned base regressor                                                                                                                                     | `problem.qr_relative_tolerance`                  |
| `active_joints`           | *(optional)* Restrict identification to this explicit subset of joint names, instead of every active joint in the chain                                                                                                                                                                                                                                                                                                                      | `joints.active_joints`                           |
| `torque_fit_joints`       | *(optional)* Active joints whose measured effort enters the fit and the fit/validation statistics. The other active joints keep their kinematics in the regressor (their motion still loads the fitted joints), but their effort is neither fitted nor scored; validation lists them under `not_fitted_joints`. Use it for joints whose effort signal is not trusted (unknown force constant, heavy quantisation). Unset: every active joint | `problem.torque_fit_joints`                      |

### `processing_params`

| Field                           | Meaning                                                                                                                                                                                                                                                                                                 | Unified path                                                                                                         |
| ------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------- |
| `cut_off_frequency_butterworth` | Low-pass Butterworth filter cutoff frequency (Hz) applied to position/velocity/acceleration/torque signals before regression                                                                                                                                                                            | `signal_processing.cutoff_frequency`                                                                                 |
| `ts`                            | Sample period (**seconds**) of the recorded data                                                                                                                                                                                                                                                        | `signal_processing.sampling_frequency` *(unified format specifies the rate directly, in Hz, rather than the period)* |
| `filter_config.filter_params`   | Arguments of the median + Butterworth filter (`f_sample`, `f_butter`, `nbutter`, `med_fil`). In the unified format, `f_sample`, `f_butter` and `nbutter` default to `sampling_frequency`, `cutoff_frequency` and `filter_order` (4); an explicit `signal_processing.filter_params` entry overrides them | `signal_processing.filter_params`                                                                                    |

`ts` → `nb_samples` is a rate, not a count

Despite the name, the legacy parser computes `identif_config["nb_samples"] = int(1 / ts)` — i.e. an implied samples-per-second rate, not the number of rows in your data file. Don't be misled by the field name; it isn't used as a row count anywhere downstream.

### `tls_params` (optional — known-payload identification)

| Field               | Meaning                                                                                                                                          |
| ------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------ |
| `mass_load`         | Known mass (kg) of a test payload attached during a supplementary run, used to refine the identified inertial parameters via total-least-squares |
| `which_body_loaded` | Index of the link/body in the regressor that carries that payload                                                                                |
| `sync_joint_motion` | **Parsed by nothing** — not even read into `identif_config` by the legacy parser. Effectively decorative today.                                  |

### `trajectory_params` (optional — used by `optimal_trajectory.py`, not `identification.py`)

| Field                           | Meaning                                                                                                                                                                                                                                                                                                                                                                                              |
| ------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `n_wps`                         | Number of waypoints in the generated exciting trajectory                                                                                                                                                                                                                                                                                                                                             |
| `freq`                          | Trajectory sample frequency (Hz)                                                                                                                                                                                                                                                                                                                                                                     |
| `t_s`                           | Time window between waypoints (s)                                                                                                                                                                                                                                                                                                                                                                    |
| `soft_lim`                      | Joint-limit safety margin/discount applied during trajectory search                                                                                                                                                                                                                                                                                                                                  |
| `max_attempts`                  | Maximum attempts to find a feasible trajectory before giving up                                                                                                                                                                                                                                                                                                                                      |
| `max_iterations`                | IPOPT iteration cap per trajectory segment (default 200; unified config: `problem.max_iterations`)                                                                                                                                                                                                                                                                                                   |
| `segment_attempts`              | Solves per trajectory segment: a failed segment is retried from a new random initial guess (default 1 = no retry; unified config: `problem.segment_attempts`)                                                                                                                                                                                                                                        |
| `collision_margin`              | Minimum clearance (m) between collision pairs (default 0.01). Pairs: the geometry model's, or, when it has none, every two geometries on non-adjacent bodies (the world and the first moving body count as adjacent); `srdf` removes pairs (#143). Initial guesses and every solved segment must keep this clearance along the whole spline, checked at `collision_check_frequency` (default 200 Hz) |
| `collision_checks_per_interval` | Check points per waypoint interval in the collision constraint, ending at the waypoint (default 1 = waypoints only)                                                                                                                                                                                                                                                                                  |
| `collision_screen`              | Optional (m): exact distances only for pairs within it, the others count as the screen; faster with many mesh pairs (UR10 example: 0.02). Default none = all distances exact                                                                                                                                                                                                                         |

## Migrating from legacy to unified format

The legacy flat format was removed in 1.0 (it warned since 0.4.6). Loading a legacy config, whether via `BaseCalibration`/`BaseIdentification.load_param` or `figaroh.utils.config_parser.get_param_from_yaml`, raises a `ConfigurationError` that names the command below. The migration tool itself still reads legacy files.

`figaroh.utils.config_migration` converts a legacy config to unified format automatically, using the exact field mapping in the tables above:

```
python -m figaroh.utils.config_migration \
    --input examples/tiago/config/tiago_config_hey5.yaml \
    --output examples/tiago/config/tiago_config_hey5_unified.yaml \
    --urdf urdf/tiago_48_hey5.urdf   # optional: runs a round-trip self-check
```

- `--template {base,manipulator,humanoid}` (default `base`) picks which template the output `extends:` — `base` (extending `base_robot_config.yaml` directly) is the safest default since it doesn't risk silently inheriting a specialized template's default the legacy file didn't actually have.
- Passing `--urdf` runs the output back through the real `unified_to_legacy_config`/`unified_to_legacy_identif_config` and diffs it against the original legacy values — the same self-check mechanism that caught the `free_flyer`/`eye_hand` bugs above. A migration that reports `MISMATCHES` should not be trusted without investigating why.
- Fields with no unified-format consumer today (`is_static_regressor`, `is_inertia_regressor`, `sync_joint_motion`, the unused joint-limit fields — see the tables above) are preserved under the output's `custom:` section rather than silently dropped, even though nothing reads them back yet.
- The tool only converts the file; it doesn't rewrite the example script that loads it, or verify the migrated config still produces the same calibration/identification *results* on real data — treat that as a required follow-up before switching an example over, the same way the `calc_updated_fkm` merge was validated against real TIAGo/TIAGo Pro/TALOS/UR10 data before landing.

## Next steps

- [Configuration System](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) — format structure and template inheritance.
- [Calibration Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md) and [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) — these fields in context, end to end.

# Configuration System

FIGAROH uses a flexible YAML-based configuration system that supports both the unified format (with template inheritance). The legacy flat format was removed in 1.0; see [Legacy format](#legacy-format-removed-in-10). Every robot example in [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) ships a config under `examples/<robot>/config/`; this page explains the format those files follow.

## Unified configuration format

The modern unified format provides better organization and template inheritance:

```
# config/ur10_unified_config.yaml (abridged)
extends: "../../templates/manipulator_robot.yaml"

robot:
  name: "ur10"
  properties:
    joints:
      active_joints: ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
                      "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]

tasks:
  calibration:
    parameters:
      calibration_level: "full_params"
    kinematics:
      tool_frame: "wrist_3_link"
    measurements:
      markers:
        - reference_joint: "wrist_3_joint"
          measurable_dof: [true, true, true, true, true, true]
    data:
      source_file: "data/calibration.csv"

  identification:
    signal_processing:
      sampling_frequency: 100.0
      cutoff_frequency: 10.0
```

`extends:` takes a path relative to the config file. The child overrides only the keys it sets; everything else comes from the template chain.

## Legacy format (removed in 1.0)

Configs in the old flat layout, such as the one below, no longer load: they raise an error that names the migration command. Convert them once with

```
python -m figaroh.utils.config_migration --input legacy_config.yaml \
    --output unified_config.yaml
```

(see [Migrating from legacy to unified format](https://thanhndv212.github.io/figaroh-plus/concepts/config_parameters/#migrating-from-legacy-to-unified-format)). The migration tool still reads this layout:

```
# legacy_config.yaml
calibration:
  calib_level: full_params
  markers:
    - ref_joint: wrist_3_joint
      measure: [True, True, True, True, True, True]

identification:
  robot_params:
    - fv: [0.01, 0.02, 0.015]
      fs: [0.001, 0.002, 0.0015]
  processing_params:
    - ts: 0.0002
      cut_off_frequency_butterworth: 100.0
```

## Parsing a config

```
from figaroh.utils.config_parser import UnifiedConfigParser

# Parse any configuration format
parser = UnifiedConfigParser("config/robot_config.yaml")
config = parser.parse()

# Create task-specific configuration
calib_config = parser.create_task_config(robot, config, "calibration")
identif_config = parser.create_task_config(robot, config, "identification")
```

## Template inheritance

The unified format's real power is `extends:` — a robot config file references a shared template so it only needs to specify what's actually robot-specific. Templates and real examples live in [figaroh-examples/examples/templates](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/templates):

| Template                 | Extends                  | For                                                                                                                              |
| ------------------------ | ------------------------ | -------------------------------------------------------------------------------------------------------------------------------- |
| `base_robot_config.yaml` | — (root of the chain)    | Defines the full schema (`meta`, `robot`, `tasks`, `environment`, `custom`); every other template extends this                   |
| `manipulator_robot.yaml` | `base_robot_config.yaml` | Fixed-base serial arms (UR10, Staubli TX40, KUKA) — no mobile base, wrist-mounted 6-DOF marker default                           |
| `humanoid_robot.yaml`    | `base_robot_config.yaml` | Humanoid / mobile manipulators (TIAGo, TALOS) — coupled wrists, mobile base, head camera + F/T sensors, 1000 Hz default sampling |

A robot config points at the template that matches its kinematic shape:

```
# examples/ur10/config/ur10_unified_config.yaml
extends: "../../templates/manipulator_robot.yaml"

robot:
  name: "ur10"
  properties:
    joints:
      active_joints: ["shoulder_pan_joint", "shoulder_lift_joint", "..."]
    mechanics:
      reduction_ratios: [50.0, 50.0, 50.0, 50.0, 50.0, 50.0]
```

`UnifiedConfigParser` resolves the chain (robot config → template → `base_robot_config.yaml`) and deep-merges it:

- **Scalars** (strings, numbers, booleans): the more specific file wins.
- **Dictionaries**: merged recursively — keys only in the template are kept, keys only in the robot config are added.
- **Lists**: replaced entirely, not merged — a robot's `markers:` list replaces the template's, it doesn't append to it.

### Creating a new robot config

1. Pick the template that matches your robot: `manipulator_robot.yaml` for a fixed-base arm, `humanoid_robot.yaml` for a humanoid or mobile manipulator, `base_robot_config.yaml` directly for anything else.
1. Point a new config at it with `extends:` and fill in only what differs (joint names, limits, friction coefficients, marker configuration).
1. Set `tasks.<task>.enabled: true` for each task your robot supports.

`examples/create_example.sh <robot_name>` in figaroh-examples scaffolds this structure for you from the TIAGo layout — see the [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md) for the full inheritance rules and worked examples.

## Next steps

- [Configuration Parameter Reference](https://thanhndv212.github.io/figaroh-plus/concepts/config_parameters/index.md) — what each `calibration:`/`identification:` field actually means, its units, and which pipeline step consumes it.
- [Tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md) walk through a full calibration and identification run using these configs.
- [Examples Gallery → Config Templates](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md) has the complete override rules and best practices for template authors.

# Integration API

`figaroh.integration` wraps the [backend abstraction](https://thanhndv212.github.io/figaroh-plus/concepts/backends/index.md) and the `BaseIdentification` workflow class into a one-line, script-friendly API — for when you want a quick identification run without writing a robot-specific `BaseIdentification` subclass (see the [tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md) and [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) for the subclassing pattern the full examples use instead).

```
from figaroh.integration import RobotIdentificationSystem

# Create from URDF, defaulting to the Pinocchio backend
system = RobotIdentificationSystem.from_urdf("robot.urdf")

# Or pick a backend explicitly
system = RobotIdentificationSystem.from_urdf("robot.urdf", backend="mujoco")

# Run identification against CSV trajectory data
results = system.identify_parameters(
    config="config/robot_config.yaml",
    data_dir="data/",
)

print(f"Identified {len(results.phi_base)} base parameters")
print(f"RMS error: {results.rms_error}")
```

## `RobotIdentificationSystem`

- `from_urdf(urdf_path, backend="pinocchio", package_dirs=None, free_flyer=False, **kwargs)` — construct from a URDF, loading it through `figaroh.tools.load_robot`.
- `from_mjcf(mjcf_path, ...)` — reserved for MJCF-native workflows; not yet implemented (`BaseIdentification` currently requires a URDF-based `Robot` object; raises `NotImplementedError` until that's refactored).
- `identify_parameters(config, data_dir=None, decimate=True, decimation_factor=10, zero_tolerance=0.001, plotting=False, save_results=False, **kwargs)` — loads `q`/`dq`/`ddq`/`tau` CSVs from `data_dir` (or `data_files` in the config), builds the regressor, solves for base parameters, and returns an `IdentificationResult`.

## `IdentificationResult`

A plain dataclass: `phi_base`, `params_base`, `phi_standard` (if physical-consistency reconstruction is enabled), `rms_error`, `correlation`, `backend`, `model_path`, `config`, and `raw` (the full result dict from `BaseIdentification`, for anything not surfaced on the dataclass directly).

## When to use this vs. a full example

`RobotIdentificationSystem` is the fast path: one call, CSV-in the four required columns (`q.csv`, `dq.csv`, `ddq.csv`, `tau.csv`), a result object out. The [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md) robots (UR10, TIAGo, TALOS, Staubli TX40) instead subclass `BaseCalibration` / `BaseIdentification` directly — reach for that pattern once you need robot-specific cost functions, custom data loaders, or the full reporting/ verification suite wired into a CLI script.

See [API Reference → Integration](https://thanhndv212.github.io/figaroh-plus/api/integration/index.md) for the complete signatures.
# Examples

# Examples Gallery

Complete, runnable reference implementations live in the companion [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) repository — one folder per robot, each with its own config, data, and results. This gallery is the index into that repository: what each example demonstrates, how to run it, and where its outputs land. For the theory behind what these scripts do, read the [Tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md) first.

| Robot                                                                                      | Type                   | Demonstrates                                                         |                                                                                             |
| ------------------------------------------------------------------------------------------ | ---------------------- | -------------------------------------------------------------------- | ------------------------------------------------------------------------------------------- |
| [UR10](https://thanhndv212.github.io/figaroh-plus/examples/ur10/index.md)                  | Fixed-base manipulator | Calibration, identification, optimal config + trajectory             | [→ folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/ur10)         |
| [TIAGo](https://thanhndv212.github.io/figaroh-plus/examples/tiago/index.md)                | Mobile manipulator     | Calibration, identification, optimal config + trajectory             | [→ folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/tiago)        |
| [TALOS](https://thanhndv212.github.io/figaroh-plus/examples/talos/index.md)                | Humanoid (torso/arm)   | Calibration                                                          | [→ folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/talos)        |
| [Staubli TX40](https://thanhndv212.github.io/figaroh-plus/examples/staubli_tx40/index.md)  | Fixed-base manipulator | Identification                                                       | [→ folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/staubli_tx40) |
| [Config Templates](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md) | —                      | The `extends:` template-inheritance system every example config uses | [→ folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/templates)    |

## Installing figaroh-examples

```
git clone https://github.com/thanhndv212/figaroh-examples
cd figaroh-examples
pip install figaroh
pip install -r requirements.txt
```

Each script assumes you run it from inside its robot folder:

```
cd examples/ur10
python calibration.py
```

## Common layout

Every robot folder follows the same shape:

```
{robot}/
  calibration.py            # kinematic calibration (if present)
  identification.py         # dynamic identification (if present)
  optimal_config.py         # optimal measurement configurations (if present)
  optimal_trajectory.py     # exciting trajectories for identification (if present)
  config/                   # YAML configs (unified format, extends a template)
  data/                     # CSV logs / measurement data
  urdf/                     # robot URDF(s)
  utils/                    # robot-specific BaseCalibration/BaseIdentification subclass
  results/                  # generated HTML reports, verification JSON, plots
```

## Creating a new example

Scaffold a new robot folder from the TIAGo template:

```
cd examples
./create_example.sh <robot_name>
```

The generated scripts are placeholders pointing back at the TIAGo example as the reference implementation — fill in the robot-specific class in `utils/`, then point its config at the matching [template](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md) (`manipulator_robot.yaml` or `humanoid_robot.yaml`).

# Staubli TX40

Fixed-base manipulator — dynamic parameter identification only (no calibration or optimal-design scripts in this example). [→ source folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/staubli_tx40)

## What's included

- `identification.py` — loads the TX40 URDF, initializes identification, runs the solve step
- `utils/staubli_tx40_tools.py` — `TX40Identification` (active joints, filtering/decimation choices, reporting)
- `config/staubli_tx40_unified_config.yaml` — extends [`manipulator_robot.yaml`](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md)
- `data/pos_read_data.csv`, `data/curr_data.csv` — input logs

## Run

```
cd examples/staubli_tx40
python identification.py
```

`--verify` and `--html-report` are on by default — a plain run already prints a terminal quality report, writes a self-contained HTML diagnostic report with an interactive before/after chart, and writes a machine-readable pass/fail verdict.

Plotting and result-saving are controlled by arguments passed to `TX40Identification.solve(...)` inside `identification.py` (e.g. `plotting=True`, `save_results=False`, `wls=True`) rather than CLI flags — edit the script directly to change them.

## See also

- [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md)
- Full README: [figaroh-examples/examples/staubli_tx40/README.md](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/staubli_tx40/README.md)

# TALOS

Humanoid robot (PAL Robotics TALOS) — kinematic calibration of the torso/arm chain. [→ source folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/talos)

## What's included

- `calibration_upperbody.py` — all-in-one entry point: calibration, URDF export, and visual validation, selected by flag
- `update_model.py` — thin shim delegating to `calibration_upperbody.py --update-model`
- `utils/talos_tools.py` — `TALOSCalibration`, a `figaroh.calibration.base_calibration.BaseCalibration` specialization for TALOS's torso/arm kinematic chain
- `config/talos_unified_config.yaml` — extends [`humanoid_robot.yaml`](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md)

## Run

```
cd examples/talos

python calibration_upperbody.py                # full pipeline
python calibration_upperbody.py --calibrate-only
python calibration_upperbody.py --update-model
python calibration_upperbody.py --viz-validation
python calibration_upperbody.py --interactive
```

## Outputs

`results/calibration_report.html`; timestamped calibration results under `data/calibration_results_*.npz` and modified URDFs under `urdf/`.

## See also

- [Calibration Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md)
- Full README: [figaroh-examples/examples/talos/README.md](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/talos/README.md)

# Config Templates

Reusable YAML starting points for robot configs — every example in this gallery extends one of these instead of writing its config from scratch. [→ source folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/templates)

See [Configuration System](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) for how `extends:` resolution and merging work; this page covers the templates themselves and how to author a new robot config against them.

## Available templates

- **`base_robot_config.yaml`** — root of the inheritance chain. Defines the full schema (`meta`, `robot.properties`, `tasks`, `environment`, `custom`) with every task type present as a documented stub.
- **`manipulator_robot.yaml`** (extends the base) — fixed-base serial arms. `robot.type: "manipulator"`, no mobile base, calibration + identification enabled by default, wrist-mounted 6-DOF marker default. Used by UR10, Staubli TX40.
- **`humanoid_robot.yaml`** (extends the base) — humanoid / mobile manipulators. `robot.type: "humanoid"`, coupled wrists, mobile base (differential drive), head camera + F/T sensors, 1000 Hz default sampling, eye-hand calibration section, hand-mounted 3-DOF (position-only) marker default. Used by TIAGo, TALOS.

## Writing a new robot config

```
# examples/my_robot/config/my_robot_unified_config.yaml
extends: "../../templates/manipulator_robot.yaml"

robot:
  name: "my_robot"
  properties:
    joints:
      active_joints: ["joint_1", "joint_2", "..."]
  # override only what differs from the template
```

1. Pick `manipulator_robot.yaml` or `humanoid_robot.yaml` (or `base_robot_config.yaml` directly if neither fits).
1. Set `tasks.<task>.enabled: true` for each task your robot supports.
1. Fill in joint names, limits, friction, marker configuration — everything else inherits the template default.

## Best practices

- Keep robot specifics under `robot.properties.*`; task specifics under `tasks.<task>.*`; environment/execution settings under `environment.*`.
- **Don't** put `extends:` inside a template — templates are the root of the chain, not participants in it.
- **Don't** put robot-specific joint names/limits in a template — that defeats the point of sharing it.
- **Don't** delete a template key from your robot config to "remove" it — override it or leave the default.
- Adding a new default parameter to a template is safe (existing configs just inherit it); removing or renaming one breaks every config that extends it.

## Verifying a config parses

```
from figaroh.utils.config_parser import UnifiedConfigParser
parser = UnifiedConfigParser("config/my_robot_unified_config.yaml")
config = parser.parse()
print(config["robot"]["name"])
```

## See also

- [Configuration System](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) — the conceptual model (the unified format, `extends:` merge rules).
- `examples/create_example.sh` — scaffolds a new robot folder (config included) from the TIAGo layout.
- Real-world configs extending these templates: [`ur10_unified_config.yaml`](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/ur10/config/ur10_unified_config.yaml), [`tiago_unified_config.yaml`](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/tiago/config/tiago_unified_config.yaml), [`staubli_tx40_unified_config.yaml`](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/staubli_tx40/config/staubli_tx40_unified_config.yaml)

# TIAGo

Mobile manipulator (PAL Robotics TIAGo). Same four workflows as [UR10](https://thanhndv212.github.io/figaroh-plus/examples/ur10/index.md), adapted for a coupled-wrist end-effector, a mobile base, and (optionally) camera-based eye-hand calibration. [→ source folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/tiago)

## What's included

- `calibration.py` — kinematic calibration (full pipeline flags identical to UR10's)
- `identification.py` — dynamic parameter identification
- `optimal_config.py` — D-optimal calibration configuration generation (supports both Hey5 and Schunk end-effectors via `--end-effector`)
- `optimal_trajectory.py` — exciting trajectory generation (uses IPOPT)
- `utils/tiago_tools.py` — `TiagoCalibration` / `TiagoIdentification` / `TiagoOptimalCalibration` / `TiagoOptimalTrajectory`
- `utils/simplified_collision_model.py` — collision geometry for self-collision constraints during trajectory optimization
- `config/tiago_unified_config.yaml` — extends [`humanoid_robot.yaml`](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md) (default); legacy `tiago_config.yaml` / `tiago_config_hey5.yaml` also present under `config/archive/`

This is the example new robots are scaffolded from (`examples/create_example.sh`) — the reference to copy when adding support for a new robot.

## Run

```
cd examples/tiago

# 1. Optimal calibration configurations
python optimal_config.py --end-effector hey5

# 2. Kinematic calibration (full pipeline: calibrate → plot → save → export → viser viz)
python calibration.py
python calibration.py --viz-validation --model urdf/tiago_48_schunk_modified_20260127.urdf

# 3. Optimal identification trajectory (requires IPOPT)
python optimal_trajectory.py

# 4. Dynamic identification (html-report + verify on by default)
python identification.py
```

All saved files are timestamped: `data/calibration/calibration_results_{ts}.npz`, `urdf/tiago_48_schunk_modified_{ts}.urdf`.

## Outputs

Archived under `results/runs/tiago-<asset>/{calibration,identification}/<timestamp>/` by default (or `results/` with `--no-archive`) — HTML report, verification JSON, and (for calibration) a `compare.html` for offline before/after comparison. See [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md).

## See also

- [Calibration Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md) / [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md)
- [Choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md) — estimation methods, with evidence from the TIAGo truth fixture
- TIAGo calibration evidence in figaroh-examples: [held-out protocol](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/development/tiago-mocap-heldout-protocol.md) and [synthetic truth fixture](https://github.com/thanhndv212/figaroh-examples/blob/main/docs/development/tiago-calibration-synthetic-truth.md)
- Full README with class hierarchy, troubleshooting, and expected accuracy: [figaroh-examples/examples/tiago/README.md](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/tiago/README.md)

# UR10

Fixed-base 6-DOF manipulator. The most complete example in the gallery — all four workflows are implemented end to end. [→ source folder](https://github.com/thanhndv212/figaroh-examples/tree/main/examples/ur10)

## What's included

- `calibration.py` — kinematic calibration, combined with URDF export and viser visual validation behind flags
- `identification.py` — dynamic parameter identification
- `optimal_config.py` — D-optimal calibration configuration generation
- `optimal_trajectory.py` — exciting trajectory generation for identification
- `utils/ur10_tools.py` — `UR10Calibration` / `UR10Identification`, the `BaseCalibration`/`BaseIdentification` subclasses
- `config/ur10_unified_config.yaml` — extends [`manipulator_robot.yaml`](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md)

## Run

```
cd examples/ur10

# Kinematic calibration (full pipeline: calibrate → plot → save → export → viser viz)
python calibration.py
python calibration.py --calibrate-only      # solve only, skip URDF export
python calibration.py --update-model        # export URDF from a saved result

# Dynamic identification (html-report + verify on by default)
python identification.py
python identification.py --config config/UR10-007.yaml --verify --html-report

# Optimal experiment design
python optimal_config.py -n 20              # → results/ur10_optimal_configurations.yaml
python optimal_trajectory.py                # → results/ur10_optimal_trajectory.{yaml,csv}
```

All saved files are timestamped: `data/calibration/calibration_results_{ts}.npz`, `urdf/ur10_robot_modified_{ts}.urdf`.

## Outputs

`results/` contains `calibration_report.html`, `identification_report.html`, `identification_verification.json`, plus torque-comparison plots (`torque_compare_per_joint.png`). See [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) for what these contain.

## See also

- [Calibration Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_walkthrough/index.md) / [Identification Walkthrough](https://thanhndv212.github.io/figaroh-plus/tutorials/identification_walkthrough/index.md) for the theory behind these scripts.
- Full README with problem statement, math, and expected accuracy figures: [figaroh-examples/examples/ur10/README.md](https://github.com/thanhndv212/figaroh-examples/blob/main/examples/ur10/README.md)
# API reference

# API Reference

Generated from docstrings in `src/figaroh/` via [mkdocstrings](https://mkdocstrings.github.io/). One page per top-level subpackage:

| Module                                                                                   | Contains                                                                                                                                                                        |
| ---------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| [Calibration](https://thanhndv212.github.io/figaroh-plus/api/calibration/index.md)       | `BaseCalibration`, calibration tools, config, parameter, data loading                                                                                                           |
| [Identification](https://thanhndv212.github.io/figaroh-plus/api/identification/index.md) | `BaseIdentification`, identification tools, config, parameter                                                                                                                   |
| [Optimal](https://thanhndv212.github.io/figaroh-plus/api/optimal/index.md)               | `BaseOptimalCalibration`, `BaseOptimalTrajectory`                                                                                                                               |
| [Backends](https://thanhndv212.github.io/figaroh-plus/api/backends/index.md)             | `DynamicsBackend` interface + Pinocchio/MuJoCo implementations                                                                                                                  |
| [Integration](https://thanhndv212.github.io/figaroh-plus/api/integration/index.md)       | `RobotIdentificationSystem`, the one-line workflow API                                                                                                                          |
| [Data contract](https://thanhndv212.github.io/figaroh-plus/api/data/index.md)            | `TrajectoryData`, `PoseObservations`, `DataSource`, `Protocol` (#55)                                                                                                            |
| [Measurements](https://thanhndv212.github.io/figaroh-plus/api/measurements/index.md)     | Measurement data structures                                                                                                                                                     |
| [Tools](https://thanhndv212.github.io/figaroh-plus/api/tools/index.md)                   | Reporting/verification, provenance & run archiving, linear solver, robot management, regressor builder, visualization, collisions, QR decomposition, IPOPT wrapper, URDF export |
| [Utils](https://thanhndv212.github.io/figaroh-plus/api/utils/index.md)                   | Config parser, cubic spline, results manager, error handling, legacy-to-unified config migration                                                                                |
| [Visualisation](https://thanhndv212.github.io/figaroh-plus/api/visualisation/index.md)   | Plotting and viser-based visualization helpers                                                                                                                                  |

The supported names, experimental areas and the deprecation policy are on [Public API and stability](https://thanhndv212.github.io/figaroh-plus/api/stability/index.md).

New to the library? Start with [Concepts](https://thanhndv212.github.io/figaroh-plus/concepts/architecture/index.md) for the architectural picture these modules fit into, or the [Tutorials](https://thanhndv212.github.io/figaroh-plus/tutorials/index.md) for a task-driven walkthrough.

# Backends

Pluggable dynamics computation backends. See [Concepts → Backends](https://thanhndv212.github.io/figaroh-plus/concepts/backends/index.md) for the design overview and a guide to choosing one.

## base

FIGAROH Dynamics Backend System

Abstract base class for pluggable dynamics computation backends. This enables FIGAROH to work with multiple simulators (Pinocchio, MuJoCo, Genesis, Isaac Sim) while maintaining consistent algorithms and APIs.

## `DynamicsBackend(model_path, **kwargs)`

Bases: `ABC`

Abstract base class for dynamics computation backends.

This interface defines the minimum set of dynamics computations required for FIGAROH's calibration and identification algorithms. Implementations wrap specific simulators (Pinocchio, MuJoCo, Genesis, Isaac Sim).

Design Philosophy:

- Abstract interface, concrete implementations
- Consistent algorithm behavior across backends
- Performance optimization in implementations
- No algorithm changes required when switching backends

Initialize dynamics backend with robot model.

Parameters:

| Name         | Type  | Description                                          | Default    |
| ------------ | ----- | ---------------------------------------------------- | ---------- |
| `model_path` | `str` | Path to robot model file (format depends on backend) | *required* |
| `**kwargs`   |       | Backend-specific configuration options               | `{}`       |

Source code in `src/figaroh/backends/base.py`

```
def __init__(self, model_path: str, **kwargs):
    """
    Initialize dynamics backend with robot model.

    Args:
        model_path: Path to robot model file (format depends on backend)
        **kwargs: Backend-specific configuration options
    """
    self._model_path = model_path
    self._config = kwargs
```

### `nq`

Number of position variables (configuration space dimension).

### `nv`

Number of velocity variables (tangent space dimension).

### `model_format`

Backend model format.

Returns:

| Type  | Description                              |
| ----- | ---------------------------------------- |
| `str` | Format name: 'urdf', 'mjcf', 'usd', etc. |

### `model_path`

Path to robot model file.

### `config`

Backend configuration options.

### `compute_mass_matrix(q)`

Compute mass/inertia matrix M(q).

The mass matrix appears in the equation of motion

M(q) * qdd + C(q, qd) * qd + g(q) = tau

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description                                        |
| ---- | --------- | -------------------------------------------------- |
| `M`  | `ndarray` | Mass matrix [nv x nv], symmetric positive definite |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_mass_matrix(self, q: np.ndarray) -> np.ndarray:
    """
    Compute mass/inertia matrix M(q).

    The mass matrix appears in the equation of motion:
        M(q) * qdd + C(q, qd) * qd + g(q) = tau

    Args:
        q: Joint positions [nq]

    Returns:
        M: Mass matrix [nv x nv], symmetric positive definite
    """
    pass
```

### `compute_coriolis_matrix(q, v)`

Compute Coriolis and centrifugal effects matrix C(q, qd).

Note: Some simulators compute C such that C(q, qd) * qd represents Coriolis forces, while others use different conventions.

Parameters:

| Name | Type      | Description           | Default    |
| ---- | --------- | --------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq]  | *required* |
| `v`  | `ndarray` | Joint velocities [nv] | *required* |

Returns:

| Name | Type      | Description               |
| ---- | --------- | ------------------------- |
| `C`  | `ndarray` | Coriolis matrix [nv x nv] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_coriolis_matrix(self, q: np.ndarray, v: np.ndarray) -> np.ndarray:
    """
    Compute Coriolis and centrifugal effects matrix C(q, qd).

    Note: Some simulators compute C such that C(q, qd) * qd represents
    Coriolis forces, while others use different conventions.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]

    Returns:
        C: Coriolis matrix [nv x nv]
    """
    pass
```

### `compute_gravity_vector(q)`

Compute gravity effects vector g(q).

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description         |
| ---- | --------- | ------------------- |
| `g`  | `ndarray` | Gravity vector [nv] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_gravity_vector(self, q: np.ndarray) -> np.ndarray:
    """
    Compute gravity effects vector g(q).

    Args:
        q: Joint positions [nq]

    Returns:
        g: Gravity vector [nv]
    """
    pass
```

### `compute_forward_kinematics(q)`

Compute forward kinematics for all frames.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Type             | Description                                                                                                                                    |
| ---------------- | ---------------------------------------------------------------------------------------------------------------------------------------------- |
| `Dict[str, Any]` | Dictionary mapping frame names to transformations:                                                                                             |
| `Dict[str, Any]` | { 'frame_name': { 'position': np.ndarray [3], 'orientation': np.ndarray [3, 3] or [4] (quat), 'transformation': np.ndarray [4, 4] (optional) } |
| `Dict[str, Any]` | }                                                                                                                                              |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_forward_kinematics(self, q: np.ndarray) -> Dict[str, Any]:
    """
    Compute forward kinematics for all frames.

    Args:
        q: Joint positions [nq]

    Returns:
        Dictionary mapping frame names to transformations:
        {
            'frame_name': {
                'position': np.ndarray [3],
                'orientation': np.ndarray [3, 3] or [4] (quat),
                'transformation': np.ndarray [4, 4] (optional)
            }
        }
    """
    pass
```

### `compute_jacobian(q, frame)`

Compute geometric Jacobian for a specific frame.

Parameters:

| Name    | Type      | Description          | Default    |
| ------- | --------- | -------------------- | ---------- |
| `q`     | `ndarray` | Joint positions [nq] | *required* |
| `frame` | `str`     | Name of the frame    | *required* |

Returns:

| Name | Type      | Description                                                                |
| ---- | --------- | -------------------------------------------------------------------------- |
| `J`  | `ndarray` | Geometric Jacobian [6 x nv] Stacked as [linear_velocity; angular_velocity] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_jacobian(self, q: np.ndarray, frame: str) -> np.ndarray:
    """
    Compute geometric Jacobian for a specific frame.

    Args:
        q: Joint positions [nq]
        frame: Name of the frame

    Returns:
        J: Geometric Jacobian [6 x nv]
           Stacked as [linear_velocity; angular_velocity]
    """
    pass
```

### `compute_regressor(q, v, a)`

Compute observation regressor matrix W(q, v, a).

The regressor satisfies: tau = W(q, v, a) * theta where theta is the parameter vector.

This is the core computation for linear-in-parameters identification.

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name | Type      | Description                      |
| ---- | --------- | -------------------------------- |
| `W`  | `ndarray` | Regressor matrix [nv x n_params] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_regressor(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute observation regressor matrix W(q, v, a).

    The regressor satisfies: tau = W(q, v, a) * theta
    where theta is the parameter vector.

    This is the core computation for linear-in-parameters identification.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        W: Regressor matrix [nv x n_params]
    """
    pass
```

### `compute_inverse_dynamics(q, v, a)`

Compute inverse dynamics (RNEA).

Given q, qd, qdd, compute required torques tau.

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name  | Type      | Description        |
| ----- | --------- | ------------------ |
| `tau` | `ndarray` | Joint torques [nv] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_inverse_dynamics(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute inverse dynamics (RNEA).

    Given q, qd, qdd, compute required torques tau.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        tau: Joint torques [nv]
    """
    pass
```

### `compute_forward_dynamics(q, v, tau)`

Compute forward dynamics (ABA).

Given q, qd, tau, compute resulting accelerations qdd.

Parameters:

| Name  | Type      | Description           | Default    |
| ----- | --------- | --------------------- | ---------- |
| `q`   | `ndarray` | Joint positions [nq]  | *required* |
| `v`   | `ndarray` | Joint velocities [nv] | *required* |
| `tau` | `ndarray` | Joint torques [nv]    | *required* |

Returns:

| Name | Type      | Description              |
| ---- | --------- | ------------------------ |
| `a`  | `ndarray` | Joint accelerations [nv] |

Source code in `src/figaroh/backends/base.py`

```
@abstractmethod
def compute_forward_dynamics(
    self, q: np.ndarray, v: np.ndarray, tau: np.ndarray
) -> np.ndarray:
    """
    Compute forward dynamics (ABA).

    Given q, qd, tau, compute resulting accelerations qdd.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        tau: Joint torques [nv]

    Returns:
        a: Joint accelerations [nv]
    """
    pass
```

### `compute_dynamics_derivatives(q, v)`

Compute derivatives of dynamics (optional, for advanced algorithms).

Parameters:

| Name | Type      | Description           | Default    |
| ---- | --------- | --------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq]  | *required* |
| `v`  | `ndarray` | Joint velocities [nv] | *required* |

Returns:

| Type                 | Description                                                                                                                                                                                         |
| -------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `Dict[str, ndarray]` | Dictionary with derivatives:                                                                                                                                                                        |
| `Dict[str, ndarray]` | { 'dM_dq': [nv x nv x nq], # Jacobian of M w.r.t. q 'dC_dq': [nv x nv x nq], # Jacobian of C w.r.t. q 'dC_dv': [nv x nv x nv], # Jacobian of C w.r.t. v 'dg_dq': [nv x nq] # Jacobian of g w.r.t. q |
| `Dict[str, ndarray]` | }                                                                                                                                                                                                   |

Source code in `src/figaroh/backends/base.py`

```
def compute_dynamics_derivatives(
    self, q: np.ndarray, v: np.ndarray
) -> Dict[str, np.ndarray]:
    """
    Compute derivatives of dynamics (optional, for advanced algorithms).

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]

    Returns:
        Dictionary with derivatives:
        {
            'dM_dq': [nv x nv x nq],  # Jacobian of M w.r.t. q
            'dC_dq': [nv x nv x nq],  # Jacobian of C w.r.t. q
            'dC_dv': [nv x nv x nv],  # Jacobian of C w.r.t. v
            'dg_dq': [nv x nq]        # Jacobian of g w.r.t. q
        }
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement dynamics derivatives"
    )
```

### `get_joint_names()`

Get list of joint names.

Returns:

| Type   | Description                  |
| ------ | ---------------------------- |
| `list` | List of joint names in order |

Source code in `src/figaroh/backends/base.py`

```
def get_joint_names(self) -> list:
    """
    Get list of joint names.

    Returns:
        List of joint names in order
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement get_joint_names"
    )
```

### `get_frame_names()`

Get list of frame names.

Returns:

| Type   | Description                                   |
| ------ | --------------------------------------------- |
| `list` | List of frame names available for FK/Jacobian |

Source code in `src/figaroh/backends/base.py`

```
def get_frame_names(self) -> list:
    """
    Get list of frame names.

    Returns:
        List of frame names available for FK/Jacobian
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement get_frame_names"
    )
```

### `get_inertias()`

Get per-body inertia objects.

Used for nonzero-inertia filtering in regressor computation.

Returns:

| Type   | Description                                      |
| ------ | ------------------------------------------------ |
| `list` | List of inertia objects (backend-specific type). |

Source code in `src/figaroh/backends/base.py`

```
def get_inertias(self) -> list:
    """
    Get per-body inertia objects.

    Used for nonzero-inertia filtering in regressor computation.

    Returns:
        List of inertia objects (backend-specific type).
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement get_inertias"
    )
```

### `get_frame_id(frame)`

Get frame ID by name.

Used for frame-based Jacobian/calibration computations.

Parameters:

| Name    | Type  | Description | Default    |
| ------- | ----- | ----------- | ---------- |
| `frame` | `str` | Frame name  | *required* |

Returns:

| Type  | Description        |
| ----- | ------------------ |
| `int` | Frame ID (integer) |

Source code in `src/figaroh/backends/base.py`

```
def get_frame_id(self, frame: str) -> int:
    """
    Get frame ID by name.

    Used for frame-based Jacobian/calibration computations.

    Args:
        frame: Frame name

    Returns:
        Frame ID (integer)
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement get_frame_id"
    )
```

### `compute_difference(q1, q2)`

Compute Lie group difference between two configurations (q2 ⊖ q1).

For revolute joints this is simple subtraction; for free-flyer it's SE3 log.

Parameters:

| Name | Type      | Description               | Default    |
| ---- | --------- | ------------------------- | ---------- |
| `q1` | `ndarray` | First configuration [nq]  | *required* |
| `q2` | `ndarray` | Second configuration [nq] | *required* |

Returns:

| Type      | Description            |
| --------- | ---------------------- |
| `ndarray` | Difference vector [nv] |

Source code in `src/figaroh/backends/base.py`

```
def compute_difference(self, q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    """
    Compute Lie group difference between two configurations (q2 ⊖ q1).

    For revolute joints this is simple subtraction; for free-flyer it's SE3 log.

    Args:
        q1: First configuration [nq]
        q2: Second configuration [nq]

    Returns:
        Difference vector [nv]
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement compute_difference"
    )
```

### `compute_integrate(q, v)`

Integrate configuration by velocity (q ⊕ v).

Parameters:

| Name | Type      | Description        | Default    |
| ---- | --------- | ------------------ | ---------- |
| `q`  | `ndarray` | Configuration [nq] | *required* |
| `v`  | `ndarray` | Velocity [nv]      | *required* |

Returns:

| Type      | Description            |
| --------- | ---------------------- |
| `ndarray` | New configuration [nq] |

Source code in `src/figaroh/backends/base.py`

```
def compute_integrate(self, q: np.ndarray, v: np.ndarray) -> np.ndarray:
    """
    Integrate configuration by velocity (q ⊕ v).

    Args:
        q: Configuration [nq]
        v: Velocity [nv]

    Returns:
        New configuration [nq]
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement compute_integrate"
    )
```

### `random_configuration()`

Generate a random configuration within joint limits.

Returns:

| Type      | Description               |
| --------- | ------------------------- |
| `ndarray` | Random configuration [nq] |

Source code in `src/figaroh/backends/base.py`

```
def random_configuration(self) -> np.ndarray:
    """
    Generate a random configuration within joint limits.

    Returns:
        Random configuration [nq]
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement random_configuration"
    )
```

### `get_model_object()`

Escape hatch: return the underlying simulator model object.

For Pinocchio this is the pin.Model; for MuJoCo it's mj.MjModel. Used by advanced features (PseudoInertia, collision model, etc.) that need backend-specific types not yet abstracted.

Returns:

| Type  | Description                   |
| ----- | ----------------------------- |
| `Any` | Backend-specific model object |

Source code in `src/figaroh/backends/base.py`

```
def get_model_object(self) -> Any:
    """
    Escape hatch: return the underlying simulator model object.

    For Pinocchio this is the pin.Model; for MuJoCo it's mj.MjModel.
    Used by advanced features (PseudoInertia, collision model, etc.) that need
    backend-specific types not yet abstracted.

    Returns:
        Backend-specific model object
    """
    raise NotImplementedError(
        f"{self.__class__.__name__} does not implement get_model_object"
    )
```

### `__enter__()`

Enter context manager (for resource management).

Source code in `src/figaroh/backends/base.py`

```
def __enter__(self):
    """Enter context manager (for resource management)."""
    return self
```

### `__exit__(exc_type, exc_val, exc_tb)`

Exit context manager (for cleanup).

Source code in `src/figaroh/backends/base.py`

```
def __exit__(self, exc_type, exc_val, exc_tb):
    """Exit context manager (for cleanup)."""
    pass
```

## pinocchio

Pinocchio Dynamics Backend for FIGAROH

Default backend using Pinocchio's rigid body dynamics library. Wraps existing Pinocchio usage into the DynamicsBackend interface.

Features:

- Excellent URDF support
- CPU-optimized dynamics algorithms
- Full frame and Jacobian support
- Lie group operations (difference, integrate)

## `PinocchioBackend(model_path, **kwargs)`

Bases: `DynamicsBackend`

Pinocchio dynamics backend for FIGAROH (default).

This backend leverages Pinocchio's rigid body dynamics algorithms for efficient computation of mass matrices, Coriolis effects, gravity, forward kinematics, Jacobians, and the regressor matrix.

Pinocchio is the primary dependency of FIGAROH and provides the most complete URDF support.

Example

> > > backend = PinocchioBackend(model_path="robot.urdf") M = backend.compute_mass_matrix(q)

Initialize Pinocchio backend.

Parameters:

| Name         | Type  | Description                                                                                                                                                                                                                                            | Default    |
| ------------ | ----- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ---------- |
| `model_path` | `str` | Path to URDF file                                                                                                                                                                                                                                      | *required* |
| `**kwargs`   |       | Additional configuration - free_flyer: Enable free-flyer root joint (default: False) - isFext: Alias for free_flyer (default: False) - package_dirs: Directories for mesh resolution (default: None) - verbose: Enable verbose output (default: False) | `{}`       |

Source code in `src/figaroh/backends/pinocchio.py`

```
def __init__(self, model_path: str, **kwargs):
    """
    Initialize Pinocchio backend.

    Args:
        model_path: Path to URDF file
        **kwargs: Additional configuration
            - free_flyer: Enable free-flyer root joint (default: False)
            - isFext: Alias for free_flyer (default: False)
            - package_dirs: Directories for mesh resolution (default: None)
            - verbose: Enable verbose output (default: False)
    """
    if not PINOCCHIO_AVAILABLE:
        raise ImportError(
            "Pinocchio is not installed. Install with: pip install pin"
        )

    super().__init__(model_path, **kwargs)

    # Optional free-flyer root joint
    root_joint = None
    if kwargs.get("free_flyer", False) or kwargs.get("isFext", False):
        root_joint = pin.JointModelFreeFlyer()

    # Package dirs for mesh files
    package_dirs = kwargs.get("package_dirs", None)

    # Build model from URDF
    try:
        if package_dirs is not None:
            self.model = pin.buildModelFromUrdf(
                model_path, package_dirs, root_joint
            )
        else:
            self.model = pin.buildModelFromUrdf(model_path, root_joint)
        self.data = self.model.createData()
    except Exception as e:
        raise RuntimeError(
            f"Failed to load model '{model_path}' with Pinocchio: {e}\n"
            f"Ensure the file is a valid URDF."
        )

    self._verbose = kwargs.get("verbose", False)
```

### `nq`

Number of position variables (configuration space dimension).

### `nv`

Number of velocity variables (tangent space dimension).

### `model_format`

Model format (URDF).

### `from_model(model, data=None, **kwargs)`

Create PinocchioBackend from an existing pin.Model.

This avoids re-loading the URDF and shares the same model/data pair as an existing Robot/RobotWrapper instance.

Parameters:

| Name       | Type | Description                                          | Default    |
| ---------- | ---- | ---------------------------------------------------- | ---------- |
| `model`    |      | Existing pinocchio.Model                             | *required* |
| `data`     |      | Existing pinocchio.Data (created from model if None) | `None`     |
| `**kwargs` |      | Additional configuration (verbose, etc.)             | `{}`       |

Returns:

| Type | Description                                           |
| ---- | ----------------------------------------------------- |
|      | PinocchioBackend instance wrapping the existing model |

Raises:

| Type          | Description                   |
| ------------- | ----------------------------- |
| `ImportError` | If Pinocchio is not installed |

Source code in `src/figaroh/backends/pinocchio.py`

```
@classmethod
def from_model(cls, model, data=None, **kwargs):
    """
    Create PinocchioBackend from an existing pin.Model.

    This avoids re-loading the URDF and shares the same model/data pair
    as an existing Robot/RobotWrapper instance.

    Args:
        model: Existing pinocchio.Model
        data: Existing pinocchio.Data (created from model if None)
        **kwargs: Additional configuration (verbose, etc.)

    Returns:
        PinocchioBackend instance wrapping the existing model

    Raises:
        ImportError: If Pinocchio is not installed
    """
    if not PINOCCHIO_AVAILABLE:
        raise ImportError(
            "Pinocchio is not installed. Install with: pip install pin"
        )

    # Create instance without calling __init__ (which loads from URDF)
    backend = cls.__new__(cls)
    DynamicsBackend.__init__(backend, model_path=None, **kwargs)
    backend.model = model
    backend.data = data if data is not None else model.createData()
    backend._verbose = kwargs.get("verbose", False)
    return backend
```

### `compute_mass_matrix(q)`

Compute mass matrix using Pinocchio's CRBA (Composite Rigid Body Algorithm).

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description                                        |
| ---- | --------- | -------------------------------------------------- |
| `M`  | `ndarray` | Mass matrix [nv x nv], symmetric positive definite |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_mass_matrix(self, q: np.ndarray) -> np.ndarray:
    """
    Compute mass matrix using Pinocchio's CRBA (Composite Rigid Body Algorithm).

    Args:
        q: Joint positions [nq]

    Returns:
        M: Mass matrix [nv x nv], symmetric positive definite
    """
    return pin.crba(self.model, self.data, q).copy()
```

### `compute_coriolis_matrix(q, v)`

Compute Coriolis and centrifugal effects matrix C(q, qd).

Uses Pinocchio's computeCoriolisMatrix algorithm.

Parameters:

| Name | Type      | Description           | Default    |
| ---- | --------- | --------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq]  | *required* |
| `v`  | `ndarray` | Joint velocities [nv] | *required* |

Returns:

| Name | Type      | Description               |
| ---- | --------- | ------------------------- |
| `C`  | `ndarray` | Coriolis matrix [nv x nv] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_coriolis_matrix(self, q: np.ndarray, v: np.ndarray) -> np.ndarray:
    """
    Compute Coriolis and centrifugal effects matrix C(q, qd).

    Uses Pinocchio's computeCoriolisMatrix algorithm.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]

    Returns:
        C: Coriolis matrix [nv x nv]
    """
    return pin.computeCoriolisMatrix(self.model, self.data, q, v).copy()
```

### `compute_gravity_vector(q)`

Compute gravity effects vector using Pinocchio's generalized gravity.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description         |
| ---- | --------- | ------------------- |
| `g`  | `ndarray` | Gravity vector [nv] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_gravity_vector(self, q: np.ndarray) -> np.ndarray:
    """
    Compute gravity effects vector using Pinocchio's generalized gravity.

    Args:
        q: Joint positions [nq]

    Returns:
        g: Gravity vector [nv]
    """
    return pin.computeGeneralizedGravity(self.model, self.data, q).copy()
```

### `compute_forward_kinematics(q)`

Compute forward kinematics for all frames.

Computes joint placements via forwardKinematics and updates all frame placements via updateFramePlacements.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Type             | Description                                                                                                           |
| ---------------- | --------------------------------------------------------------------------------------------------------------------- |
| `Dict[str, Any]` | Dictionary mapping frame names to transformations:                                                                    |
| `Dict[str, Any]` | { 'frame_name': { 'position': np.ndarray [3], 'orientation': np.ndarray [3, 3], 'transformation': np.ndarray [4, 4] } |
| `Dict[str, Any]` | }                                                                                                                     |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_forward_kinematics(self, q: np.ndarray) -> Dict[str, Any]:
    """
    Compute forward kinematics for all frames.

    Computes joint placements via forwardKinematics and updates all
    frame placements via updateFramePlacements.

    Args:
        q: Joint positions [nq]

    Returns:
        Dictionary mapping frame names to transformations:
        {
            'frame_name': {
                'position': np.ndarray [3],
                'orientation': np.ndarray [3, 3],
                'transformation': np.ndarray [4, 4]
            }
        }
    """
    # Compute joint placements
    pin.forwardKinematics(self.model, self.data, q)

    # Update all frame placements
    pin.updateFramePlacements(self.model, self.data)

    fk_results = {}

    # Iterate over all frames (skip universe at index 0)
    for i in range(1, len(self.model.frames)):
        frame = self.model.frames[i]
        placement = self.data.oMf[i]

        fk_results[frame.name] = {
            "position": placement.translation.copy(),
            "orientation": placement.rotation.copy(),
            "transformation": placement.homogeneous.copy(),
        }

    return fk_results
```

### `compute_jacobian(q, frame)`

Compute geometric Jacobian for a specific frame.

Parameters:

| Name    | Type      | Description          | Default    |
| ------- | --------- | -------------------- | ---------- |
| `q`     | `ndarray` | Joint positions [nq] | *required* |
| `frame` | `str`     | Name of the frame    | *required* |

Returns:

| Name | Type      | Description                                                                |
| ---- | --------- | -------------------------------------------------------------------------- |
| `J`  | `ndarray` | Geometric Jacobian [6 x nv] Stacked as [linear_velocity; angular_velocity] |

Raises:

| Type         | Description                        |
| ------------ | ---------------------------------- |
| `ValueError` | If frame is not found in the model |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_jacobian(self, q: np.ndarray, frame: str) -> np.ndarray:
    """
    Compute geometric Jacobian for a specific frame.

    Args:
        q: Joint positions [nq]
        frame: Name of the frame

    Returns:
        J: Geometric Jacobian [6 x nv]
           Stacked as [linear_velocity; angular_velocity]

    Raises:
        ValueError: If frame is not found in the model
    """
    # Look up frame ID
    frame_id = self.model.getFrameId(frame)
    if frame_id >= len(self.model.frames):
        raise ValueError(
            f"Frame '{frame}' not found in model. "
            f"Available frames: {[f.name for f in self.model.frames[1:]]}"
        )

    # Compute frame Jacobian (ensure [6, nv] shape)
    J = pin.computeFrameJacobian(self.model, self.data, q, frame_id, pin.LOCAL)

    return J.reshape(6, self.model.nv).copy()
```

### `compute_regressor(q, v, a)`

Compute observation regressor matrix W(q, v, a).

The regressor satisfies: tau = W(q, v, a) * theta where theta is the parameter vector.

Uses Pinocchio's computeJointTorqueRegressor, which computes the regressor for all 10 standard inertial parameters per body.

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name | Type      | Description                          |
| ---- | --------- | ------------------------------------ |
| `W`  | `ndarray` | Regressor matrix [nv x (10 * nbody)] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_regressor(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute observation regressor matrix W(q, v, a).

    The regressor satisfies: tau = W(q, v, a) * theta
    where theta is the parameter vector.

    Uses Pinocchio's computeJointTorqueRegressor, which computes
    the regressor for all 10 standard inertial parameters per body.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        W: Regressor matrix [nv x (10 * nbody)]
    """
    W = pin.computeJointTorqueRegressor(self.model, self.data, q, v, a)
    # Ensure 2D shape [nv, n_params] (Pinocchio may return 1D for nv=1)
    if W.ndim == 1:
        W = W.reshape(self.model.nv, -1)
    return W.copy()
```

### `compute_inverse_dynamics(q, v, a)`

Compute inverse dynamics (RNEA) using Pinocchio's rnea.

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name  | Type      | Description        |
| ----- | --------- | ------------------ |
| `tau` | `ndarray` | Joint torques [nv] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_inverse_dynamics(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute inverse dynamics (RNEA) using Pinocchio's rnea.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        tau: Joint torques [nv]
    """
    return pin.rnea(self.model, self.data, q, v, a).copy()
```

### `compute_forward_dynamics(q, v, tau)`

Compute forward dynamics (ABA) using Pinocchio's aba.

Parameters:

| Name  | Type      | Description           | Default    |
| ----- | --------- | --------------------- | ---------- |
| `q`   | `ndarray` | Joint positions [nq]  | *required* |
| `v`   | `ndarray` | Joint velocities [nv] | *required* |
| `tau` | `ndarray` | Joint torques [nv]    | *required* |

Returns:

| Name | Type      | Description              |
| ---- | --------- | ------------------------ |
| `a`  | `ndarray` | Joint accelerations [nv] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_forward_dynamics(
    self, q: np.ndarray, v: np.ndarray, tau: np.ndarray
) -> np.ndarray:
    """
    Compute forward dynamics (ABA) using Pinocchio's aba.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        tau: Joint torques [nv]

    Returns:
        a: Joint accelerations [nv]
    """
    return pin.aba(self.model, self.data, q, v, tau).copy()
```

### `get_joint_names()`

Get list of joint names.

Returns:

| Type   | Description                                      |
| ------ | ------------------------------------------------ |
| `list` | List of joint names in order (skipping universe) |

Source code in `src/figaroh/backends/pinocchio.py`

```
def get_joint_names(self) -> list:
    """
    Get list of joint names.

    Returns:
        List of joint names in order (skipping universe)
    """
    return list(self.model.names[1:])
```

### `get_frame_names()`

Get list of frame names.

Returns:

| Type   | Description                                                       |
| ------ | ----------------------------------------------------------------- |
| `list` | List of frame names available for FK/Jacobian (skipping universe) |

Source code in `src/figaroh/backends/pinocchio.py`

```
def get_frame_names(self) -> list:
    """
    Get list of frame names.

    Returns:
        List of frame names available for FK/Jacobian (skipping universe)
    """
    return [f.name for f in self.model.frames[1:]]
```

### `get_inertias()`

Get per-body inertia objects.

Returns:

| Type   | Description                                                              |
| ------ | ------------------------------------------------------------------------ |
| `list` | List of inertia objects (includes universe at index 0 with zero inertia) |

Source code in `src/figaroh/backends/pinocchio.py`

```
def get_inertias(self) -> list:
    """
    Get per-body inertia objects.

    Returns:
        List of inertia objects (includes universe at index 0 with zero inertia)
    """
    return list(self.model.inertias)
```

### `get_frame_id(frame)`

Get frame ID by name.

Parameters:

| Name    | Type  | Description | Default    |
| ------- | ----- | ----------- | ---------- |
| `frame` | `str` | Frame name  | *required* |

Returns:

| Type  | Description        |
| ----- | ------------------ |
| `int` | Frame ID (integer) |

Raises:

| Type         | Description                        |
| ------------ | ---------------------------------- |
| `ValueError` | If frame is not found in the model |

Source code in `src/figaroh/backends/pinocchio.py`

```
def get_frame_id(self, frame: str) -> int:
    """
    Get frame ID by name.

    Args:
        frame: Frame name

    Returns:
        Frame ID (integer)

    Raises:
        ValueError: If frame is not found in the model
    """
    frame_id = self.model.getFrameId(frame)
    if frame_id >= len(self.model.frames):
        raise ValueError(
            f"Frame '{frame}' not found in model. "
            f"Available frames: {[f.name for f in self.model.frames[1:]]}"
        )
    return frame_id
```

### `compute_difference(q1, q2)`

Compute Lie group difference between two configurations (q2 ⊖ q1).

Parameters:

| Name | Type      | Description               | Default    |
| ---- | --------- | ------------------------- | ---------- |
| `q1` | `ndarray` | First configuration [nq]  | *required* |
| `q2` | `ndarray` | Second configuration [nq] | *required* |

Returns:

| Type      | Description            |
| --------- | ---------------------- |
| `ndarray` | Difference vector [nv] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_difference(self, q1: np.ndarray, q2: np.ndarray) -> np.ndarray:
    """
    Compute Lie group difference between two configurations (q2 ⊖ q1).

    Args:
        q1: First configuration [nq]
        q2: Second configuration [nq]

    Returns:
        Difference vector [nv]
    """
    return pin.difference(self.model, q1, q2)
```

### `compute_integrate(q, v)`

Integrate configuration by velocity (q ⊕ v).

Parameters:

| Name | Type      | Description        | Default    |
| ---- | --------- | ------------------ | ---------- |
| `q`  | `ndarray` | Configuration [nq] | *required* |
| `v`  | `ndarray` | Velocity [nv]      | *required* |

Returns:

| Type      | Description            |
| --------- | ---------------------- |
| `ndarray` | New configuration [nq] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def compute_integrate(self, q: np.ndarray, v: np.ndarray) -> np.ndarray:
    """
    Integrate configuration by velocity (q ⊕ v).

    Args:
        q: Configuration [nq]
        v: Velocity [nv]

    Returns:
        New configuration [nq]
    """
    return pin.integrate(self.model, q, v)
```

### `random_configuration()`

Generate a random configuration within joint limits.

Returns:

| Type      | Description               |
| --------- | ------------------------- |
| `ndarray` | Random configuration [nq] |

Source code in `src/figaroh/backends/pinocchio.py`

```
def random_configuration(self) -> np.ndarray:
    """
    Generate a random configuration within joint limits.

    Returns:
        Random configuration [nq]
    """
    return pin.randomConfiguration(self.model)
```

### `get_model_object()`

Escape hatch: return the underlying Pinocchio model.

Returns:

| Type  | Description      |
| ----- | ---------------- |
| `Any` | pin.Model object |

Source code in `src/figaroh/backends/pinocchio.py`

```
def get_model_object(self) -> Any:
    """
    Escape hatch: return the underlying Pinocchio model.

    Returns:
        pin.Model object
    """
    return self.model
```

## mujoco

MuJoCo Dynamics Backend for FIGAROH

High-performance dynamics computation using MuJoCo's optimized algorithms. Extracted and integrated from figaroh-mujoco project.

Features:

- Sparse matrix operations
- Built-in URDF → MJCF conversion
- Efficient contact dynamics
- 2-3x faster than Pinocchio for large systems

## `MuJoCoBackend(model_path, **kwargs)`

Bases: `DynamicsBackend`

MuJoCo dynamics backend for FIGAROH.

This backend leverages MuJoCo's highly optimized sparse matrix operations and efficient dynamics algorithms. MuJoCo automatically converts URDF files to its internal MJCF format.

Performance

- Mass matrix: 2-3x faster than Pinocchio
- Regressor: 2-3x faster (sparse operations)
- Best for: Large systems, optimal control, contact dynamics

Example

> > > backend = MuJoCoBackend(model_path="robot.urdf") M = backend.compute_mass_matrix(q)

Initialize MuJoCo backend.

Parameters:

| Name         | Type  | Description                                                                | Default    |
| ------------ | ----- | -------------------------------------------------------------------------- | ---------- |
| `model_path` | `str` | Path to URDF or MJCF file                                                  | *required* |
| `**kwargs`   |       | Additional configuration - verbose: Enable verbose output (default: False) | `{}`       |

Source code in `src/figaroh/backends/mujoco.py`

```
def __init__(self, model_path: str, **kwargs):
    """
    Initialize MuJoCo backend.

    Args:
        model_path: Path to URDF or MJCF file
        **kwargs: Additional configuration
            - verbose: Enable verbose output (default: False)
    """
    if not MUJOCO_AVAILABLE:
        raise ImportError(
            "MuJoCo is not installed. Install with: pip install mujoco>=3.0.0"
        )

    super().__init__(model_path, **kwargs)

    # Load model (MuJoCo auto-converts URDF → MJCF)
    try:
        self.model = mj.MjModel.from_xml_path(model_path)
        self.data = mj.MjData(self.model)
    except Exception as e:
        raise RuntimeError(
            f"Failed to load model '{model_path}' with MuJoCo: {e}\n"
            f"Ensure the file is valid URDF or MJCF format."
        )

    # Pre-allocate matrices for performance
    self._M = np.zeros((self.model.nv, self.model.nv))
    self._temp_vec = np.zeros(self.model.nv)
    self._verbose = kwargs.get("verbose", False)

    # Lazy-loaded Pinocchio model for analytical regressor computation
    # (MuJoCo does not support runtime inertial parameter perturbation)
    self._pin_model = None
    self._pin_data = None

    if self._verbose:
        print(
            f"MuJoCo model loaded: {self.model.nq} positions, {self.model.nv} velocities"
        )
```

### `nq`

Number of position variables.

### `nv`

Number of velocity variables.

### `model_format`

Model format (MJCF, but supports URDF input).

### `compute_mass_matrix(q)`

Compute mass matrix using MuJoCo's mj_crb (Composite Rigid Body) algorithm.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description                                        |
| ---- | --------- | -------------------------------------------------- |
| `M`  | `ndarray` | Mass matrix [nv x nv], symmetric positive definite |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_mass_matrix(self, q: np.ndarray) -> np.ndarray:
    """
    Compute mass matrix using MuJoCo's mj_crb (Composite Rigid Body) algorithm.

    Args:
        q: Joint positions [nq]

    Returns:
        M: Mass matrix [nv x nv], symmetric positive definite
    """
    # Set joint positions
    self.data.qpos[:] = q

    # mj_forward must be called to initialize FK before mj_crb
    mj.mj_forward(self.model, self.data)

    # Compute mass matrix using composite rigid body algorithm
    mj.mj_crb(self.model, self.data)

    # Extract full mass matrix from sparse representation
    # MuJoCo 3.10 changed mj_fullM from (model, dst, qM) to
    # (model, data, dst); newer releases also remove data.qM.
    try:
        mj.mj_fullM(self.model, self.data, self._M)
    except TypeError:
        # The older Python binding rejects the new argument types.
        mj.mj_fullM(self.model, self._M, self.data.qM)

    return self._M.copy()
```

### `compute_coriolis_matrix(q, v)`

Compute Coriolis matrix C(q,v) via finite differences.

Uses the property that Coriolis forces f(v) = C(q,v)*v are quadratic in v. By Euler's theorem and Christoffel symbol symmetry, the Jacobian df/dv = 2*C, so C = (1/2) * df/dv.

Parameters:

| Name | Type      | Description           | Default    |
| ---- | --------- | --------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq]  | *required* |
| `v`  | `ndarray` | Joint velocities [nv] | *required* |

Returns:

| Name | Type      | Description               |
| ---- | --------- | ------------------------- |
| `C`  | `ndarray` | Coriolis matrix [nv x nv] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_coriolis_matrix(self, q: np.ndarray, v: np.ndarray) -> np.ndarray:
    """
    Compute Coriolis matrix C(q,v) via finite differences.

    Uses the property that Coriolis forces f(v) = C(q,v)*v are quadratic in v.
    By Euler's theorem and Christoffel symbol symmetry, the Jacobian
    df/dv = 2*C, so C = (1/2) * df/dv.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]

    Returns:
        C: Coriolis matrix [nv x nv]
    """
    nv = self.model.nv

    # Handle zero velocity: C(q, 0) = 0
    if np.allclose(v, 0):
        return np.zeros((nv, nv))

    # Compute gravity bias
    self.data.qpos[:] = q
    self.data.qvel[:] = 0
    self.data.qacc[:] = 0
    mj.mj_inverse(self.model, self.data)
    gravity = self.data.qfrc_inverse.copy()

    # Compute bias forces at velocity v
    self.data.qvel[:] = v
    mj.mj_inverse(self.model, self.data)
    bias = self.data.qfrc_inverse.copy()

    coriolis_forces = bias - gravity  # = C(q,v) * v

    # Compute Jacobian of coriolis_forces w.r.t. v via finite differences
    # df/dv[:,j] = (f(v + eps*e_j) - f(v)) / eps
    # C = (1/2) * df/dv (because f is quadratic and Christoffel symbols are symmetric)
    eps = 1e-6
    C = np.zeros((nv, nv))
    for j in range(nv):
        v_perturbed = v.copy()
        v_perturbed[j] += eps

        self.data.qpos[:] = q
        self.data.qvel[:] = v_perturbed
        self.data.qacc[:] = 0
        mj.mj_inverse(self.model, self.data)
        bias_perturbed = self.data.qfrc_inverse.copy()

        coriolis_perturbed = bias_perturbed - gravity
        C[:, j] = (coriolis_perturbed - coriolis_forces) / (2.0 * eps)

    # Restore state
    self.data.qpos[:] = q
    self.data.qvel[:] = v
    self.data.qacc[:] = 0
    mj.mj_inverse(self.model, self.data)

    return C
```

### `compute_gravity_vector(q)`

Compute gravity vector.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Name | Type      | Description         |
| ---- | --------- | ------------------- |
| `g`  | `ndarray` | Gravity vector [nv] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_gravity_vector(self, q: np.ndarray) -> np.ndarray:
    """
    Compute gravity vector.

    Args:
        q: Joint positions [nq]

    Returns:
        g: Gravity vector [nv]
    """
    # Set position and zero velocity/acceleration
    self.data.qpos[:] = q
    self.data.qvel[:] = 0
    self.data.qacc[:] = 0

    # Compute inverse dynamics with zero velocity/acceleration
    mj.mj_inverse(self.model, self.data)

    return self.data.qfrc_inverse.copy()
```

### `compute_forward_kinematics(q)`

Compute forward kinematics for all bodies.

Parameters:

| Name | Type      | Description          | Default    |
| ---- | --------- | -------------------- | ---------- |
| `q`  | `ndarray` | Joint positions [nq] | *required* |

Returns:

| Type             | Description                                      |
| ---------------- | ------------------------------------------------ |
| `Dict[str, Any]` | Dictionary mapping body names to transformations |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_forward_kinematics(self, q: np.ndarray) -> Dict[str, Any]:
    """
    Compute forward kinematics for all bodies.

    Args:
        q: Joint positions [nq]

    Returns:
        Dictionary mapping body names to transformations
    """
    self.data.qpos[:] = q
    mj.mj_forward(self.model, self.data)

    fk_results = {}

    # Iterate over all bodies (excluding world)
    for i in range(1, self.model.nbody):
        body_name = mj.mj_id2name(self.model, mj.mjtObj.mjOBJ_BODY, i)
        if body_name is None:
            body_name = f"body_{i}"

        # Get body position and orientation
        pos = self.data.xpos[i].copy()

        # Rotation matrix from quaternion
        quat = self.data.xquat[i]  # [w, x, y, z]
        rot_mat = np.zeros(9)
        mj.mju_quat2Mat(rot_mat, quat)
        rot_mat = rot_mat.reshape(3, 3)

        # Build 4x4 transformation matrix
        T = np.eye(4)
        T[:3, :3] = rot_mat
        T[:3, 3] = pos

        fk_results[body_name] = {
            "position": pos,
            "orientation": rot_mat,
            "quaternion": quat.copy(),
            "transformation": T,
        }

    return fk_results
```

### `compute_jacobian(q, frame)`

Compute geometric Jacobian for a specific body/site.

Parameters:

| Name    | Type      | Description              | Default    |
| ------- | --------- | ------------------------ | ---------- |
| `q`     | `ndarray` | Joint positions [nq]     | *required* |
| `frame` | `str`     | Name of the body or site | *required* |

Returns:

| Name | Type      | Description                                               |
| ---- | --------- | --------------------------------------------------------- |
| `J`  | `ndarray` | Geometric Jacobian [6 x nv], stacked as [linear; angular] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_jacobian(self, q: np.ndarray, frame: str) -> np.ndarray:
    """
    Compute geometric Jacobian for a specific body/site.

    Args:
        q: Joint positions [nq]
        frame: Name of the body or site

    Returns:
        J: Geometric Jacobian [6 x nv], stacked as [linear; angular]
    """
    self.data.qpos[:] = q
    mj.mj_forward(self.model, self.data)

    # Try to find body by name
    body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_BODY, frame)

    if body_id < 0:
        # Try site
        body_id = mj.mj_name2id(self.model, mj.mjtObj.mjOBJ_SITE, frame)
        if body_id < 0:
            raise ValueError(f"Frame '{frame}' not found in model")
        use_site = True
    else:
        use_site = False

    # Allocate Jacobian
    jacp = np.zeros(3 * self.model.nv)  # Linear part
    jacr = np.zeros(3 * self.model.nv)  # Angular part

    # Compute Jacobian
    if use_site:
        mj.mj_jacSite(self.model, self.data, jacp, jacr, body_id)
    else:
        mj.mj_jacBody(self.model, self.data, jacp, jacr, body_id)

    # Reshape and stack
    jacp = jacp.reshape(3, self.model.nv)
    jacr = jacr.reshape(3, self.model.nv)
    J = np.vstack([jacp, jacr])

    return J
```

### `compute_regressor(q, v, a)`

Compute observation regressor matrix W(q, v, a).

The regressor satisfies: tau = W @ theta where theta is the 10D inertial parameter vector per body in Pinocchio convention: [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz].

Uses Pinocchio's analytical computeJointTorqueRegressor (MuJoCo does not support runtime inertial parameter perturbation for finite differences).

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name | Type      | Description                          |
| ---- | --------- | ------------------------------------ |
| `W`  | `ndarray` | Regressor matrix [nv, 10\*(nbody-1)] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_regressor(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute observation regressor matrix W(q, v, a).

    The regressor satisfies: tau = W @ theta where theta is the 10D inertial
    parameter vector per body in Pinocchio convention:
    [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz].

    Uses Pinocchio's analytical computeJointTorqueRegressor (MuJoCo does not
    support runtime inertial parameter perturbation for finite differences).

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        W: Regressor matrix [nv, 10*(nbody-1)]
    """
    pin_model, pin_data = self._get_pin_model()

    W = pin.computeJointTorqueRegressor(pin_model, pin_data, q, v, a)

    # Ensure 2D shape [nv, n_params] (Pinocchio may return 1D for nv=1)
    if W.ndim == 1:
        W = W.reshape(self.model.nv, -1)

    return W.copy()
```

### `compute_inverse_dynamics(q, v, a)`

Compute inverse dynamics (RNEA) using mj_inverse.

Parameters:

| Name | Type      | Description              | Default    |
| ---- | --------- | ------------------------ | ---------- |
| `q`  | `ndarray` | Joint positions [nq]     | *required* |
| `v`  | `ndarray` | Joint velocities [nv]    | *required* |
| `a`  | `ndarray` | Joint accelerations [nv] | *required* |

Returns:

| Name  | Type      | Description        |
| ----- | --------- | ------------------ |
| `tau` | `ndarray` | Joint torques [nv] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_inverse_dynamics(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray
) -> np.ndarray:
    """
    Compute inverse dynamics (RNEA) using mj_inverse.

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        a: Joint accelerations [nv]

    Returns:
        tau: Joint torques [nv]
    """
    self.data.qpos[:] = q
    self.data.qvel[:] = v
    self.data.qacc[:] = a

    mj.mj_inverse(self.model, self.data)

    return self.data.qfrc_inverse.copy()
```

### `compute_forward_dynamics(q, v, tau)`

Compute forward dynamics (ABA) using mj_forward.

Applies joint torques via qfrc_applied (applied forces) rather than ctrl (which goes through actuators, which may not exist in URDF models).

Parameters:

| Name  | Type      | Description           | Default    |
| ----- | --------- | --------------------- | ---------- |
| `q`   | `ndarray` | Joint positions [nq]  | *required* |
| `v`   | `ndarray` | Joint velocities [nv] | *required* |
| `tau` | `ndarray` | Joint torques [nv]    | *required* |

Returns:

| Name | Type      | Description              |
| ---- | --------- | ------------------------ |
| `a`  | `ndarray` | Joint accelerations [nv] |

Source code in `src/figaroh/backends/mujoco.py`

```
def compute_forward_dynamics(
    self, q: np.ndarray, v: np.ndarray, tau: np.ndarray
) -> np.ndarray:
    """
    Compute forward dynamics (ABA) using mj_forward.

    Applies joint torques via qfrc_applied (applied forces) rather than
    ctrl (which goes through actuators, which may not exist in URDF models).

    Args:
        q: Joint positions [nq]
        v: Joint velocities [nv]
        tau: Joint torques [nv]

    Returns:
        a: Joint accelerations [nv]
    """
    self.data.qpos[:] = q
    self.data.qvel[:] = v
    # Zero out any previous applied forces
    self.data.qfrc_applied[:] = 0.0
    self.data.qfrc_applied[: len(tau)] = tau

    mj.mj_forward(self.model, self.data)

    return self.data.qacc.copy()
```

### `get_joint_names()`

Get list of joint names.

Source code in `src/figaroh/backends/mujoco.py`

```
def get_joint_names(self) -> list:
    """Get list of joint names."""
    names = []
    for i in range(self.model.njnt):
        name = mj.mj_id2name(self.model, mj.mjtObj.mjOBJ_JOINT, i)
        if name is None:
            name = f"joint_{i}"
        names.append(name)
    return names
```

### `get_frame_names()`

Get list of body and site names.

Source code in `src/figaroh/backends/mujoco.py`

```
def get_frame_names(self) -> list:
    """Get list of body and site names."""
    names = []

    # Add body names
    for i in range(1, self.model.nbody):  # Skip world
        name = mj.mj_id2name(self.model, mj.mjtObj.mjOBJ_BODY, i)
        if name is None:
            name = f"body_{i}"
        names.append(name)

    # Add site names
    for i in range(self.model.nsite):
        name = mj.mj_id2name(self.model, mj.mjtObj.mjOBJ_SITE, i)
        if name is None:
            name = f"site_{i}"
        names.append(name)

    return names
```

### `__exit__(exc_type, exc_val, exc_tb)`

Cleanup MuJoCo resources.

Source code in `src/figaroh/backends/mujoco.py`

```
def __exit__(self, exc_type, exc_val, exc_tb):
    """Cleanup MuJoCo resources."""
    # MuJoCo handles cleanup automatically
    pass
```

# Calibration

The calibration module provides tools for geometric calibration of robots, including the [reporting & verification suite](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) (`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
1. Data loading and validation
1. Parameter identification using base regressor analysis
1. Robust optimization with outlier detection and removal
1. Solution evaluation and validation
1. 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
    # Data contract (#55): the observations read, and stage records
    self.observations = None
    self.stages = []
```

### `initialize()`

Initialize calibration data and parameters.

Performs the initialization phase of calibration by:

1. Loading measurement data from files
1. Creating parameter list through base regressor analysis
1. 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=None, 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()
1. Optionally generates visualization plots if enabled
1. 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 |
| ------------------- | ---- | --------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `max_iterations`    |      | Maximum number of fits; between fits, samples whose position error exceeds outlier_threshold are excluded and the rest refitted.  | `3`     |
| `outlier_threshold` |      | Position error [m] above which a sample is excluded; None (default) uses the config's parameters.outlier_threshold (outlier_eps). | `None`  |
| `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" when the solver converged and to "FAILED" when it did not (the fit stage is then `failed` and the export paths refuse, see :meth:`require_exportable`)
- 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=None,
    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:
        max_iterations: Maximum number of fits; between fits, samples
            whose position error exceeds ``outlier_threshold`` are
            excluded and the rest refitted.
        outlier_threshold: Position error [m] above which a sample is
            excluded; ``None`` (default) uses the config's
            ``parameters.outlier_threshold`` (``outlier_eps``).
        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" when the solver converged
          and to "FAILED" when it did not (the fit stage is then
          ``failed`` and the export paths refuse, see
          :meth:`require_exportable`)
        - 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
    """
    from figaroh.tools.stages import record_stage

    self._run_started_at = datetime.now(timezone.utc).isoformat()
    try:
        result, outlier_indices = self.solve_optimisation(
            method=method,
            max_iterations=max_iterations,
            outlier_threshold=outlier_threshold,
            enable_logging=enable_logging,
        )
    except Exception as e:
        record_stage(self, "fit", "failed", f"{type(e).__name__}: {e}")
        raise

    # Evaluate solution
    evaluation = self._evaluate_solution(result, outlier_indices)
    record_stage(
        self,
        "fit",
        "ok" if getattr(result, "success", True) else "failed",
        str(getattr(result, "message", "")),
        {
            "rmse": (float(evaluation.get("rmse", float("nan"))), "m"),
            "parameters": len(result.x),
            "excluded_outliers": len(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"Samples excluded as outliers: {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
1. 3D pose visualizations comparing measured vs predicted poses
1. 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.

Only the unified configuration format is supported; a legacy flat config raises an error naming `figaroh.utils.config_migration`.

Parameters:

| Name           | Type  | Description                          | Default         |
| -------------- | ----- | ------------------------------------ | --------------- |
| `config_file`  | `str` | Path to a unified configuration file | *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.

    Only the unified configuration format is supported; a legacy flat
    config raises an error naming ``figaroh.utils.config_migration``.

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

        # The legacy flat format was removed in 1.0
        require_unified_config(config_file)
        parser = UnifiedConfigParser(config_file)
        unified_config = parser.parse()
        unified_calib_config = create_task_config(
            self.robot, unified_config, setting_type
        )
        # Convert unified format to the calib_config dict
        self.calib_config = unified_to_legacy_config(
            self.robot, unified_calib_config
        )

    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
1. Adds default names for unknown base and tip frames
1. 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

    if estimation.settings(self.calib_config)["method"] != "structural":
        try:
            estimation.configure(self)
            return True
        except Exception as e:
            raise CalibrationError(f"Parameter list creation failed: {e}")

    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)
        self._record_full_mapping()

        if hasattr(self, "q_measured") and hasattr(self, "PEE_measured"):
            self.eliminate_absorbed_parameters()

        return True

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

### `initial_frame_guess()`

Closed-form guess for the unknown base and tip frames.

See :func:`estimate_frames_closed_form`. Empty when the frames are known or the measurement does not determine them (partial position measurability, several markers, camera anchor).

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

```
def initial_frame_guess(self) -> Dict[str, float]:
    """Closed-form guess for the unknown base and tip frames.

    See :func:`estimate_frames_closed_form`. Empty when the frames are
    known or the measurement does not determine them (partial position
    measurability, several markers, camera anchor).
    """
    return estimate_frames_closed_form(
        self.model,
        self.data,
        self.q_measured,
        self.PEE_measured,
        self.calib_config,
    )
```

### `eliminate_absorbed_parameters(tol=0.0001)`

Drop joint parameters that the base/tip frames absorb on this data.

The structural selection in :func:`calculate_base_kinematics_regressor` uses the joint regressor alone. When the base and tip frames are also estimated, some retained joint parameters are combinations of frame parameters (e.g. a vertical prismatic or revolute first joint against the base's z translation and yaw), so the problem is rank deficient. This builds the full measurement Jacobian at the measured configurations, with the configured measurability, evaluated at the closed-form frame guess, and drops every joint parameter whose unit-normalised column is a combination of the frame columns and the joint columns kept before it (:func:`select_identifiable_parameters`). Frame parameters are always kept.

Set `calib_config["eliminate_absorbed_parameters"] = False` to skip.

Returns:

| Name   | Type        | Description                                     |
| ------ | ----------- | ----------------------------------------------- |
| `list` | `List[str]` | Names of the dropped parameters (also stored in |
|        | `List[str]` | calib_config["absorbed_param_name"]).           |

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

```
def eliminate_absorbed_parameters(self, tol: float = 1e-4) -> List[str]:
    """Drop joint parameters that the base/tip frames absorb on this data.

    The structural selection in :func:`calculate_base_kinematics_regressor`
    uses the joint regressor alone. When the base and tip frames are also
    estimated, some retained joint parameters are combinations of frame
    parameters (e.g. a vertical prismatic or revolute first joint against
    the base's z translation and yaw), so the problem is rank deficient.
    This builds the full measurement Jacobian at the measured
    configurations, with the configured measurability, evaluated at the
    closed-form frame guess, and drops every joint parameter whose
    unit-normalised column is a combination of the frame columns and the
    joint columns kept before it (:func:`select_identifiable_parameters`).
    Frame parameters are always kept.

    Set ``calib_config["eliminate_absorbed_parameters"] = False`` to skip.

    Returns:
        list: Names of the dropped parameters (also stored in
        ``calib_config["absorbed_param_name"]``).
    """
    cfg = self.calib_config
    cfg["absorbed_param_name"] = []
    if not cfg.get("eliminate_absorbed_parameters", True):
        return []
    names = list(cfg["param_name"])
    frames = self._frame_param_names()
    guess = self.initial_frame_guess()
    var0 = np.array([guess.get(n, 0.0) for n in names])
    J = measurement_jacobian(self.model, self.data, var0, self.q_measured, cfg)
    _, dropped = select_identifiable_parameters(J, names, frames, tol=tol)
    if dropped:
        drop_calibration_parameters(cfg, dropped)
        logger.info(
            "Dropped %d parameter(s) absorbed by the base/tip frames: %s",
            len(dropped),
            dropped,
        )
    cfg["absorbed_param_name"] = dropped
    return dropped
```

### `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
    """
    from figaroh.data.observations import PoseObservations
    from figaroh.tools.stages import record_stage

    # Same CSV layout and arrays as load_data(); del_list rows are
    # masked rather than deleted, and the config is not written by the
    # loader (#55, #105)
    try:
        self.observations = PoseObservations.from_csv(
            self._data_path,
            self.model,
            self.calib_config,
            del_list=self.del_list_ or (),
        )
        self.PEE_measured, self.q_measured = self.observations.to_legacy(
            self.model, self.calib_config
        )
    except Exception as e:
        record_stage(self, "data", "failed", f"{type(e).__name__}: {e}")
        raise CalibrationError(f"Data loading failed: {e}")
    self.calib_config["NbSample"] = len(self.q_measured)
    record_stage(
        self,
        "data",
        "ok",
        "PoseObservations from CSV",
        {
            "samples": len(self.q_measured),
            "masked_samples": int((~self.observations.mask).sum()),
        },
    )

    # 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 as e:
            # Don't fail calibration if validation data is unavailable
            logger.warning("Validation data %s not loaded: %s", val_data_path, e)
```

### `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. Regularise with priors (`estimation.method: map`, #120), not rows appended here.

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:
    >>> 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. Regularise with priors
    (``estimation.method: map``, #120), not rows appended here.

    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:
            >>> 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 measurement weighting.",
        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=None, enable_logging=False)`

Solve calibration optimization with robust outlier handling.

This method implements a comprehensive optimization strategy:

1. Sets up logging for progress tracking
1. Excludes samples above the outlier threshold and refits
1. Evaluates solution quality with detailed metrics
1. 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 number of fits                                                                     | `3`     |
| `outlier_threshold` | `float`   | Position error [m] above which a sample is excluded; None uses calib_config["outlier_eps"] | `None`  |
| `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: Optional[float] = None,
    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. Excludes samples above the outlier threshold and refits
    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 number of fits
        outlier_threshold (float, optional): Position error [m] above
            which a sample is excluded; ``None`` uses
            ``calib_config["outlier_eps"]``
        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: zero, except the base/tip frames, which start
    # at their closed-form estimate when it is available
    if var_init is None:
        var_init, _ = initialize_variables(self.calib_config, mode=0)
        guess = self.initial_frame_guess()
        for i, name in enumerate(self.calib_config["param_name"]):
            if name in guess:
                var_init[i] = guess[name]

    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
        # Residual variance from the measurement residuals only: a
        # subclass cost_function may append regularisation rows after
        # them. (least_squares' result.cost is 0.5 * sum(fun**2), so it
        # must not be squared, #107.)
        n_meas = len(self.PEE_measured)
        r_meas = np.asarray(result.fun)[:n_meas]
        # rows of samples excluded as outliers are zero, not observations
        excluded = getattr(self, "_excluded_rows", None)
        if excluded is not None:
            n_meas -= len(excluded)
        prior_noise = self.calib_config.get("prior_noise")
        self.residual_dof = n_meas - nvars
        if prior_noise is not None:
            # MAP: the prior rows in result.jac make this the posterior
            # covariance, with the noise the priors were scaled by
            sigma_ro_sq = prior_noise**2
        elif self.residual_dof <= 0:
            # as many parameters as observations: the residual
            # variance, hence the uncertainty, is not estimable (#100)
            logger.warning(
                f"{n_meas} observations for {nvars} parameters: "
                f"{self.residual_dof} residual degrees of freedom, "
                "parameter uncertainty not estimable"
            )
            self._C_param = None
            self.std_dev = [float("nan")] * nvars
            self.std_pctg = [float("nan")] * nvars
            return
        else:
            sigma_ro_sq = np.sum(r_meas**2) / self.residual_dof
        # Covariance from the full Jacobian, so regularisation rows act
        # as prior information on the parameters they constrain.
        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}")
```

### `redistribute_parameters()`

Joint corrections with standard deviations, for export.

- `structural` method: the fitted base parameters are lifted onto every joint parameter by a weighted minimum-norm lift (:func:`figaroh.calibration.estimation.lift_structural`, figaroh-plus#111): among all joint corrections that reproduce the fit, the most plausible under the expected error sizes (`calib_config["estimation"]["priors"]`, defaults in `estimation.DEFAULT_PRIORS`). Rows the base frame carries and rows the fit dropped are held at 0, so the lifted model predicts what the fit predicts, and the result does not depend on which representative the QR chose. `std_dev` is conditional on that choice of lift: it propagates the fitted uncertainty and is 0 in directions the data does not see.
- other methods (figaroh-plus#113): the fitted joint parameters directly; candidates left out of the fit are 0.

Tool-point and base-frame parameters are not included.

Returns:

| Name   | Type   | Description                                          |
| ------ | ------ | ---------------------------------------------------- |
| `dict` | `dict` | {name: {"value": float, "std_dev": float}} for every |
|        | `dict` | joint parameter of the calibration level.            |

Raises:

| Type               | Description                                                                                                                       |
| ------------------ | --------------------------------------------------------------------------------------------------------------------------------- |
| `CalibrationError` | If solve() hasn't run yet (no _C_param/ self.var_), or create_param_list() didn't populate the base-mapping keys in calib_config. |

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

```
def redistribute_parameters(self) -> dict:
    """Joint corrections with standard deviations, for export.

    - ``structural`` method: the fitted base parameters are lifted onto
      every joint parameter by a weighted minimum-norm lift
      (:func:`figaroh.calibration.estimation.lift_structural`,
      figaroh-plus#111): among all joint corrections that reproduce the
      fit, the most plausible under the expected error sizes
      (``calib_config["estimation"]["priors"]``, defaults in
      ``estimation.DEFAULT_PRIORS``). Rows the base frame carries and
      rows the fit dropped are held at 0, so the lifted model predicts
      what the fit predicts, and the result does not depend on which
      representative the QR chose. ``std_dev`` is conditional on that
      choice of lift: it propagates the fitted uncertainty and is 0 in
      directions the data does not see.
    - other methods (figaroh-plus#113): the fitted joint parameters
      directly; candidates left out of the fit are 0.

    Tool-point and base-frame parameters are not included.

    Returns:
        dict: ``{name: {"value": float, "std_dev": float}}`` for every
        joint parameter of the calibration level.

    Raises:
        CalibrationError: If `solve()` hasn't run yet (no `_C_param`/
            `self.var_`), or `create_param_list()` didn't populate the
            base-mapping keys in `calib_config`.
    """
    C_param = getattr(self, "_C_param", None)
    var_ = getattr(self, "var_", None)
    if C_param is None or var_ is None:
        raise CalibrationError(
            "redistribute_parameters requires solve() to have run first"
        )
    report = self.calib_config.get("estimation_report")
    if report is not None:
        # non-structural methods (#113) estimate the full joint
        # parameters directly: nothing to redistribute. Candidates
        # left out of the fit are at nominal (0, std 0).
        names = list(self.calib_config["param_name"])
        std = np.sqrt(np.abs(np.diag(C_param)))
        fitted = {n: (float(v), float(s)) for n, v, s in zip(names, var_, std)}
        return {
            n: {
                "value": fitted.get(n, (0.0, 0.0))[0],
                "std_dev": fitted.get(n, (0.0, 0.0))[1],
            }
            for n in report["candidates"]
        }
    if self.calib_config.get("base_mapping_matrix") is None or (
        self.calib_config.get("base_mapping_param_names") is None
    ):
        raise CalibrationError(
            "base mapping matrix not available in calib_config -- "
            "was create_param_list() run?"
        )
    names, theta, cov = estimation.lift_structural(self)
    std = np.sqrt(np.abs(np.diag(cov)))
    return {
        name: {"value": float(theta[i]), "std_dev": float(std[i])}
        for i, name in enumerate(names)
    }
```

### `require_exportable(allow_failed_fit=False)`

Refuse to export a calibration whose `fit` stage `failed`.

The gateway of every calibration export (:meth:`joint_corrections`, hence the URDF export, and `export_geometric_calibration_yaml`). A failed fit records stage `export` as failed and raises :class:`CalibrationError`. `allow_failed_fit=True` is the explicit override for a diagnostic export: it logs a warning, records stage `export` as `fallback` and notes the override in the run provenance. A successful fit is unaffected.

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

```
def require_exportable(self, allow_failed_fit: bool = False) -> None:
    """Refuse to export a calibration whose ``fit`` stage ``failed``.

    The gateway of every calibration export (:meth:`joint_corrections`,
    hence the URDF export, and ``export_geometric_calibration_yaml``).
    A failed fit records stage ``export`` as failed and raises
    :class:`CalibrationError`. ``allow_failed_fit=True`` is the explicit
    override for a diagnostic export: it logs a warning, records stage
    ``export`` as ``fallback`` and notes the override in the run
    provenance. A successful fit is unaffected.
    """
    from figaroh.tools.stages import record_stage

    fit = next(
        (s for s in getattr(self, "stages", None) or [] if s.stage == "fit"), None
    )
    if fit is None or fit.status != "failed":
        return
    reason = f"the fit failed ({fit.reason or 'solver did not converge'})"
    if allow_failed_fit:
        logger.warning(
            "Exporting from a failed fit (allow_failed_fit=True): %s", reason
        )
        record_stage(
            self,
            "export",
            "fallback",
            f"exported from a failed fit, diagnostic only: {reason}",
        )
        prov = getattr(self, "_run_provenance", None)
        if isinstance(prov, dict):
            prov["export_override"] = {
                "allow_failed_fit": True,
                "fit_reason": fit.reason,
            }
        return
    record_stage(self, "export", "failed", reason)
    raise CalibrationError(
        f"export failed: {reason}; nothing to export. Pass "
        f"allow_failed_fit=True for a diagnostic export only."
    )
```

### `joint_corrections(lift=True, drop_unsupported=False, allow_failed_fit=False)`

Joint parameter values to write into a URDF (`export_urdf`).

`lift=True`: :meth:`redistribute_parameters` (the weighted lift for `structural`; the fitted values otherwise), so the URDF and the PAL export carry the same corrections. `lift=False`: the fitted joint parameters as estimated (for `structural`, one representative per dependent group and the rest at 0). Either way the reloaded URDF, with :meth:`metrology_frames` applied outside it, reproduces the calibrated forward kinematics (figaroh-plus#62).

Frame parameters are never included; see :meth:`metrology_frames`.

Parameters:

| Name               | Type   | Description                                                                                                                                                                       | Default |
| ------------------ | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `lift`             | `bool` | see above.                                                                                                                                                                        | `True`  |
| `drop_unsupported` | `bool` | leave out fitted parameters a URDF cannot carry (elastic k\_\*, contact planes, ...). By default they raise, because the reloaded model would then not reproduce the calibration. | `False` |
| `allow_failed_fit` | `bool` | export although the solver failed, for diagnosis only; see :meth:require_exportable.                                                                                              | `False` |

Raises:

| Type               | Description                                                                                                                                                                        |
| ------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `CalibrationError` | if the fit stage failed (stage export is recorded as failed), or if the fit has parameters that are neither frames nor kinematic joint corrections, and drop_unsupported is False. |

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

```
def joint_corrections(
    self,
    lift: bool = True,
    drop_unsupported: bool = False,
    allow_failed_fit: bool = False,
) -> Dict[str, float]:
    """Joint parameter values to write into a URDF (``export_urdf``).

    ``lift=True``: :meth:`redistribute_parameters` (the weighted lift
    for ``structural``; the fitted values otherwise), so the URDF and
    the PAL export carry the same corrections. ``lift=False``: the
    fitted joint parameters as estimated (for ``structural``, one
    representative per dependent group and the rest at 0). Either way
    the reloaded URDF, with :meth:`metrology_frames` applied outside
    it, reproduces the calibrated forward kinematics (figaroh-plus#62).

    Frame parameters are never included; see :meth:`metrology_frames`.

    Args:
        lift: see above.
        drop_unsupported: leave out fitted parameters a URDF cannot
            carry (elastic ``k_*``, contact planes, ...). By default
            they raise, because the reloaded model would then not
            reproduce the calibration.
        allow_failed_fit: export although the solver failed, for
            diagnosis only; see :meth:`require_exportable`.

    Raises:
        CalibrationError: if the ``fit`` stage ``failed`` (stage
            ``export`` is recorded as failed), or if the fit has parameters that are neither
            frames nor kinematic joint corrections, and
            ``drop_unsupported`` is False.
    """
    from figaroh.tools.urdf_exporter import is_kinematic_correction

    self.require_exportable(allow_failed_fit)
    if lift:
        values = {n: v["value"] for n, v in self.redistribute_parameters().items()}
    else:
        frames = set(self._frame_param_names())
        values = {
            n: float(v)
            for n, v in zip(self.calib_config["param_name"], self.var_)
            if n not in frames
        }
    unsupported = [n for n in values if not is_kinematic_correction(n)]
    if unsupported and not drop_unsupported:
        from figaroh.tools.stages import record_stage

        record_stage(
            self,
            "export",
            "failed",
            f"not representable in a URDF: {unsupported[:6]}",
        )
        raise CalibrationError(
            f"{len(unsupported)} fitted parameter(s) cannot be written to "
            f"a URDF, so the exported model would not reproduce this "
            f"calibration: {unsupported[:6]}"
            f"{' ...' if len(unsupported) > 6 else ''}. Pass "
            f"drop_unsupported=True to export the kinematic corrections "
            f"only, and keep the others from calibrator.var_."
        )
    return {n: v for n, v in values.items() if n not in unsupported}
```

### `metrology_frames()`

Fitted base frame and tool point: the measurement setup.

`base_*` places the robot base in the measurement frame (mocap world, camera); `pEE*`/`phiEE*` place the measured point on the tool. They describe this setup, not the robot, so they are never in :meth:`joint_corrections` and `export_urdf` does not write them. To reproduce the calibrated measurements from an exported URDF, apply them outside it, e.g. `calc_updated_fkm(reloaded_model, ..., values, q, dict(calib_config, param_name=list(frames)))`. Frames given as known (`known_baseframe`/`known_tipframe`) are not fitted and not returned.

Raises:

| Type               | Description                |
| ------------------ | -------------------------- |
| `CalibrationError` | If solve() hasn't run yet. |

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

```
def metrology_frames(self) -> Dict[str, float]:
    """Fitted base frame and tool point: the measurement setup.

    ``base_*`` places the robot base in the measurement frame (mocap
    world, camera); ``pEE*``/``phiEE*`` place the measured point on the
    tool. They describe this setup, not the robot, so they are never in
    :meth:`joint_corrections` and ``export_urdf`` does not write them.
    To reproduce the calibrated measurements from an exported URDF,
    apply them outside it, e.g. ``calc_updated_fkm(reloaded_model, ...,
    values, q, dict(calib_config, param_name=list(frames)))``.
    Frames given as known (``known_baseframe``/``known_tipframe``) are
    not fitted and not returned.

    Raises:
        CalibrationError: If `solve()` hasn't run yet.
    """
    if getattr(self, "var_", None) is None:
        raise CalibrationError("metrology_frames requires solve() to have run")
    frames = set(self._frame_param_names())
    return {
        n: float(v)
        for n, v in zip(self.calib_config["param_name"], self.var_)
        if n in frames
    }
```

### `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 have run (STATUS "CALIBRATED", or "FAILED" to inspect a fit that did not converge)
- 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 have run (STATUS "CALIBRATED", or "FAILED" to
          inspect a fit that did not converge)
        - 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 == "NOT 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 == "NOT 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, scope='prediction')`

Return scoped acceptance evidence with explicit incomplete states.

`scope="execution"` checks finite numerical fit outputs; it does not certify prediction, physical parameters or export. `scope="prediction"` (default) additionally requires independent validation and explicit application error limits. No universal improvement, correlation, conditioning or prediction-error gate is imposed.

`thresholds` maps metric names to `threshold`, `comparison` (min/max), and optional `required` (default True). Missing required evidence produces not_evaluated, not PASS. Nonfinite evidence fails. `passed` is True only when all required checks in scope pass.

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

```
def verify(
    self,
    thresholds: Optional[Dict[str, Dict[str, Any]]] = None,
    scope: str = "prediction",
):
    """Return scoped acceptance evidence with explicit incomplete states.

    ``scope="execution"`` checks finite numerical fit outputs;
    it does not certify prediction, physical parameters or export.
    ``scope="prediction"`` (default) additionally requires independent validation
    and explicit application error limits. No universal improvement,
    correlation, conditioning or prediction-error gate is imposed.

    ``thresholds`` maps metric names to ``threshold``, ``comparison``
    (min/max), and optional ``required`` (default True). Missing required
    evidence produces not_evaluated, not PASS. Nonfinite evidence fails.
    ``passed`` is True only when all required checks in scope pass.
    """
    if not hasattr(self, "evaluation_metrics"):
        raise AttributeError("No calibration results available. Run solve() first.")

    from figaroh.tools._report_common import (
        CALIBRATION_DEFAULT_THRESHOLDS,
        scoped_verification,
    )
    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")),
    }
    independent = (
        bool(getattr(self, "_val_available", False)) and validation is not None
    )
    # Position/orientation metrics only for the kinds actually
    # measured, not inferred from the component count (#100)
    comps = _measured_components(self.calib_config)
    prediction_keys = []
    if comps["pos_rows"]:
        prediction_keys.append("position_rmse_mm")
    if comps["orient_rows"]:
        prediction_keys.append("orientation_rmse_deg")
    if validation is not None:
        # Training-data fallback stays labelled as training evidence, so
        # it can never satisfy a validation_* prediction limit.
        prefix = "" if independent else "training_"
        if comps["pos_rows"]:
            metrics[f"{prefix}position_rmse_mm"] = validation.get(
                "pos_rmse_calibrated_mm", float("nan")
            )
        if comps["orient_rows"]:
            metrics[f"{prefix}orientation_rmse_deg"] = validation.get(
                "orient_rmse_calibrated_deg", float("nan")
            )
    verdict = scoped_verification(
        metrics,
        thresholds,
        scope,
        {
            "finite_fit_rmse": eval_.get("rmse"),
            "finite_parameters": results_data.get("calibrated parameters values"),
            "finite_residuals": results_data.get("residuals"),
        },
        independent,
        prediction_keys,
        solver_success=eval_.get("optimization_success"),
        facts={
            "requested_validation_loaded": (
                not self.calib_config.get("validation_data_file")
                or bool(getattr(self, "_val_available", False))
            )
        },
    )
    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")

    dof_names = comps["names"]
    if validation is not None and "error_nominal_per_dof" in validation:
        # one entry per point of each sample (marker-major, #119)
        n_val = validation.get("n_val_samples", 0) * validation.get("n_markers", 1)
        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"),
    }
    from figaroh.tools.stages import apply_to_verdict

    # the reported parameters come from the fit (phi_base / var_)
    apply_to_verdict(verdict, self, selected_stage="fit")
    return verdict
```

### `export_verification_report(output_path=None, output_dir='results', thresholds=None, scope='prediction')`

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`         |
| `scope`       | `str`                                 | Forwarded to :meth:verify; default prediction acceptance.                               | `'prediction'` |

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,
    scope: str = "prediction",
) -> 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`.
        scope: Forwarded to :meth:`verify`; default prediction acceptance.

    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, scope=scope)
    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")

    from figaroh.tools._report_common import verification_json_data
    from figaroh.tools.stages import with_schema as _with_schema

    with open(output_path, "w") as f:
        json.dump(
            verification_json_data(_with_schema(self, verdict_dict)),
            f,
            indent=2,
            allow_nan=False,
        )

    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)
    from figaroh.tools.stages import stages_line

    print(f"  Stages:       {stages_line(self)}")

    # ── 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}"
    )
    threshold = eval_.get("outlier_threshold")
    print(
        f"  Excluded:     {eval_['n_outliers']} "
        f"/ {self.calib_config['NbSample']} samples "
        f"({eval_['outlier_percentage']:.1f}%), position error > "
        + (f"{threshold * 1e3:.1f} mm" if threshold is not None else "no threshold")
    )
    excluded = eval_.get("excluded_samples") or []
    if excluded:
        errors = eval_.get("excluded_sample_errors") or [float("nan")] * len(
            excluded
        )
        print(
            "                "
            + ", ".join(
                f"#{i} ({e * 1e3:.1f} mm)" for i, e in zip(excluded, errors)
            )
        )

    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 (measured components of each kind)")

        def _fmt(value, spec, unit):
            if value is None or np.isnan(value):
                return "not measured"
            return f"{value:{spec}} {unit}"

        for stat, key in (("RMSE", "rmse"), ("MAE", "mae"), ("max", "max")):
            if f"pos_{key}_mm" not in overall:
                continue
            pos = _fmt(overall[f"pos_{key}_mm"], ".2f", "mm")
            orient = _fmt(overall[f"orient_{key}_deg"], ".4f", "deg")
            print(
                f"    {'Position ' + stat + ':':<18s}{pos}    "
                f"{'Orientation ' + stat + ':':<19s}{orient}"
            )

    # ── Validation ──
    print("-" * 70)
    if val is not None:
        if val.get("validation_source") == "calibration_data_fallback":
            # a held-out set evaluated outside BaseCalibration is not
            # seen here (#100)
            print(
                "  ⚠ WARNING: no validation_data_file loaded by this "
                "calibration — falling back to calibration data. "
                "These are NOT an independent generalization test; "
                "a held-out evaluation done elsewhere is not shown."
            )
            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}")
        for label, kind, unit, digits in (
            ("Position RMSE", "pos_rmse", "mm", 2),
            ("Orientation RMSE", "orient_rmse", "deg", 4),
            ("Position max", "pos_max", "mm", 2),
            ("Orientation max", "orient_max", "deg", 4),
        ):
            nominal = val[f"{kind}_nominal_{unit}"]
            if np.isnan(nominal):
                continue  # no component of this kind measured (#100)
            calibrated = val[f"{kind}_calibrated_{unit}"]
            gain = val[f"{kind.split('_')[0]}_improvement_pct"]
            arrow = "\u2193" if gain > 0 else "\u2191"
            print(
                f"  {label:<20s} "
                f"{nominal:10.{digits}f} {unit}"
                f"{calibrated:12.{digits}f} {unit}"
                f"{gain:13.1f}%  {arrow}"
            )
        for k, (nominal, calibrated) in enumerate(_per_point_rows(val)):
            gain = (nominal - calibrated) / nominal * 100 if nominal else 0.0
            arrow = "\u2193" if gain > 0 else "\u2191"
            print(
                f"  {f'  point {k + 1} RMSE':<20s} "
                f"{nominal:10.2f} mm{calibrated:12.2f} mm"
                f"{gain:13.1f}%  {arrow}"
            )
    else:
        # None also when a subclass evaluates its held-out set
        # itself, so do not claim there is none (#100)
        print("  Validation: not computed by this calibration.")
        print(
            "    Set validation_data_file, or report the held-out "
            "evaluation done outside BaseCalibration."
        )

    # ── 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", [])
    residual_dof = eval_.get("residual_dof")
    if (
        residual_dof is not None
        and residual_dof <= 0
        and self.calib_config.get("prior_noise") is None
    ):
        print("-" * 70)
        print(
            f"  Parameter Uncertainty: {residual_dof} residual degrees "
            "of freedom — uncertainty not estimable"
        )
    elif 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

## `unified_to_legacy_config(robot, unified_calib_config)`

Convert unified configuration format to legacy calib_config format.

Maps the unified configuration structure to the `calib_config` dict the calibration classes consume.

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` | Calibration configuration dict (calib_config) |

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 unified configuration structure to the ``calib_config`` dict the
    calibration classes consume.

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

    Returns:
        dict: Calibration configuration dict (``calib_config``)

    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)

    # 7b. Extract eye-hand (camera) calibration frames, if enabled
    _extract_eye_hand_params(calib_config, unified_calib_config)

    # 8. Extract data configuration
    calib_config["NbSample"] = data.get("number_of_samples", 500)
    calib_config["data_file"] = data.get("source_file")
    # held-out measurements in the same format; "" means none
    calib_config["validation_data_file"] = data.get("validation_data_file") or None
    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"]
        # base_mapping_slice is positional: prepending shifts it
        if "base_mapping_slice" in calib_config:
            start, end = calib_config["base_mapping_slice"]
            calib_config["base_mapping_slice"] = (
                start + len(BASE_TPL),
                end + len(BASE_TPL),
            )
```

## `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)

    # a missing (occluded) point would make every residual NaN: name the rows
    bad = ~np.isfinite(pose_ee).all(axis=1)
    if bad.any():
        rows = np.delete(np.arange(len(df)), del_list or [])[bad]
        raise ValueError(
            f"{path_to_file}: non-finite marker measurements in CSV rows "
            f"{rows.tolist()} (e.g. an occluded point); exclude them with "
            "del_list or clean the file"
        )

    # 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"]):
        # a copy: q0 is robot.q0, which must not end up as the last sample (#125)
        config = calib_config["q0"].copy()
        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.parentJoint] * 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
```

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

Update forward kinematics with world frame transformations.

Single, unified FK-update function for calibration: composes the full chain of transformations::

```
wMf = wMo * oMee * eeMf
```

where

- `wMo`: world (measurement) frame to the kinematic chain's start frame. Estimated directly when `BASE_TPL` params are present in `param_name` (unknown base frame, e.g. `known_baseframe=False`), estimated via a known camera/ref-frame anchor when `calib_config["base_to_ref_frame"]`/`"ref_frame"` are set (e.g. eye-hand calibration), or identity otherwise.
- `oMee`: start frame to end frame, through the updated kinematic chain (`full_params`/`joint_offset` geometric error parameters), optionally including joint elasticity when `calib_config["non_geom"]` is set — a per-joint compliance parameter (`ELAS_TPL`) that adds a gravity-torque-proportional deflection about that joint's own motion axis, then reverts it, on every sample.
- `eeMf`: end frame to the measured marker frame (`EE_TPL` params), or identity if not estimated. With several markers (`NbMarkers` > 1, e.g. the points of one rigid body), each marker `k` has its own `eeMf_k` (`pEEx_k` ... `phiEEz_k`) on the same tool frame, and its rows follow marker `k - 1`'s (figaroh-plus#119).

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 - base_to_ref_frame, ref_frame: Optional camera-style known chain anchor (eye-hand calibration); None to disable - non_geom: Whether to apply joint elasticity - actJoint_idx: Active joint indices - measurability: Active DOFs - NbMarkers: Number of markers on the tool frame | *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 marker measurements in world frame, ordered       |
|           |      | marker, then measured component, then sample (as load_data) |

Notes

- Requires base or end-effector parameters in param_name to estimate wMo / eeMf; otherwise they default to identity.
- Validates all parameters in param_name are consumed exactly once.

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.

    Single, unified FK-update function for calibration: composes the full
    chain of transformations::

        wMf = wMo * oMee * eeMf

    where:
        - ``wMo``: world (measurement) frame to the kinematic chain's start
          frame. Estimated directly when ``BASE_TPL`` params are present in
          ``param_name`` (unknown base frame, e.g. ``known_baseframe=False``),
          estimated via a known camera/ref-frame anchor when
          ``calib_config["base_to_ref_frame"]``/``"ref_frame"`` are set (e.g.
          eye-hand calibration), or identity otherwise.
        - ``oMee``: start frame to end frame, through the updated kinematic
          chain (``full_params``/``joint_offset`` geometric error
          parameters), optionally including joint elasticity when
          ``calib_config["non_geom"]`` is set — a per-joint compliance
          parameter (``ELAS_TPL``) that adds a gravity-torque-proportional
          deflection about that joint's own motion axis, then reverts it,
          on every sample.
        - ``eeMf``: end frame to the measured marker frame (``EE_TPL``
          params), or identity if not estimated. With several markers
          (``NbMarkers`` > 1, e.g. the points of one rigid body), each
          marker ``k`` has its own ``eeMf_k`` (``pEEx_k`` ... ``phiEEz_k``)
          on the same tool frame, and its rows follow marker ``k - 1``'s
          (figaroh-plus#119).

    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
            - base_to_ref_frame, ref_frame: Optional camera-style known
              chain anchor (eye-hand calibration); None to disable
            - non_geom: Whether to apply joint elasticity
            - actJoint_idx: Active joint indices
            - measurability: Active DOFs
            - NbMarkers: Number of markers on the tool frame
        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 marker measurements in world frame, ordered
        marker, then measured component, then sample (as ``load_data``)

    Notes:
        - Requires base or end-effector parameters in param_name to
          estimate wMo / eeMf; otherwise they default to identity.
        - Validates all parameters in param_name are consumed exactly once.
    """

    # 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))

    # store parameter updated to the model
    updated_params = []

    # check if baseframe and end--effector frame are known; with no
    # parameters at all (nominal FK) neither is (#129)
    base_param_incl = any("base" in key for key in param_dict)
    ee_param_incl = any("EE" in key for key in param_dict)

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

    # 1/ calc transformation from the world frame to start frame: wMo
    base_to_ref_frame = calib_config.get("base_to_ref_frame")
    if base_to_ref_frame is not None:
        # known chain anchor + unknown camera/ref pose (e.g. eye-hand calib)
        start_f = calib_config["ref_frame"]
        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"], base_to_ref_frame
        )
        ref_to_cam = cartesian_to_SE3(base_tf)
        cam_to_ref = ref_to_cam.actInv(pin.SE3.Identity())
        wMo = b_to_cam * cam_to_ref
    elif base_param_incl:
        # fully unknown world (measurement) frame to start frame transform
        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 each marker frame,
    # if not known: eeMf (one per marker, identity when not estimated)
    n_markers = calib_config["NbMarkers"]
    eeMf = [pin.SE3.Identity()] * n_markers
    if ee_param_incl:
        for marker_idx in range(1, n_markers + 1):
            pee = np.zeros(6)
            for axis_pee_id, axis_pee in enumerate(EE_TPL):
                key = "{}_{}".format(axis_pee, marker_idx)
                if key in param_dict:
                    if verbose == 1:
                        logger.debug("Updating marker frame with [{}]".format(key))
                    pee[axis_pee_id] += param_dict[key]
                    updated_params.append(key)
            eeMf[marker_idx - 1] = cartesian_to_SE3(pee)

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

    # placements are restored from this copy once the samples are evaluated
    saved_placements = {
        j_id: model.jointPlacements[j_id].copy()
        for j_id in calib_config["actJoint_idx"]
    }

    # 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)

        # update joint placement: full_params perturb the placement in the
        # parent frame; joint offsets act about the joint's own axes (q + offset)
        if calib_config["calib_model"] == "joint_offset":
            model = apply_joint_offset(model, j_id, xyz_rpy)
        else:
            model = update_joint_placement(model, j_id, xyz_rpy)

    # joint elasticity: one compliance parameter per active joint (ELAS_TPL,
    # see _build_elastic_param_names), matched once here since the mapping
    # joint -> param is static; the deflection itself is gravity-torque
    # dependent and recomputed per sample below.
    elastic_map = {}
    if calib_config.get("non_geom"):
        for j_id in calib_config["actJoint_idx"]:
            j_name = model.names[j_id]
            for key in param_dict.keys():
                if j_name in key:
                    for elas_id, elas in enumerate(ELAS_TPL):
                        if elas in key:
                            if verbose == 1:
                                logger.debug(
                                    "Joint [{}] elastic gain [{}] on axis {}".format(
                                        j_name, key, elas
                                    )
                                )
                            elastic_map[j_id] = (key, elas_id)
                            updated_params.append(key)

    # 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 markers: rows are marker-major, then component
    n_dofs = calib_config["calibration_index"]
    PEE = np.zeros((n_markers * n_dofs, 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)

        if elastic_map:
            if backend is not None:
                tau = backend.compute_gravity_vector(q_[i, :])
            else:
                tau = pin.computeGeneralizedGravity(model, data, q_[i, :])

            geometric_placements = {
                j_id: model.jointPlacements[j_id].copy() for j_id in elastic_map
            }
            for j_id, (key, elas_id) in elastic_map.items():
                xyz_rpy = np.zeros(6)
                xyz_rpy[elas_id] = param_dict[key] * tau[j_id - 1]
                model = apply_joint_offset(model, j_id, xyz_rpy)

            # jointPlacements changed: data.oMf is stale until FK is redone
            if backend is not None:
                backend.compute_forward_kinematics(q_[i, :])
            else:
                pin.framesForwardKinematics(model, data, q_[i, :])
                pin.updateFramePlacements(model, data)

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

            # revert model back to origin from the added joint elastic error
            for j_id, placement in geometric_placements.items():
                model.jointPlacements[j_id] = placement
        else:
            oMee = get_rel_transform(model, data, start_f, end_f)

        # calculate transformation from world frame to each marker frame
        wMee = wMo * oMee
        for k, eeMf_k in enumerate(eeMf):
            wMf = wMee * eeMf_k
            trans = wMf.translation.tolist()
            orient = pin.rpy.matrixToRpy(wMf.rotation).tolist()
            loc = trans + orient
            measure = [
                loc[mea_id]
                for mea_id, mea in enumerate(calib_config["measurability"])
                if mea
            ]
            PEE[k * n_dofs : (k + 1) * n_dofs, i] = np.array(measure)

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

    # revert model back to original
    for j_id, placement in saved_placements.items():
        model.jointPlacements[j_id] = placement

    return PEE
```

## `update_joint_placement(model, joint_idx, xyz_rpy)`

Apply a `full_params` placement error in the joint frame.

`M <- M * SE3(exp3(xyz_rpy[3:6]), xyz_rpy[0:3])`: the translation is expressed in the joint frame and the rotation is a rotation vector. This is the convention of Pinocchio's kinematic regressor (`computeFrameKinematicRegressor(..., LOCAL)`), from which the base parameters and the base-mapping matrix are derived, so the fitted model has exactly the dependencies that selection assumed (figaroh-plus#110). Earlier versions added the translation in the parent frame and the rotation to the placement's RPY angles, which disagrees with the regressor wherever the nominal placement is rotated.

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,) d_px, d_py, d_pz (m) and d_phix, d_phiy, d_phiz (rad, rotation vector), in the joint frame | *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):
    """Apply a ``full_params`` placement error in the joint frame.

    ``M <- M * SE3(exp3(xyz_rpy[3:6]), xyz_rpy[0:3])``: the translation is
    expressed in the joint frame and the rotation is a rotation vector. This
    is the convention of Pinocchio's kinematic regressor
    (``computeFrameKinematicRegressor(..., LOCAL)``), from which the base
    parameters and the base-mapping matrix are derived, so the fitted model
    has exactly the dependencies that selection assumed (figaroh-plus#110).
    Earlier versions added the translation in the parent frame and the
    rotation to the placement's RPY angles, which disagrees with the
    regressor wherever the nominal placement is rotated.

    Args:
        model (pin.Model): Robot model to modify
        joint_idx (int): Index of joint to update
        xyz_rpy (ndarray): (6,) ``d_px, d_py, d_pz`` (m) and ``d_phix,
            d_phiy, d_phiz`` (rad, rotation vector), in the joint frame

    Returns:
        pin.Model: Updated robot model

    Side Effects:
        Modifies model.jointPlacements[joint_idx] in place
    """
    xyz_rpy = np.asarray(xyz_rpy, dtype=float)
    delta = pin.SE3(pin.exp3(xyz_rpy[3:6]), xyz_rpy[0:3])
    model.jointPlacements[joint_idx] = model.jointPlacements[joint_idx] * delta
    return model
```

## `apply_joint_offset(model, joint_idx, offset)`

Offset a joint about its own axes (joint-angle / joint-position offset).

Composes the offset on the child side of the joint placement, `M <- M * SE3(R(offset[3:6]), offset[0:3])`, so that it is expressed in the joint frame. For a revolute joint about z, `offset[5] = d` is then exactly the configuration `q + d`; for a prismatic joint along x, `offset[0] = d` is `q + d`. This is the meaning of the `offset{PX,PY,PZ,RX,RY,RZ}_<joint>` parameters (`JOINT_OFFSETTPL`, axis taken from the joint's `shortname()`) and of the elastic deflections (`ELAS_TPL`).

:func:`update_joint_placement` composes on the same side for the `full_params` (`d_p*`, `d_phi*`) placement errors, with all three rotation components as one rotation vector.

Parameters:

| Name        | Type      | Description                                                                                      | Default    |
| ----------- | --------- | ------------------------------------------------------------------------------------------------ | ---------- |
| `model`     | `Model`   | Robot model to modify                                                                            | *required* |
| `joint_idx` | `int`     | Index of joint to update                                                                         | *required* |
| `offset`    | `ndarray` | (6,) translation (x, y, z) and rotation (roll, pitch, yaw) offsets, expressed in the joint frame | *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 apply_joint_offset(model, joint_idx, offset):
    """Offset a joint about its own axes (joint-angle / joint-position offset).

    Composes the offset on the child side of the joint placement,
    ``M <- M * SE3(R(offset[3:6]), offset[0:3])``, so that it is expressed in
    the joint frame. For a revolute joint about z, ``offset[5] = d`` is then
    exactly the configuration ``q + d``; for a prismatic joint along x,
    ``offset[0] = d`` is ``q + d``. This is the meaning of the
    ``offset{PX,PY,PZ,RX,RY,RZ}_<joint>`` parameters (``JOINT_OFFSETTPL``,
    axis taken from the joint's ``shortname()``) and of the elastic
    deflections (``ELAS_TPL``).

    :func:`update_joint_placement` composes on the same side for the
    ``full_params`` (``d_p*``, ``d_phi*``) placement errors, with all three
    rotation components as one rotation vector.

    Args:
        model (pin.Model): Robot model to modify
        joint_idx (int): Index of joint to update
        offset (ndarray): (6,) translation (x, y, z) and rotation (roll,
            pitch, yaw) offsets, expressed in the joint frame

    Returns:
        pin.Model: Updated robot model

    Side Effects:
        Modifies model.jointPlacements[joint_idx] in place
    """
    offset = np.asarray(offset, dtype=float)
    delta = pin.SE3(pin.rpy.rpyToMatrix(offset[3:6]), offset[0:3])
    model.jointPlacements[joint_idx] = model.jointPlacements[joint_idx] * delta
    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
1. 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
    R = np.zeros([6 * calib_config["NbSample"], 6 * (model.njoints - 1)])
    J = np.zeros([6 * calib_config["NbSample"], model.njoints - 1])
    # seeded, so the selected parameter set does not depend on global RNG
    # state (figaroh-plus#99)
    rng = np.random.default_rng(calib_config.get("random_seed", 0))
    for i in range(calib_config["NbSample"]):
        if MIN_MODEL == 1:
            q_rand = random_joint_configuration(model, rng)
            # a copy: q0 is robot.q0 and must not be overwritten (#125)
            q_i = calib_config["q0"].copy()
            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
1. Eliminating unidentifiable parameters
1. 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)

    # Several markers of one body (figaroh-plus#119) observe the tool
    # frame's orientation, even when each marker's position alone is
    # measured: the structural regressor then uses every component.
    # Dependencies the actual points leave are removed at the data level.
    reg_config = calib_config
    if calib_config.get("NbMarkers", 1) > 1:
        reg_config = dict(calib_config, measurability=[True] * 6, calibration_index=6)

    # calculate kinematic regressor with random configs
    if not calib_config["free_flyer"]:
        Rrand = calculate_identifiable_kinematics_model(
            [], model, data, reg_config, backend=backend
        )
    else:
        Rrand = calculate_identifiable_kinematics_model(
            q, model, data, reg_config, backend=backend
        )
    # calculate kinematic regressor with input configs
    if np.any(np.array(q)):
        R = calculate_identifiable_kinematics_model(
            q, model, data, reg_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 the reduced
    # regressor, and the base-mapping matrix M s.t. phi_base = M @ theta_r
    # (theta_r ordered as paramsrand_e) -- one QR pass on the structural
    # (random-config) regressor covers both, since the base/dependent
    # column split is a property of the kinematic chain, not of any
    # particular dataset.
    # LAPACK pivoting: calibration selection is made deterministic in #113,
    # not here, so calibration results stay as they were.
    decomposer = QRDecomposer(tolerance=tol_qr, deterministic=False)
    M, paramsrand_base, idx_base, _ = decomposer.get_base_mapping_matrix_double(
        Rrand_e, paramsrand_e
    )

    # get base regressor from random data
    Rrand_b = build_baseRegressor(Rrand_e, idx_base)

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

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

    # update calibrating calib_config['param_name']/calibrating parameters
    # -- record the slice these base entries land at (param_name may
    # already carry earlier entries, e.g. elastic-gain params) so callers
    # can locate the corresponding values in a solved variable vector
    # (which is built one-to-one, in order, from param_name) regardless of
    # any later in-place renaming (add_base_name) or appending (add_pee_name).
    _base_slice_start = len(calib_config["param_name"])
    for j in idx_base:
        calib_config["param_name"].append(paramsrand_e[j])
    _base_slice_end = len(calib_config["param_name"])

    # Structural base-mapping, stashed for optional downstream
    # redistribution (see figaroh.tools.qrdecomposition.redistribute_min_norm
    # and BaseCalibration.redistribute_parameters). base_mapping_row_names
    # is a snapshot of the base-parameter name order *before*
    # add_base_name/add_pee_name may rename/prepend entries in
    # calib_config["param_name"], so callers should locate fitted values by
    # base_mapping_slice (positional), not by re-looking-up these names.
    calib_config["base_mapping_matrix"] = M
    calib_config["base_mapping_param_names"] = list(paramsrand_e)
    calib_config["base_mapping_row_names"] = [paramsrand_e[j] for j in idx_base]
    calib_config["base_mapping_slice"] = (_base_slice_start, _base_slice_end)

    return Rrand_b, R_b, R_e, paramsrand_base, paramsrand_e
```

## `random_joint_configuration(model, rng)`

Draw a configuration uniformly within joint limits from `rng`.

One-DoF joints are drawn within their position limits, or within [-pi, pi] when the limits are missing or wider than a turn; other joints stay at the neutral configuration. Unlike `pin.randomConfiguration`, the draw depends only on `rng`, not on Pinocchio's global generator.

Parameters:

| Name    | Type        | Description      | Default    |
| ------- | ----------- | ---------------- | ---------- |
| `model` | `Model`     | Robot model      | *required* |
| `rng`   | `Generator` | Random generator | *required* |

Returns:

| Name      | Type | Description         |
| --------- | ---- | ------------------- |
| `ndarray` |      | (nq,) configuration |

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

```
def random_joint_configuration(model, rng):
    """Draw a configuration uniformly within joint limits from ``rng``.

    One-DoF joints are drawn within their position limits, or within
    [-pi, pi] when the limits are missing or wider than a turn; other joints
    stay at the neutral configuration. Unlike ``pin.randomConfiguration``,
    the draw depends only on ``rng``, not on Pinocchio's global generator.

    Args:
        model (pin.Model): Robot model
        rng (np.random.Generator): Random generator

    Returns:
        ndarray: (nq,) configuration
    """
    q = pin.neutral(model)
    for joint in model.joints[1:]:
        if joint.nq != 1:
            continue
        lo = model.lowerPositionLimit[joint.idx_q]
        hi = model.upperPositionLimit[joint.idx_q]
        if not (np.isfinite(lo) and np.isfinite(hi)) or hi - lo > 2 * np.pi:
            lo, hi = -np.pi, np.pi
        q[joint.idx_q] = rng.uniform(lo, hi)
    return q
```

## `estimate_frames_closed_form(model, data, q, PEE, calib_config, n_iter=50)`

Closed-form initial guess for the unknown base and tip frames.

With nominal joint parameters, the measured marker satisfies `P_i = R_b (p_i + R_i t_tip) + t_b` (and `R_meas_i = R_b R_i R_tip` when orientation is measured), where `(R_i, p_i)` is the nominal start-to-end frame transform. Rotations are estimated by Kabsch (positions) or chordal averaging (orientations), alternated with a linear least-squares solve for `t_b` and `t_tip`.

With several markers (`NbMarkers` > 1, points of one rigid body, figaroh-plus#119), each has its own `t_tip_k` and the base frame is fitted to all of them; orientations, if measured, are not used then and tip rotations are guessed as zero.

Only applies to markers whose position is fully measured, with the base frame estimated directly (`base_*` parameters, no camera `base_to_ref_frame` anchor). Otherwise returns an empty dict.

Parameters:

| Name           | Type      | Description                                         | Default    |
| -------------- | --------- | --------------------------------------------------- | ---------- |
| `model`        | `Model`   | Robot model (nominal joint placements)              | *required* |
| `data`         | `Data`    | Robot data                                          | *required* |
| `q`            | `ndarray` | (NbSample, nq) joint configurations                 | *required* |
| `PEE`          | `ndarray` | Flattened DOF-major measurements, as from load_data | *required* |
| `calib_config` | `dict`    | Calibration configuration                           | *required* |
| `n_iter`       | `int`     | Alternation iterations                              | `50`       |

Returns:

| Name   | Type | Description                                         |
| ------ | ---- | --------------------------------------------------- |
| `dict` |      | Initial values keyed by the base\_\*, pEE\*/phiEE\* |
|        |      | names present in calib_config["param_name"].        |

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

```
def estimate_frames_closed_form(model, data, q, PEE, calib_config, n_iter=50):
    """Closed-form initial guess for the unknown base and tip frames.

    With nominal joint parameters, the measured marker satisfies
    ``P_i = R_b (p_i + R_i t_tip) + t_b`` (and ``R_meas_i = R_b R_i R_tip``
    when orientation is measured), where ``(R_i, p_i)`` is the nominal
    start-to-end frame transform. Rotations are estimated by Kabsch
    (positions) or chordal averaging (orientations), alternated with a linear
    least-squares solve for ``t_b`` and ``t_tip``.

    With several markers (``NbMarkers`` > 1, points of one rigid body,
    figaroh-plus#119), each has its own ``t_tip_k`` and the base frame is
    fitted to all of them; orientations, if measured, are not used then and
    tip rotations are guessed as zero.

    Only applies to markers whose position is fully measured, with the
    base frame estimated directly (``base_*`` parameters, no camera
    ``base_to_ref_frame`` anchor). Otherwise returns an empty dict.

    Args:
        model (pin.Model): Robot model (nominal joint placements)
        data (pin.Data): Robot data
        q (ndarray): (NbSample, nq) joint configurations
        PEE (ndarray): Flattened DOF-major measurements, as from ``load_data``
        calib_config (dict): Calibration configuration
        n_iter (int): Alternation iterations

    Returns:
        dict: Initial values keyed by the ``base_*``, ``pEE*``/``phiEE*``
        names present in ``calib_config["param_name"]``.
    """
    names = list(calib_config["param_name"])
    meas = list(calib_config["measurability"])
    n_markers = calib_config.get("NbMarkers", 1)
    if (
        not any(n in names for n in BASE_TPL)
        or calib_config.get("base_to_ref_frame") is not None
        or not all(meas[:3])
    ):
        return {}
    n = len(q)
    M = np.asarray(PEE, dtype=float).reshape(n_markers, sum(meas), n)
    P = np.transpose(M[:, :3], (0, 2, 1))  # (n_markers, n, 3)
    orient = n_markers == 1 and len(meas) == 6 and all(meas[3:6])
    if orient:
        R_meas = np.array([pin.rpy.rpyToMatrix(M[0, 3:6, i]) for i in range(n)])

    R, p = np.empty((n, 3, 3)), np.empty((n, 3))
    for i in range(n):
        pin.framesForwardKinematics(model, data, q[i])
        T = get_rel_transform(
            model, data, calib_config["start_frame"], calib_config["end_frame"]
        )
        R[i], p[i] = T.rotation, T.translation

    tip_pos = [
        any(f"{e}_{k + 1}" in names for e in EE_TPL[:3]) for k in range(n_markers)
    ]
    tip_rot = orient and any(f"{e}_1" in names for e in EE_TPL[3:])
    R_b, R_tip = np.eye(3), np.eye(3)
    t_b, t_tip = np.zeros(3), np.zeros((n_markers, 3))
    free = [k for k in range(n_markers) if tip_pos[k]]
    for _ in range(n_iter):
        if orient:
            R_b = _chordal_mean(R_meas @ np.transpose(R @ R_tip, (0, 2, 1)))
            if tip_rot:
                R_tip = _chordal_mean(np.transpose(R_b @ R, (0, 2, 1)) @ R_meas)
        else:
            source = np.concatenate([p + R @ t_tip[k] for k in range(n_markers)])
            R_b = _kabsch(source, P.reshape(-1, 3))
        # P_k,i - R_b p_i = R_b R_i t_tip_k + t_b, linear in (t_tip_k, t_b);
        # tips not estimated stay at 0
        rhs = (P - (p @ R_b.T)[None]).reshape(-1)
        A = np.zeros((n_markers, n, 3, 3 * len(free) + 3))
        for col, k in enumerate(free):
            A[k, :, :, 3 * col : 3 * col + 3] = R_b @ R
        A[:, :, :, -3:] = np.eye(3)
        sol = np.linalg.lstsq(A.reshape(-1, A.shape[-1]), rhs, rcond=None)[0]
        for col, k in enumerate(free):
            t_tip[k] = sol[3 * col : 3 * col + 3]
        t_b = sol[-3:]

    values = np.concatenate([t_b, pin.rpy.matrixToRpy(R_b)])
    guess = {n_: v for n_, v in zip(BASE_TPL, values) if n_ in names}
    for k in range(n_markers):
        tip = np.concatenate([t_tip[k], pin.rpy.matrixToRpy(R_tip)])
        for e, v in zip(EE_TPL, tip):
            if f"{e}_{k + 1}" in names:
                guess[f"{e}_{k + 1}"] = v
    return guess
```

## `measurement_jacobian(model, data, var, q, calib_config, step=1e-06)`

Central-difference Jacobian of `calc_updated_fkm` w.r.t. `var`.

Rows follow the flattened, DOF-major measurement vector; columns follow `calib_config["param_name"]`.

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

```
def measurement_jacobian(model, data, var, q, calib_config, step=1e-6):
    """Central-difference Jacobian of ``calc_updated_fkm`` w.r.t. ``var``.

    Rows follow the flattened, DOF-major measurement vector; columns follow
    ``calib_config["param_name"]``.
    """
    var = np.asarray(var, dtype=float)
    cfg = dict(calib_config, NbSample=len(q))
    cols = []
    for j in range(len(var)):
        dv = np.zeros_like(var)
        dv[j] = step
        hi = calc_updated_fkm(model, data, var + dv, q, cfg)
        lo = calc_updated_fkm(model, data, var - dv, q, cfg)
        cols.append((hi - lo) / (2 * step))
    return np.column_stack(cols)
```

## `select_identifiable_parameters(jacobian, names, always_keep=(), tol=0.0001)`

Split parameters into identifiable and absorbed ones, deterministically.

Columns are normalised to unit length (so units do not matter) and taken in order: first `always_keep` (e.g. base and tip frames), then the rest of `names` in order. A column is kept when its component orthogonal to the columns already kept exceeds `tol`; otherwise it is a combination of them and is reported as absorbed. `always_keep` columns are never dropped.

Parameters:

| Name          | Type       | Description                                                                                                                                                                                                                                                      | Default    |
| ------------- | ---------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `jacobian`    | `ndarray`  | (n_meas, n_params) measurement Jacobian                                                                                                                                                                                                                          | *required* |
| `names`       | `list`     | Parameter names, one per column                                                                                                                                                                                                                                  | *required* |
| `always_keep` | `iterable` | Names kept unconditionally and tested first                                                                                                                                                                                                                      | `()`       |
| `tol`         | `float`    | Threshold on the orthogonal residual of a unit column. Exact dependencies give ~1e-10 (finite-difference noise); the default 1e-4 also drops near-dependencies whose column alone would have a condition number above 1e4 (a direction the data barely excites). | `0.0001`   |

Returns:

| Name    | Type | Description                                                      |
| ------- | ---- | ---------------------------------------------------------------- |
| `tuple` |      | (kept names in original order, absorbed names in original order) |

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

```
def select_identifiable_parameters(jacobian, names, always_keep=(), tol=1e-4):
    """Split parameters into identifiable and absorbed ones, deterministically.

    Columns are normalised to unit length (so units do not matter) and taken
    in order: first ``always_keep`` (e.g. base and tip frames), then the rest
    of ``names`` in order. A column is kept when its component orthogonal to
    the columns already kept exceeds ``tol``; otherwise it is a combination of
    them and is reported as absorbed. ``always_keep`` columns are never
    dropped.

    Args:
        jacobian (ndarray): (n_meas, n_params) measurement Jacobian
        names (list): Parameter names, one per column
        always_keep (iterable): Names kept unconditionally and tested first
        tol (float): Threshold on the orthogonal residual of a unit column.
            Exact dependencies give ~1e-10 (finite-difference noise); the
            default 1e-4 also drops near-dependencies whose column alone
            would have a condition number above 1e4 (a direction the data
            barely excites).

    Returns:
        tuple: (kept names in original order, absorbed names in original order)
    """
    names = list(names)
    always_keep = [n for n in names if n in set(always_keep)]
    order = always_keep + [n for n in names if n not in set(always_keep)]
    basis = np.zeros((jacobian.shape[0], 0))
    kept = set()
    for name in order:
        col = jacobian[:, names.index(name)]
        norm = np.linalg.norm(col)
        if norm > 0:
            r = col / norm
            for _ in range(2):  # re-orthogonalise for numerical stability
                r = r - basis @ (basis.T @ r)
            res = np.linalg.norm(r)
        else:
            res = 0.0
        if name in always_keep or res > tol:
            kept.add(name)
            if res > tol:
                basis = np.column_stack([basis, r / res])
    return [n for n in names if n in kept], [n for n in names if n not in kept]
```

## `drop_calibration_parameters(calib_config, dropped)`

Remove parameters from `param_name`, keeping the base mapping aligned.

A dropped name inside `base_mapping_slice` also loses its row of `base_mapping_matrix` / `base_mapping_row_names`; names before the slice shift it left.

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

```
def drop_calibration_parameters(calib_config, dropped):
    """Remove parameters from ``param_name``, keeping the base mapping aligned.

    A dropped name inside ``base_mapping_slice`` also loses its row of
    ``base_mapping_matrix`` / ``base_mapping_row_names``; names before the
    slice shift it left.
    """
    names = list(calib_config["param_name"])
    for idx in sorted((names.index(n) for n in dropped), reverse=True):
        if "base_mapping_slice" in calib_config:
            start, end = calib_config["base_mapping_slice"]
            if start <= idx < end:
                row = idx - start
                calib_config["base_mapping_matrix"] = np.delete(
                    calib_config["base_mapping_matrix"], row, axis=0
                )
                rows = list(calib_config["base_mapping_row_names"])
                del rows[row]
                calib_config["base_mapping_row_names"] = rows
                calib_config["base_mapping_slice"] = (start, end - 1)
            elif idx < start:
                calib_config["base_mapping_slice"] = (start - 1, end - 1)
        del names[idx]
    calib_config["param_name"] = names
```

## estimation

Parameter selection and estimation methods; see [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md).

Choosing which calibration parameters to estimate, and how (#113).

`calib_config["estimation"]["method"]` selects one of:

`structural` (default) The structural base parameters from random configurations, then the data-level elimination of frame-absorbed parameters (#102). Unchanged behaviour. `excitation` Start from every joint parameter of the calibration level, drop exact dependencies on the measured postures, then remove, one at a time, the parameter whose predicted standard error is largest relative to its expected size, while that ratio exceeds `excitation_k`. Kept parameters are estimated freely. `map` Estimate every joint parameter with a zero-mean Gaussian prior of the given expected sizes (`priors`). Weakly excited directions stay near nominal; nothing is dropped. `map_cv` `map` with the prior sizes scaled by a factor chosen by k-fold cross-validation over the training postures (optionally one factor per parameter group). Needs no robot-specific sizes. `cv_subset` Nested parameter sets in the `excitation` removal order; the set size is chosen by k-fold cross-validation over the training postures.

The base and tool frames are always estimated freely (no prior). Noise is `noise_std` in the units of the cost function's measurement residuals (metres for position unless the robot's cost function weights them), or, when `None`, estimated from a free fit of the identifiable parameters. Selection runs once, in :meth:`BaseCalibration.create_param_list`; the choices and diagnostics are stored in `calib_config["estimation_report"]`. See the guide `docs/source/concepts/calibration_estimation.md`.

## `EstimationError`

Bases: `ValueError`

Invalid estimation settings or data for the chosen method.

## `settings(calib_config)`

`calib_config["estimation"]` merged over :data:`DEFAULTS`.

Source code in `src/figaroh/calibration/estimation.py`

```
def settings(calib_config: dict) -> dict:
    """``calib_config["estimation"]`` merged over :data:`DEFAULTS`."""
    user = dict(calib_config.get("estimation") or {})
    out = dict(DEFAULTS, **user)
    out["priors"] = dict(DEFAULT_PRIORS, **(user.get("priors") or {}))
    if out["method"] not in METHODS:
        raise EstimationError(
            f"estimation.method '{out['method']}' not in {', '.join(METHODS)}"
        )
    unknown = set(out["priors"]) - set(GROUPS)
    if unknown:
        raise EstimationError(f"unknown prior groups {sorted(unknown)}")
    if out["cv_rule"] not in ("min", "one_se"):
        raise EstimationError("estimation.cv_rule must be 'min' or 'one_se'")
    return out
```

## `joint_candidates(model, calib_config)`

Every joint parameter of the calibration level, in joint order.

Source code in `src/figaroh/calibration/estimation.py`

```
def joint_candidates(model, calib_config: dict) -> List[str]:
    """Every joint parameter of the calibration level, in joint order."""
    act = [model.names[j] for j in calib_config["actJoint_idx"]]
    if calib_config["calib_model"] == "full_params":
        return list(get_fullparam_offset(act))
    names = list(get_joint_offset(model, act))  # covers every model joint
    by_joint = {n.split("_", 1)[1]: n for n in names}
    return [by_joint[j] for j in act if j in by_joint]
```

## `prior_group(name, model)`

Prior group of a parameter name; `None` for frame parameters.

Source code in `src/figaroh/calibration/estimation.py`

```
def prior_group(name: str, model) -> Optional[str]:
    """Prior group of a parameter name; ``None`` for frame parameters."""
    if name.startswith("base_") or "EE" in name:
        return None
    if name.startswith("d_"):
        _, kind, joint = name.split("_", 2)
        axis = _joint_axis(model, joint)
        if axis == ("R", kind[-1]) and kind.startswith("phi"):
            return "joint_offset"
        if axis == ("P", kind[-1]) and kind.startswith("p"):
            return "prismatic_offset"
        return "rotation" if kind.startswith("phi") else "translation"
    if name.startswith("offsetP"):
        return "prismatic_offset"
    return "joint_offset"
```

## `prior_std(names, model, priors)`

Expected size per parameter; `inf` for frame parameters.

Source code in `src/figaroh/calibration/estimation.py`

```
def prior_std(names: Sequence[str], model, priors: Dict[str, float]) -> np.ndarray:
    """Expected size per parameter; ``inf`` for frame parameters."""
    out = []
    for n in names:
        group = prior_group(n, model)
        out.append(np.inf if group is None else float(priors[group]))
    return np.array(out)
```

## `identifiable(calib, names, frames, tol)`

Drop exact dependencies on the measured postures (frames kept).

Source code in `src/figaroh/calibration/estimation.py`

```
def identifiable(calib, names, frames, tol) -> List[str]:
    """Drop exact dependencies on the measured postures (frames kept)."""
    cfg = calib.calib_config
    x0 = _start(calib, names)
    J = measurement_jacobian(
        calib.model, calib.data, x0, calib.q_measured, dict(cfg, param_name=names)
    )
    kept, _ = select_identifiable_parameters(J, names, frames, tol=tol)
    return kept
```

## `excitation_order(J, names, frames, prior, noise, k=None)`

Backward elimination by predicted standard error / expected size.

Returns (kept, removed in order, ratio at removal). With `k` None the elimination runs until only frames remain.

Source code in `src/figaroh/calibration/estimation.py`

```
def excitation_order(J, names, frames, prior, noise, k=None):
    """Backward elimination by predicted standard error / expected size.

    Returns (kept, removed in order, ratio at removal). With ``k`` None the
    elimination runs until only frames remain.
    """
    active = list(names)
    removed, ratios = [], []
    while True:
        cols = [names.index(n) for n in active]
        Ja = J[:, cols]
        cov = noise**2 * np.linalg.inv(Ja.T @ Ja)
        ratio = np.sqrt(np.abs(np.diag(cov))) / prior[cols]
        joint = [i for i, n in enumerate(active) if n not in frames]
        if not joint:
            break
        worst = max(joint, key=lambda i: (ratio[i], -i))
        if k is not None and ratio[worst] <= k:
            break
        removed.append(active[worst])
        ratios.append(float(ratio[worst]))
        active.pop(worst)
    return active, removed, ratios
```

## `configure(calib)`

Set `param_name` and prior weights for a non-structural method.

Source code in `src/figaroh/calibration/estimation.py`

```
def configure(calib) -> dict:
    """Set ``param_name`` and prior weights for a non-structural method."""
    cfg = calib.calib_config
    est = settings(cfg)
    if cfg.get("non_geom"):
        raise EstimationError(
            f"estimation.method '{est['method']}' does not support "
            "include_non_geometric; use 'structural'"
        )
    if not (hasattr(calib, "q_measured") and hasattr(calib, "PEE_measured")):
        raise EstimationError(
            f"estimation.method '{est['method']}' selects on the measured "
            "postures: load the data before create_param_list()"
        )
    model = calib.model
    candidates = joint_candidates(model, cfg)
    base = [] if cfg["known_baseframe"] else list(BASE_TPL)
    cfg["param_name"] = base + candidates
    if not cfg["known_tipframe"]:
        add_pee_name(cfg)
    all_names = list(cfg["param_name"])
    for key in _BASE_MAPPING_KEYS:
        cfg.pop(key, None)
    frames = calib._frame_param_names()
    prior = prior_std(all_names, model, est["priors"])
    prior_of = dict(zip(all_names, prior))

    ident = identifiable(calib, all_names, frames, est["dependency_tol"])
    prelim = _fit(calib, ident)
    n_meas = len(calib.PEE_measured)
    noise, noise_source = est["noise_std"], "given"
    if noise is None:
        dof = n_meas - len(ident)
        if dof <= 0:
            raise EstimationError(
                "too few measurements to estimate the noise; set estimation.noise_std"
            )
        noise = float(np.sqrt(np.sum(prelim.fun[:n_meas] ** 2) / dof))
        noise_source = "estimated (free fit of the identifiable parameters)"
    report = {
        "method": est["method"],
        "candidates": candidates,
        "dependent": [n for n in all_names if n not in ident],
        "noise_std": noise,
        "noise_source": noise_source,
        "priors": est["priors"],
    }

    def weights(names, scale=None):
        scale = scale or {}
        s = np.array(
            [prior_of[n] * scale.get(prior_group(n, model), 1.0) for n in names]
        )
        return np.where(np.isfinite(s), noise / s, 0.0)

    method = est["method"]
    final, w = None, None
    if method in ("excitation", "cv_subset"):
        J = prelim.jac[:n_meas]
        prior_ident = np.array([prior_of[n] for n in ident])
        k = est["excitation_k"] if method == "excitation" else None
        kept, removed, ratios = excitation_order(
            J, ident, frames, prior_ident, noise, k
        )
        report["removal_order"] = list(zip(removed, ratios))
        if method == "excitation":
            report["excitation_k"] = est["excitation_k"]
            final = kept
        else:
            final = _choose_subset(calib, ident, removed, est, report)
    elif method == "map":
        final, w = all_names, weights(all_names)
    elif method == "map_cv":
        scale = _choose_scale(calib, all_names, weights, est, report)
        report["prior_scale"] = scale
        final, w = all_names, weights(all_names, scale)

    cfg["param_name"] = _canonical(all_names, final)
    cfg["prior_weights"] = None if w is None else w
    cfg["prior_noise"] = None if w is None else noise
    cfg["absorbed_param_name"] = report["dependent"]
    report["param_name"] = list(cfg["param_name"])
    cfg["estimation_report"] = report
    calib.estimation_report = report
    logger.info(
        "estimation '%s': %d parameters (noise %.3g, %s)",
        method,
        len(cfg["param_name"]),
        noise,
        noise_source,
    )
    return report
```

## `lift_structural(calib, priors=None, refine_iterations=3)`

Weighted minimum-norm lift of fitted base parameters to joint corrections.

The `structural` method estimates one representative per group of dependent joint parameters; which one is a numerical accident. Among all joint corrections that reproduce the fit, return the most plausible under the expected sizes `priors` (default: the estimation priors):

```
theta = argmin theta' P^-1 theta  subject to  M_full theta = t
```

`M_full` is the base mapping before any row was dropped; `t` holds the fitted value of every kept joint row, and 0 on the rows the base frame carries (`base_frame_row_names`) and on the rows the fit dropped (held at 0). The result does not depend on which representatives were chosen.

`M` is a first-order (structural) map, so with corrections of tens of mrad the lifted model drifts from the fit by up to ~0.1 mm. When the measured postures are available, `refine_iterations` Gauss-Newton steps then make the lifted model reproduce the fitted predictions at those postures, each step taking the smallest correction in the same prior-weighted norm.

Returns:

| Type                  | Description                                                     |
| --------------------- | --------------------------------------------------------------- |
| `(names, theta, cov)` | joint parameter names                                           |
|                       | (base_mapping_param_names), lifted values, and their covariance |
|                       | conditional on this lift (propagated from the fitted base       |
|                       | parameters; directions the data does not see have zero variance |
|                       | here, not their prior).                                         |

Source code in `src/figaroh/calibration/estimation.py`

```
def lift_structural(
    calib, priors: Optional[Dict[str, float]] = None, refine_iterations: int = 3
):
    """Weighted minimum-norm lift of fitted base parameters to joint corrections.

    The ``structural`` method estimates one representative per group of
    dependent joint parameters; which one is a numerical accident. Among all
    joint corrections that reproduce the fit, return the most plausible
    under the expected sizes ``priors`` (default: the estimation priors):

        theta = argmin theta' P^-1 theta  subject to  M_full theta = t

    ``M_full`` is the base mapping before any row was dropped; ``t`` holds
    the fitted value of every kept joint row, and 0 on the rows the base
    frame carries (``base_frame_row_names``) and on the rows the fit
    dropped (held at 0). The result does not depend on which
    representatives were chosen.

    ``M`` is a first-order (structural) map, so with corrections of tens of
    mrad the lifted model drifts from the fit by up to ~0.1 mm. When the
    measured postures are available, ``refine_iterations`` Gauss-Newton
    steps then make the lifted model reproduce the fitted predictions at
    those postures, each step taking the smallest correction in the same
    prior-weighted norm.

    Returns:
        (names, theta, cov): joint parameter names
        (``base_mapping_param_names``), lifted values, and their covariance
        conditional on this lift (propagated from the fitted base
        parameters; directions the data does not see have zero variance
        here, not their prior).
    """
    cfg = calib.calib_config
    M = np.asarray(cfg.get("base_mapping_matrix_full", cfg["base_mapping_matrix"]))
    rows_full = list(
        cfg.get("base_mapping_row_names_full", cfg["base_mapping_row_names"])
    )
    kept = list(cfg["base_mapping_row_names"])
    frame_rows = set(cfg.get("base_frame_row_names", []))
    names = list(cfg["base_mapping_param_names"])
    start, _ = cfg["base_mapping_slice"]
    x = np.asarray(calib.var_)
    C = np.asarray(calib._C_param)

    free = [
        (i, start + kept.index(r))
        for i, r in enumerate(rows_full)
        if r in kept and r not in frame_rows
    ]
    t = np.zeros(len(rows_full))
    for i, k in free:
        t[i] = x[k]

    sizes = (
        settings(cfg)["priors"] if priors is None else dict(DEFAULT_PRIORS, **priors)
    )
    P = prior_std(names, getattr(calib, "model", None), sizes) ** 2
    G = (P[:, None] * M.T) @ np.linalg.inv((M * P) @ M.T)
    theta = G @ t
    if refine_iterations and hasattr(calib, "q_measured"):
        frames = set(calib._frame_param_names())
        other = [
            n
            for n in cfg["param_name"]
            if n not in frames and not n.startswith(("d_", "offset"))
        ]
        if other:
            # e.g. contact-plane parameters: core's FK cannot evaluate the fit
            logger.info(
                "lift not refined (first order only): parameters %s are not "
                "joint or frame parameters",
                other[:3],
            )
        else:
            theta = _refine_lift(calib, names, theta, np.sqrt(P), refine_iterations)
    rows_i = [i for i, _ in free]
    var_k = [k for _, k in free]
    Gs = G[:, rows_i]
    cov = Gs @ C[np.ix_(var_k, var_k)] @ Gs.T
    return names, theta, cov
```

## 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

## `regularization_coefficient(value)`

`coeff_regularize` from a config value; a non-zero value is an error.

The coefficient weighted `sqrt(c) * theta` rows that robot cost functions appended for every joint parameter, so a metre and a radian were penalised alike (#120). It was deprecated in 0.6.0 and removed in 1.0: `estimation.method: map` replaces it with priors of physical size per parameter group. Zero or absent is accepted and passed through.

Raises:

| Type                 | Description        |
| -------------------- | ------------------ |
| `ConfigurationError` | value is non-zero. |

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

```
def regularization_coefficient(value):
    """``coeff_regularize`` from a config value; a non-zero value is an error.

    The coefficient weighted ``sqrt(c) * theta`` rows that robot cost
    functions appended for every joint parameter, so a metre and a radian
    were penalised alike (#120). It was deprecated in 0.6.0 and removed in
    1.0: ``estimation.method: map`` replaces it with priors of physical size
    per parameter group. Zero or absent is accepted and passed through.

    Raises:
        ConfigurationError: ``value`` is non-zero.
    """
    if value:
        raise ConfigurationError(
            f"regularization_coefficient ({value}) was removed in 1.0: it "
            "penalised metres and radians alike. Remove it (or set it to 0) "
            "and use estimation.method: map (priors per parameter group, "
            "docs/source/concepts/calibration_estimation.md; figaroh-plus#120)."
        )
    return value
```

## `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
```

## `unified_to_legacy_config(robot, unified_calib_config)`

Convert unified configuration format to legacy calib_config format.

Maps the unified configuration structure to the `calib_config` dict the calibration classes consume.

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` | Calibration configuration dict (calib_config) |

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 unified configuration structure to the ``calib_config`` dict the
    calibration classes consume.

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

    Returns:
        dict: Calibration configuration dict (``calib_config``)

    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)

    # 7b. Extract eye-hand (camera) calibration frames, if enabled
    _extract_eye_hand_params(calib_config, unified_calib_config)

    # 8. Extract data configuration
    calib_config["NbSample"] = data.get("number_of_samples", 500)
    calib_config["data_file"] = data.get("source_file")
    # held-out measurements in the same format; "" means none
    calib_config["validation_data_file"] = data.get("validation_data_file") or None
    calib_config["sample_configs_file"] = data.get("sample_configurations_file")

    return calib_config
```

## 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"]
        # base_mapping_slice is positional: prepending shifts it
        if "base_mapping_slice" in calib_config:
            start, end = calib_config["base_mapping_slice"]
            calib_config["base_mapping_slice"] = (
                start + len(BASE_TPL),
                end + len(BASE_TPL),
            )
```

## `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)

    # a missing (occluded) point would make every residual NaN: name the rows
    bad = ~np.isfinite(pose_ee).all(axis=1)
    if bad.any():
        rows = np.delete(np.arange(len(df)), del_list or [])[bad]
        raise ValueError(
            f"{path_to_file}: non-finite marker measurements in CSV rows "
            f"{rows.tolist()} (e.g. an occluded point); exclude them with "
            "del_list or clean the file"
        )

    # 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"]):
        # a copy: q0 is robot.q0, which must not end up as the last sample (#125)
        config = calib_config["q0"].copy()
        config[calib_config["config_idx"]] = q_act[i, :]
        q_exp[i, :] = config

    return PEEm_exp, q_exp
```

# Data contract

Typed inputs that carry their conventions: joint order, clock, signal origin, effort kind and unit, valid-sample mask, session and source files. See the decision record [Data and result contract](https://github.com/thanhndv212/figaroh-plus/blob/devel/docs/decisions/data-result-contract.md). Each type converts to and from the legacy dictionaries and CSV loader.

Dynamic identification input: a trajectory with its conventions attached.

See docs/decisions/data-result-contract.md. Joint order, clock, signal origin, effort kind and unit, the valid-sample mask and the source travel with the arrays instead of being implied by an adapter.

The effort keeps the units it was recorded in (a motor current, a motor-side torque, a load fraction, a joint torque, or a linear actuator's force). :meth:`TrajectoryData.check_effort` decides whether a solver may use it.

## `TrajectoryData(t, joint_names, q, effort, effort_kind, effort_unit, clock='recorded', dq=None, ddq=None, origin=dict(), effort_conversion='', effort_raw=None, effort_raw_kind=(), effort_raw_unit=(), mask=None, sample_index=None, source=DataSource())`

A recorded trajectory, in the order of `joint_names`.

Arrays are (n_samples, n_joints); `t` is (n_samples,) in seconds. `dq`/`ddq` may be `None` (to be derived). `mask` marks valid samples; nothing is deleted, and filtering runs on the full signal before the mask selects rows (:meth:`stacked_mask`).

### `from_legacy(raw, joint_names, *, effort_kind, effort_unit, clock='recorded', source=None, origin=None, sample_index=None)`

From the dict `load_trajectory_data` returns today.

Keys `timestamps` ((n, 1) or (n,)), `positions`, `velocities`, `accelerations`, `torques`; velocities/accelerations may be `None`. The legacy dict says nothing about the effort, so its kind and unit are required here.

Source code in `src/figaroh/data/trajectory.py`

```
@classmethod
def from_legacy(
    cls,
    raw: dict,
    joint_names: Sequence[str],
    *,
    effort_kind: Union[str, Sequence[str]],
    effort_unit: Union[str, Sequence[str]],
    clock: str = "recorded",
    source: Optional[DataSource] = None,
    origin: Optional[Dict[str, str]] = None,
    sample_index: Optional[np.ndarray] = None,
) -> "TrajectoryData":
    """From the dict ``load_trajectory_data`` returns today.

    Keys ``timestamps`` ((n, 1) or (n,)), ``positions``, ``velocities``,
    ``accelerations``, ``torques``; velocities/accelerations may be
    ``None``. The legacy dict says nothing about the effort, so its kind
    and unit are required here.
    """
    return cls(
        t=raw["timestamps"],
        joint_names=tuple(joint_names),
        q=raw["positions"],
        dq=raw.get("velocities"),
        ddq=raw.get("accelerations"),
        effort=raw["torques"],
        effort_kind=effort_kind,
        effort_unit=effort_unit,
        clock=clock,
        origin=dict(origin or {}),
        sample_index=sample_index,
        source=source or DataSource(),
    )
```

### `to_legacy()`

The legacy dict, every sample (the mask applies to regressor rows, after filtering; see :meth:`stacked_mask`).

Source code in `src/figaroh/data/trajectory.py`

```
def to_legacy(self) -> dict:
    """The legacy dict, every sample (the mask applies to regressor rows,
    after filtering; see :meth:`stacked_mask`)."""
    return {
        "timestamps": self.t.reshape(-1, 1),
        "positions": self.q,
        "velocities": self.dq,
        "accelerations": self.ddq,
        "torques": self.effort,
    }
```

### `stacked_effort(effort=None)`

Joint-major stacking the regressor uses: joint 1's samples, then joint 2's, ... (`effort.T.flatten()`).

Source code in `src/figaroh/data/trajectory.py`

```
def stacked_effort(self, effort: Optional[np.ndarray] = None) -> np.ndarray:
    """Joint-major stacking the regressor uses: joint 1's samples, then
    joint 2's, ... (``effort.T.flatten()``)."""
    effort = self.effort if effort is None else np.asarray(effort)
    return effort.T.flatten()
```

### `stacked_mask()`

Row mask matching :meth:`stacked_effort`.

Source code in `src/figaroh/data/trajectory.py`

```
def stacked_mask(self) -> np.ndarray:
    """Row mask matching :meth:`stacked_effort`."""
    return np.tile(self.mask, len(self.joint_names))
```

### `converted(scale, offset=0.0, *, effort_kind, effort_unit, description)`

`effort * scale + offset` per joint, keeping the recorded signal.

For a conversion with known constants (reduction ratio, torque constant, gravity offset); `description` records the steps.

Source code in `src/figaroh/data/trajectory.py`

```
def converted(
    self,
    scale: Union[float, Sequence[float]],
    offset: Union[float, Sequence[float]] = 0.0,
    *,
    effort_kind: Union[str, Sequence[str]],
    effort_unit: Union[str, Sequence[str]],
    description: str,
) -> "TrajectoryData":
    """``effort * scale + offset`` per joint, keeping the recorded signal.

    For a conversion with known constants (reduction ratio, torque
    constant, gravity offset); ``description`` records the steps.
    """
    scale = np.broadcast_to(
        np.asarray(scale, dtype=float), (len(self.joint_names),)
    )
    offset = np.broadcast_to(
        np.asarray(offset, dtype=float), (len(self.joint_names),)
    )
    raw = self.effort if self.effort_raw is None else self.effort_raw
    raw_kind = self.effort_kind if self.effort_raw is None else self.effort_raw_kind
    raw_unit = self.effort_unit if self.effort_raw is None else self.effort_raw_unit
    return replace(
        self,
        effort=self.effort * scale + offset,
        effort_kind=effort_kind,
        effort_unit=effort_unit,
        effort_conversion=(
            f"{self.effort_conversion}; {description}"
            if self.effort_conversion
            else description
        ),
        effort_raw=raw,
        effort_raw_kind=raw_kind,
        effort_raw_unit=raw_unit,
    )
```

### `check_effort(model, drive_gain_joints=())`

Refuse an effort a dynamic solver must not use as joint effort.

Accepted per joint: `joint_torque` (N·m) on a revolute joint, `joint_force` (N) on a prismatic joint, or a drive-side kind (motor current/torque, load fraction) on a joint listed in `drive_gain_joints`, i.e. whose model identifies an unknown drive gain. Anything else would be solved as if it were N·m.

Raises:

| Type         | Description                            |
| ------------ | -------------------------------------- |
| `ValueError` | naming every joint, its kind and unit. |

Source code in `src/figaroh/data/trajectory.py`

```
def check_effort(self, model, drive_gain_joints: Sequence[str] = ()) -> None:
    """Refuse an effort a dynamic solver must not use as joint effort.

    Accepted per joint: ``joint_torque`` (N·m) on a revolute joint,
    ``joint_force`` (N) on a prismatic joint, or a drive-side kind
    (motor current/torque, load fraction) on a joint listed in
    ``drive_gain_joints``, i.e. whose model identifies an unknown drive
    gain. Anything else would be solved as if it were N·m.

    Raises:
        ValueError: naming every joint, its kind and unit.
    """
    gains = set(drive_gain_joints)
    problems = []
    for name, kind, unit in zip(
        self.joint_names, self.effort_kind, self.effort_unit
    ):
        if not model.existJointName(name):
            problems.append(f"{name}: not in the model")
            continue
        short = model.joints[model.getJointId(name)].shortname()
        revolute = short.startswith("JointModelR")
        prismatic = short.startswith("JointModelP")
        if kind in DRIVE_KINDS and name in gains:
            continue
        if kind == JOINT_TORQUE and revolute and unit in _UNITS[JOINT_TORQUE]:
            continue
        if kind == JOINT_FORCE and prismatic and unit in _UNITS[JOINT_FORCE]:
            continue
        joint_type = "revolute" if revolute else "prismatic" if prismatic else short
        hint = (
            " (declare a drive gain for this joint, or convert it)"
            if kind in DRIVE_KINDS
            else ""
        )
        problems.append(f"{name} ({joint_type}): {kind} in {unit}{hint}")
    if problems:
        raise ValueError("Effort is not joint effort for: " + "; ".join(problems))
```

Geometric calibration input: postures and what was observed at them.

See docs/decisions/data-result-contract.md. Kept separate from :class:`~figaroh.data.trajectory.TrajectoryData`: postures are independent samples with points or poses in a named frame, not a time series of efforts.

## `PoseObservations(joint_names, q, values=None, point_names=(), measurability=None, frame='', registered_to='', orientation='unspecified', constraint=None, mask=None, session=None, sample_index=None, source=DataSource())`

Postures `q` (n, n_joints) and observations `values`.

`values` is (n, n_points, 6): x, y, z in m and phix, phiy, phiz in rad (convention `orientation`) of each observed point, NaN where a DOF is not observed (`measurability`, (n_points, 6)). `frame` names the frame the values are expressed in and `registered_to` the robot frame it is registered to. A constraint-only dataset (e.g. a contact gap that must be zero) has `values=None` and names its `constraint`. `mask` marks valid postures; nothing is deleted.

### `select_points(names)`

The same postures with only the named points (#131).

E.g. a file with four tracked points, of which a calibration fits one: the adapter keeps all four and the calibration selects.

Source code in `src/figaroh/data/observations.py`

```
def select_points(self, names: Sequence[str]) -> "PoseObservations":
    """The same postures with only the named points (#131).

    E.g. a file with four tracked points, of which a calibration fits
    one: the adapter keeps all four and the calibration selects.
    """
    if self.values is None:
        raise ValueError("constraint-only observations have no points")
    missing = [p for p in names if p not in self.point_names]
    if missing:
        raise KeyError(f"points {missing} not in {list(self.point_names)}")
    idx = [self.point_names.index(p) for p in names]
    return replace(
        self,
        values=self.values[:, idx],
        point_names=tuple(names),
        measurability=self.measurability[idx],
    )
```

### `from_csv(path, model, calib_config, *, del_list=(), frame='', registered_to='', orientation='unspecified', source=None)`

Read the CSV layout `calibration.data_loader.load_data` reads.

Columns `x1, y1, ... phiz1` per marker (the measured DOF only) and one column per active joint. `del_list` rows are masked, not deleted. A `session_id` column, if present, gives the sessions. Does not modify `calib_config`.

Raises:

| Type       | Description                   |
| ---------- | ----------------------------- |
| `KeyError` | listing every missing column. |

Source code in `src/figaroh/data/observations.py`

```
@classmethod
def from_csv(
    cls,
    path: Union[str, Path],
    model,
    calib_config: dict,
    *,
    del_list: Sequence[int] = (),
    frame: str = "",
    registered_to: str = "",
    orientation: str = "unspecified",
    source: Optional[DataSource] = None,
) -> "PoseObservations":
    """Read the CSV layout ``calibration.data_loader.load_data`` reads.

    Columns ``x1, y1, ... phiz1`` per marker (the measured DOF only) and
    one column per active joint. ``del_list`` rows are masked, not
    deleted. A ``session_id`` column, if present, gives the sessions.
    Does not modify ``calib_config``.

    Raises:
        KeyError: listing every missing column.
    """
    df = pd.read_csv(path)
    n_points = int(calib_config["NbMarkers"])
    meas = np.array(calib_config["measurability"], dtype=bool)
    joints = [model.names[i] for i in calib_config["actJoint_idx"]]
    columns = [
        f"{DOF[d]}{i + 1}" for i in range(n_points) for d in range(6) if meas[d]
    ]
    missing = [c for c in columns + joints if c not in df.columns]
    if missing:
        raise KeyError(f"{path}: missing columns {missing}")
    n = len(df)
    values = np.full((n, n_points, 6), np.nan)
    for i in range(n_points):
        for d in range(6):
            if meas[d]:
                values[:, i, d] = df[f"{DOF[d]}{i + 1}"].to_numpy(dtype=float)
    mask = np.ones(n, bool)
    mask[list(del_list)] = False
    session = (
        df["session_id"].astype(str).to_numpy(dtype=object)
        if ("session_id" in df.columns)
        else None
    )
    if source is None:
        source = DataSource.from_files([path], adapter="PoseObservations.from_csv")
    return cls(
        joint_names=tuple(joints),
        q=df[joints].to_numpy(dtype=float),
        values=values,
        point_names=tuple(str(i + 1) for i in range(n_points)),
        measurability=np.tile(meas, (n_points, 1)),
        frame=frame,
        registered_to=registered_to,
        orientation=orientation,
        mask=mask,
        session=session,
        sample_index=df.index.to_numpy(),
        source=source,
    )
```

### `to_legacy(model, calib_config)`

`(PEE_measured, q_measured)` exactly as `load_data` returns them.

Valid (unmasked) postures only. `PEE_measured` is component-major (`x1` over samples, then `y1`, ...); `q_measured` is the full configuration, `calib_config["q0"]` with the active joints set. Does not modify `calib_config` or `q0`; the sample count is `len(q_measured)`.

Source code in `src/figaroh/data/observations.py`

```
def to_legacy(self, model, calib_config: dict) -> Tuple[np.ndarray, np.ndarray]:
    """``(PEE_measured, q_measured)`` exactly as ``load_data`` returns them.

    Valid (unmasked) postures only. ``PEE_measured`` is component-major
    (``x1`` over samples, then ``y1``, ...); ``q_measured`` is the full
    configuration, ``calib_config["q0"]`` with the active joints set.
    Does not modify ``calib_config`` or ``q0``; the sample count is
    ``len(q_measured)``.
    """
    if self.values is None:
        raise ValueError(
            f"constraint-only observations ({self.constraint}) have no "
            "measured values"
        )
    joints = [model.names[i] for i in calib_config["actJoint_idx"]]
    if list(self.joint_names) != joints:
        raise ValueError(
            f"joint order {list(self.joint_names)} differs from the "
            f"calibration's active joints {joints}"
        )
    keep = self.mask
    columns = [
        self.values[keep, i, d]
        for i in range(len(self.point_names))
        for d in range(6)
        if self.measurability[i, d]
    ]
    pee = np.vstack(columns).flatten("C") if columns else np.zeros(0)
    q = np.tile(np.array(calib_config["q0"], dtype=float), (int(keep.sum()), 1))
    q[:, calib_config["config_idx"]] = self.q[keep]
    return pee, q
```

Where a dataset came from (docs/decisions/data-result-contract.md).

## `Session(id, date=None)`

A recording's identity: what it is, not how a study uses it.

Roles (training, validation, ...) belong to a :class:`~figaroh.data.protocol.Protocol`, not here.

## `DataSource(files=dict(), adapter='', session=None, notes='')`

Files (with sha256), the adapter that read them, and the session.

### `from_files(paths, adapter='', session=None, notes='')`

Hash the given files now.

Source code in `src/figaroh/data/source.py`

```
@classmethod
def from_files(
    cls,
    paths: Iterable[Union[str, Path]],
    adapter: str = "",
    session: Optional[Session] = None,
    notes: str = "",
) -> "DataSource":
    """Hash the given files now."""
    files = {str(Path(p)): file_sha256(p) for p in paths}
    return cls(files=files, adapter=adapter, session=session, notes=notes)
```

## `file_sha256(path)`

sha256 of a file's bytes.

Source code in `src/figaroh/data/source.py`

```
def file_sha256(path: Union[str, Path]) -> str:
    """sha256 of a file's bytes."""
    digest = hashlib.sha256()
    with open(path, "rb") as f:
        for block in iter(lambda: f.read(1 << 20), b""):
            digest.update(block)
    return digest.hexdigest()
```

Which session plays which role in a study (data-result-contract.md).

A session's identity is part of the data (:class:`~figaroh.data.source. Session`); its role is not: the same recording can validate one study and train another. A `Protocol` assigns roles and names each session's files by sha256, so a frozen protocol cannot silently change under it.

YAML layout::

```
name: tiago-mocap-heldout
version: 1
sessions:
  - id: 2021-11-30-1544
    role: training
    files:
      data/qualisys_2021-11-30_static_postures.csv: b6c0051e...
```

## `Protocol(name, version, sessions)`

### `verify(root=None)`

Check every session file against its recorded sha256.

Paths are relative to `root` (default: the working directory).

Raises:

| Type         | Description                            |
| ------------ | -------------------------------------- |
| `ValueError` | listing every missing or changed file. |

Source code in `src/figaroh/data/protocol.py`

```
def verify(self, root: Optional[Union[str, Path]] = None) -> None:
    """Check every session file against its recorded sha256.

    Paths are relative to ``root`` (default: the working directory).

    Raises:
        ValueError: listing every missing or changed file.
    """
    root = Path(root) if root is not None else Path.cwd()
    problems = []
    for s in self.sessions:
        for rel, expected in s.files.items():
            path = root / rel
            if not path.exists():
                problems.append(f"{s.id}: {rel} is missing")
            elif file_sha256(path) != expected:
                problems.append(f"{s.id}: {rel} does not match its sha256")
    if problems:
        raise ValueError(
            f"Protocol {self.name} v{self.version}: " + "; ".join(problems)
        )
```

# Identification

The identification module provides tools for dynamic parameter identification of robots, including the [reporting & verification suite](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md) (`print_quality_report()`, `export_html_report()`, `verify()`, `export_verification_report()`) attached directly to `BaseIdentification`.

## 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.).

## `BaseIdentification(robot, config_file='config/robot_config.yaml')`

Bases: `ABC`

Base class for robot dynamic parameter identification.

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

Initialize base identification with robot model and configuration.

Parameters:

| Name          | Type | Description                           | Default                      |
| ------------- | ---- | ------------------------------------- | ---------------------------- |
| `robot`       |      | Robot model loaded with FIGAROH       | *required*                   |
| `config_file` |      | Path to robot configuration YAML file | `'config/robot_config.yaml'` |

Source code in `src/figaroh/identification/base_identification.py`

```
def __init__(self, robot, config_file="config/robot_config.yaml"):
    """Initialize base identification with robot model and configuration.

    Args:
        robot: Robot model loaded with FIGAROH
        config_file: Path to robot configuration YAML file
    """
    self.robot = robot
    self.model = self.robot.model
    self.data = self.robot.data

    # Load configuration initiating self.identif_config
    self.load_param(config_file)

    # Initialize attributes for identification results
    self.dynamic_regressor = None
    self.standard_parameter = None
    self.additional_parameters = None
    self.custom_parameters = None
    self.params_base = None
    self.dynamic_regressor_base = None
    self.phi_base = None
    self.rms_error = None
    self.correlation = None
    self.processed_data = None
    self.result = None
    self.num_samples = None
    self.tau_ref = None
    self.tau_identif = None
    self.tau_noised = None

    # Held-out validation dataset (separate file, never a split of the
    # training data). Populated by _load_validation_data() if
    # identif_config["validation_data_file"] is set.
    self._val_available = False
    self._val_processed_data = None
    self._val_num_samples = None

    # Data contract (#55): the TrajectoryData an adapter returned, if
    # any, its valid-sample masks, and the stage records of this run
    self.trajectory = None
    self._sample_mask = None
    self._val_trajectory = None
    self._val_sample_mask = None
    self.stages = []

    # Diagnostics captured during solve() for validation / reporting:
    # column indices eliminated by _eliminate_zero_columns() and the
    # base-column selection from the QR decomposition, both needed to
    # evaluate phi_base against a regressor built from new (held-out)
    # trajectory data.
    self._idx_eliminated = None
    self._base_indices = None
    self.selected = None
    self._decimate_used = False

    # Set default filter configuration, can be overridden in subclasses
    self.filter_config = self.identif_config.get(
        "filter_config",
        {
            "differentiation_method": "gradient",
            "filter_params": {
                "nbutter": 4,
                "f_butter": 2,
                "med_fil": 5,
                "f_sample": 100,
            },
        },
    )
    logger.info(f"{self.__class__.__name__} initialized")
```

### `solve(decimate=True, decimation_factor=10, zero_tolerance=0.001, plotting=True, save_results=False, html_report=False, wls=False)`

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:

| Name                | Type    | Description                                                                                                                                                                                                                                                                            | Default |
| ------------------- | ------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `decimate`          | `bool`  | Whether to apply decimation to reduce data size                                                                                                                                                                                                                                        | `True`  |
| `decimation_factor` | `int`   | Factor for signal decimation (default: 10)                                                                                                                                                                                                                                             | `10`    |
| `zero_tolerance`    | `float` | Tolerance for eliminating zero columns                                                                                                                                                                                                                                                 | `0.001` |
| `plotting`          | `bool`  | Whether to generate plots                                                                                                                                                                                                                                                              | `True`  |
| `save_results`      | `bool`  | Whether to save parameters to file                                                                                                                                                                                                                                                     | `False` |
| `html_report`       | `bool`  | If True, also export an HTML diagnostic report (see :meth:export_html_report) after the terminal quality report is printed.                                                                                                                                                            | `False` |
| `wls`               | `bool`  | If True, refine the OLS base-parameter estimate with iteratively-weighted least squares (Gautier, 1997) before computing quality metrics — see :meth:\_apply_weighted_least_squares. Relative standard deviations from the WLS fit are stored under self.result["wls_std_deviations"]. | `False` |

Returns:

| Name      | Type | Description              |
| --------- | ---- | ------------------------ |
| `ndarray` |      | Base parameters phi_base |

Raises:

| Type             | Description                                                      |
| ---------------- | ---------------------------------------------------------------- |
| `AssertionError` | If prerequisites not met (dynamic_regressor, standard_parameter) |
| `ValueError`     | If data shapes are incompatible                                  |
| `LinAlgError`    | If QR decomposition fails                                        |

Source code in `src/figaroh/identification/base_identification.py`

```
def solve(
    self,
    decimate=True,
    decimation_factor=10,
    zero_tolerance=0.001,
    plotting=True,
    save_results=False,
    html_report=False,
    wls=False,
):
    """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.

    Args:
        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
        html_report (bool): If True, also export an HTML diagnostic
            report (see :meth:`export_html_report`) after the terminal
            quality report is printed.
        wls (bool): If True, refine the OLS base-parameter estimate with
            iteratively-weighted least squares (Gautier, 1997) before
            computing quality metrics — see
            :meth:`_apply_weighted_least_squares`. Relative standard
            deviations from the WLS fit are stored under
            ``self.result["wls_std_deviations"]``.

    Returns:
        ndarray: Base parameters phi_base

    Raises:
        AssertionError: If prerequisites not met (dynamic_regressor, standard_parameter)
        ValueError: If data shapes are incompatible
        np.linalg.LinAlgError: If QR decomposition fails
    """
    logger.info(
        f"Starting {self.__class__.__name__} dynamic parameter identification..."
    )

    self._run_started_at = datetime.now(timezone.utc).isoformat()
    self._decimate_used = decimate

    # Validate prerequisites
    self._validate_prerequisites()

    # Step 1: Eliminate zero columns
    regressor_reduced, active_params = self._eliminate_zero_columns(zero_tolerance)

    # Step 2: Apply decimation if requested
    if decimate:
        tau_processed, W_processed = self._apply_decimation(
            regressor_reduced, decimation_factor
        )
    else:
        tau_processed, W_processed = self._prepare_undecimated_data(
            regressor_reduced
        )

    # Step 2b: leave out masked samples (after filtering/decimation)
    tau_processed, W_processed = self._mask_rows(
        tau_processed, W_processed, decimation_factor if decimate else 1
    )

    # Step 2c: keep only the joints whose effort enters the fit
    tau_processed, W_processed = self._select_fit_rows(tau_processed, W_processed)

    self._reset_solve_state(tau_processed, W_processed, active_params)

    # Step 3: Calculate base parameters
    from figaroh.tools.stages import record_stage

    try:
        results = self._calculate_base_parameters(
            tau_processed, W_processed, active_params
        )
    except Exception as e:
        record_stage(self, "fit", "failed", f"{type(e).__name__}: {e}")
        raise

    # Step 3b: Optional weighted least squares refinement
    wls_std = None
    if wls:
        phi_wls, wls_std = self._apply_weighted_least_squares()
        self.phi_base = phi_wls
        results["phi_base"] = phi_wls
        results["tau_estimated"] = self.dynamic_regressor_base @ phi_wls
        self.tau_identif = results["tau_estimated"]

    # Step 4: Store results and compute quality metrics
    self._run_finished_at = datetime.now(timezone.utc).isoformat()
    self._compute_quality_metrics()
    record_stage(
        self,
        "fit",
        "ok",
        "weighted least squares" if wls else "least squares",
        {
            "effort_rmse": (float(self.rms_error), "N·m or N (stacked joints)"),
            "rows": len(self.tau_noised),
            "base_parameters": len(self.phi_base),
        },
    )
    self._store_results(results)
    if wls_std is not None:
        self.result["wls_std_deviations"] = wls_std

    # Print quality report
    self.print_quality_report()

    # Step 5: Optional plotting
    if plotting:
        self.plot_results()

    # Step 6: Optional parameter saving
    if save_results:
        self.save_results()

    # Step 7: Optional HTML diagnostic report
    if html_report:
        self.export_html_report()

    return self.phi_base
```

### `solve_with_custom_solver(method='lstsq', regularization=None, alpha=0.0, constraints=None, bounds=None, decimate=False, decimation_factor=10, zero_tolerance=0.001, plotting=False, save_results=False, **solver_kwargs)`

Alternative solving method using advanced linear solver.

This method provides more flexibility than the default QR-based solve(), offering multiple solving methods, regularization, and constraints.

Parameters:

| Name                | Type    | Description                                                     | Default   |
| ------------------- | ------- | --------------------------------------------------------------- | --------- |
| `method`            | `str`   | Solving method ('lstsq', 'ridge', 'lasso', 'constrained', etc.) | `'lstsq'` |
| `regularization`    | `str`   | Regularization type ('l1', 'l2', 'elastic_net')                 | `None`    |
| `alpha`             | `float` | Regularization strength                                         | `0.0`     |
| `constraints`       | `dict`  | Linear constraints                                              | `None`    |
| `bounds`            | `tuple` | Box constraints on parameters                                   | `None`    |
| `decimate`          | `bool`  | Whether to apply decimation                                     | `False`   |
| `decimation_factor` | `int`   | Decimation factor if decimate=True                              | `10`      |
| `zero_tolerance`    | `float` | Tolerance for eliminating zero columns                          | `0.001`   |
| `plotting`          | `bool`  | Whether to generate plots                                       | `False`   |
| `save_results`      | `bool`  | Whether to save parameters to file                              | `False`   |
| `**solver_kwargs`   |         | Additional arguments for LinearSolver                           | `{}`      |

Returns:

| Name      | Type | Description                |
| --------- | ---- | -------------------------- |
| `ndarray` |      | Identified base parameters |

Example

> > > #### Ridge regression with L2 regularization
> > >
> > > phi = identification.solve_with_custom_solver( ... method='ridge', alpha=0.01)
> > >
> > > #### Constrained optimization with physical bounds
> > >
> > > bounds = [(0, 100) for _ in range(n_params)] phi = identification.solve_with_custom_solver( ... method='constrained', bounds=bounds)

Source code in `src/figaroh/identification/base_identification.py`

```
def solve_with_custom_solver(
    self,
    method="lstsq",
    regularization=None,
    alpha=0.0,
    constraints=None,
    bounds=None,
    decimate=False,
    decimation_factor=10,
    zero_tolerance=0.001,
    plotting=False,
    save_results=False,
    **solver_kwargs,
):
    """
    Alternative solving method using advanced linear solver.

    This method provides more flexibility than the default QR-based
    solve(), offering multiple solving methods, regularization, and
    constraints.

    Args:
        method (str): Solving method ('lstsq', 'ridge', 'lasso',
            'constrained', etc.)
        regularization (str): Regularization type ('l1', 'l2',
            'elastic_net')
        alpha (float): Regularization strength
        constraints (dict): Linear constraints
        bounds (tuple): Box constraints on parameters
        decimate (bool): Whether to apply decimation
        decimation_factor (int): Decimation factor if decimate=True
        zero_tolerance (float): Tolerance for eliminating zero columns
        plotting (bool): Whether to generate plots
        save_results (bool): Whether to save parameters to file
        **solver_kwargs: Additional arguments for LinearSolver

    Returns:
        ndarray: Identified base parameters

    Example:
        >>> # Ridge regression with L2 regularization
        >>> phi = identification.solve_with_custom_solver(
        ...     method='ridge', alpha=0.01)

        >>> # Constrained optimization with physical bounds
        >>> bounds = [(0, 100) for _ in range(n_params)]
        >>> phi = identification.solve_with_custom_solver(
        ...     method='constrained', bounds=bounds)
    """
    logger.info(
        f"Starting {self.__class__.__name__} identification "
        f"with custom solver..."
    )

    # Validate prerequisites
    self._validate_prerequisites()

    # Step 1: Eliminate zero columns
    regressor_reduced, active_params = self._eliminate_zero_columns(zero_tolerance)

    # Step 2: Apply decimation if requested
    if decimate:
        tau_processed, W_processed = self._apply_decimation(
            regressor_reduced, decimation_factor
        )
    else:
        tau_processed, W_processed = self._prepare_undecimated_data(
            regressor_reduced
        )
    tau_processed, W_processed = self._select_fit_rows(tau_processed, W_processed)

    self._reset_solve_state(tau_processed, W_processed, active_params)

    # Step 3: Solve using custom solver
    solver = LinearSolver(
        method=method,
        regularization=regularization,
        alpha=alpha,
        constraints=constraints,
        bounds=bounds,
        verbose=True,
        **solver_kwargs,
    )

    # # Solve for reduced parameters
    # phi_reduced = solver.solve(W_processed, tau_processed)

    # # Map back to full parameter space
    # phi_full = np.zeros(len(self.standard_parameter))
    # active_indices = [
    #     i for i, active in enumerate(active_params.values()) if active
    # ]
    # phi_full[active_indices] = phi_reduced

    # Step 4: Compute base parameters using QR decomposition
    from figaroh.tools.qrdecomposition import QRDecomposer

    W_base, _, base_parameters, _, phi_std = QRDecomposer().double_decomposition(
        tau_processed, W_processed, active_params, self.standard_parameter
    )
    phi_base = solver.solve(W_base, tau_processed)
    base_param_dict = {
        param: phi_base[i] for i, param in enumerate(base_parameters)
    }

    # Store results
    self.dynamic_regressor_base = W_base
    self.phi_base = phi_base
    self.params_base = list(base_param_dict.keys())
    self.tau_identif = W_base @ phi_base
    self.tau_noised = tau_processed

    # Step 5: Compute quality metrics and store
    self._compute_quality_metrics()

    results = {
        "base_regressor": W_base,
        "base_param_dict": base_param_dict,
        "base_parameters": base_parameters,
        "phi_base": phi_base,
        "tau_estimated": self.tau_identif,
        "tau_processed": tau_processed,
        "solver_info": solver.solver_info,
        "solver_method": method,
        "regularization": regularization,
        "alpha": alpha,
    }

    self._store_results(results)

    # Step 6: Optional plotting
    if plotting:
        self.plot_results()

    # Step 7: Optional parameter saving
    if save_results:
        self.save_results()

    logger.info(f"  RMSE: {self.rms_error:.6f}")
    logger.info(f"  Correlation: {self.correlation:.6f}")

    return self.phi_base
```

### `load_param(config_file, setting_type='identification')`

Load the identification parameters from the yaml file.

Only the unified configuration format is supported; a legacy flat config raises an error naming `figaroh.utils.config_migration`.

Parameters:

| Name           | Type  | Description                          | Default            |
| -------------- | ----- | ------------------------------------ | ------------------ |
| `config_file`  | `str` | Path to a unified configuration file | *required*         |
| `setting_type` | `str` | Configuration section to load        | `'identification'` |

Source code in `src/figaroh/identification/base_identification.py`

```
def load_param(self, config_file, setting_type="identification"):
    """Load the identification parameters from the yaml file.

    Only the unified configuration format is supported; a legacy flat
    config raises an error naming ``figaroh.utils.config_migration``.

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

        # The legacy flat format was removed in 1.0
        require_unified_config(config_file)
        parser = UnifiedConfigParser(config_file)
        unified_config = parser.parse()
        unified_identif_config = create_task_config(
            self.robot, unified_config, setting_type
        )
        # Convert unified format to the identif_config dict
        self.identif_config = unified_to_legacy_identif_config(
            self.robot, unified_identif_config
        )
    except Exception as e:
        logger.error(f"Error loading config {config_file}: {e}")
        raise
```

### `load_trajectory_data(data_source=None)`

Load and process CSV data.

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

Parameters:

| Name          | Type  | Description                                                                                                                                                                                                                                                                                                                                                                                              | Default |
| ------------- | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `data_source` | `str` | Optional override identifying an alternate dataset to load instead of the class's normal training data (e.g. a directory holding a held-out validation trajectory with the same file layout/naming as the training data). When None (default), the subclass loads its usual training data exactly as before. Passed through from identif_config["validation_data_file"] by :meth:\_load_validation_data. | `None`  |

Returns:

| Name   | Type | Description                                               |
| ------ | ---- | --------------------------------------------------------- |
| `dict` |      | Dictionary with keys 'timestamps', 'positions',           |
|        |      | 'velocities', 'accelerations', 'torques' (numpy arrays;   |
|        |      | 'velocities'/'accelerations' may be None to be derived by |
|        |      | differentiation).                                         |

Source code in `src/figaroh/identification/base_identification.py`

```
@abstractmethod
def load_trajectory_data(self, data_source: str = None):
    """Load and process CSV data.

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

    Args:
        data_source: Optional override identifying an alternate dataset
            to load instead of the class's normal training data (e.g.
            a directory holding a held-out validation trajectory with
            the same file layout/naming as the training data). When
            ``None`` (default), the subclass loads its usual training
            data exactly as before. Passed through from
            ``identif_config["validation_data_file"]`` by
            :meth:`_load_validation_data`.

    Returns:
        dict: Dictionary with keys 'timestamps', 'positions',
        'velocities', 'accelerations', 'torques' (numpy arrays;
        'velocities'/'accelerations' may be None to be derived by
        differentiation).
    """
    pass
```

### `process_data(truncate=None)`

Load and process data

Source code in `src/figaroh/identification/base_identification.py`

```
def process_data(self, truncate=None):
    """Load and process data"""
    # Set default filter configuration
    filter_config = self.filter_config

    # load raw data: the legacy dict, or a TrajectoryData (#55)
    self.raw_data = self._adopt_trajectory(self.load_trajectory_data())

    # Truncate data if truncation indices are provided
    self.raw_data = self._truncate_data(self.raw_data, truncate)
    self._sample_mask = self._trajectory_mask(self.trajectory, truncate)

    # Apply filtering and differentiation kinematics data
    self.process_kinematics_data(filter_config)

    # Process joint torque data
    self.processed_data["torques"] = self.process_torque_data()

    # Update sample count to ensure consistency
    self.num_samples = self.processed_data["positions"].shape[0]

    # Build full configuration
    self._build_full_configuration()
    self._record_data_stage()
```

### `fit_joints()`

Active joints whose effort rows enter the fit, in active order.

`identif_config["torque_fit_joints"]` names them; by default every active joint. The other active joints keep their kinematics in the regressor (their motion still loads the fitted joints) but their measured effort is neither fitted nor scored: use it for joints whose effort signal is not trusted (unknown force constant, quantised).

Source code in `src/figaroh/identification/base_identification.py`

```
def fit_joints(self):
    """Active joints whose effort rows enter the fit, in active order.

    ``identif_config["torque_fit_joints"]`` names them; by default every
    active joint. The other active joints keep their kinematics in the
    regressor (their motion still loads the fitted joints) but their
    measured effort is neither fitted nor scored: use it for joints whose
    effort signal is not trusted (unknown force constant, quantised).
    """
    active = list(self.identif_config.get("active_joints", []))
    chosen = self.identif_config.get("torque_fit_joints")
    if chosen is None:
        return active
    unknown = [j for j in chosen if j not in active]
    if unknown:
        raise ValueError(
            f"torque_fit_joints {unknown} are not active joints {active}"
        )
    if not chosen:
        raise ValueError("torque_fit_joints is empty: nothing to fit")
    return [j for j in active if j in chosen]
```

### `calculate_full_regressor()`

Build regressor matrix, compute pre-identified values of standard parameters, compute joint torques based on pre-identified standard parameters.

Source code in `src/figaroh/identification/base_identification.py`

```
def calculate_full_regressor(self):
    """Build regressor matrix, compute pre-identified values of standard
    parameters, compute joint torques based on pre-identified standard
    parameters."""
    # Build full regressor matrix
    self.dynamic_regressor = build_regressor_basic(
        self.robot,
        self.processed_data["positions"],
        self.processed_data["velocities"],
        self.processed_data["accelerations"],
        self.identif_config,
    )
```

### `initialize_standard_parameters()`

Initialize standard parameters for the robot.

Source code in `src/figaroh/identification/base_identification.py`

```
def initialize_standard_parameters(
    self,
):
    """Initialize standard parameters for the robot."""

    # Compute standard parameters
    self.standard_parameter = get_standard_parameters(
        self.model, self.identif_config
    )

    # additional parameters can be added in robot-specific subclass
    if (
        self.identif_config.get("has_friction", False)
        or self.identif_config.get("has_actuator_inertia", False)
        or self.identif_config.get("has_joint_offset", False)
    ):
        self.additional_parameters = add_standard_additional_parameters(
            self.model, self.identif_config
        )
        self.standard_parameter.update(self.additional_parameters)

    # Add custom parameters specific to the robot
    if self.identif_config.get("has_custom_parameters", False):
        self.custom_parameters = add_custom_parameters(
            self.model, self.identif_config.get("custom_parameters", {})
        )
        self.standard_parameter.update(self.custom_parameters)

    # Convert all string values to floats in the standard_parameter dict
    for key, value in self.standard_parameter.items():
        if isinstance(value, str):
            self.standard_parameter[key] = float(value)
```

### `compute_reference_torque()`

Compute reference joint torques based on standard parameters and dynamic regressor.

Source code in `src/figaroh/identification/base_identification.py`

```
def compute_reference_torque(self):
    """Compute reference joint torques based on standard parameters and dynamic regressor."""

    # joint torque estimated from p,v,a with std params
    phi_ref = np.array(list(self.standard_parameter.values()))
    tau_ref = np.dot(self.dynamic_regressor, phi_ref)

    # filter only active joints
    self.tau_ref = tau_ref[
        range(len(self.identif_config["act_idxv"]) * self.num_samples)
    ]
```

### `process_kinematics_data(filter_config=None)`

Process kinematics data (positions, velocities, accelerations) with filtering.

Source code in `src/figaroh/identification/base_identification.py`

```
def process_kinematics_data(self, filter_config=None):
    """Process kinematics data (positions, velocities, accelerations) with filtering."""
    self.filter_kinematics_data(filter_config)
```

### `filter_kinematics_data(filter_config=None)`

Apply filtering to data with configurable parameters.

Parameters:

| Name            | Type   | Description                                                                                                                                 | Default |
| --------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `filter_config` | `dict` | Filter configuration with keys: - differentiation_method: Method for derivative estimation - filter_params: Parameters for signal filtering | `None`  |

Raises:

| Type         | Description                 |
| ------------ | --------------------------- |
| `ValueError` | If required data is missing |

Source code in `src/figaroh/identification/base_identification.py`

```
def filter_kinematics_data(self, filter_config=None):
    """Apply filtering to data with configurable parameters.

    Args:
        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
    """
    # Validate required data
    if self.raw_data.get("timestamps") is None:
        raise ValueError("Timestamps are required for data processing")
    if self.raw_data.get("positions") is None:
        raise ValueError("Position data is required for processing")

    # Create processed data copy to avoid modifying raw data
    self.processed_data = {}

    # Process timestamps (no filtering needed)
    self.processed_data["timestamps"] = self.raw_data["timestamps"]

    # Define signal processing pipeline
    signal_pipeline = [
        ("positions", self.raw_data["positions"], None),
        ("velocities", self.raw_data.get("velocities"), "positions"),
        ("accelerations", self.raw_data.get("accelerations"), "velocities"),
    ]

    # Process signals through pipeline
    for signal_name, signal_data, dependency in signal_pipeline:
        if signal_data is not None:
            # Apply filtering to existing data
            self.processed_data[signal_name] = self._apply_filters(
                signal_data, **filter_config["filter_params"]
            )
        else:
            # Estimate missing signal from dependency
            if dependency:
                dependency_data = self.processed_data[dependency]
                self.processed_data[signal_name] = self._differentiate_signal(
                    self.processed_data["timestamps"],
                    dependency_data,
                    method=filter_config["differentiation_method"],
                )
            else:
                raise ValueError(
                    f"Cannot process {signal_name}: no data or dependency"
                )
```

### `process_torque_data(**kwargs)`

Process torque data (generic implementation, should be overridden for robot-specific processing).

Source code in `src/figaroh/identification/base_identification.py`

```
def process_torque_data(self, **kwargs):
    """Process torque data (generic implementation, should be overridden for robot-specific processing)."""
    # Generic torque processing - robots should override this method
    if self.raw_data["torques"] is not None:
        self.processed_data["torques"] = self.raw_data["torques"]
        return self.processed_data["torques"]
    else:
        raise ValueError("Torque data is required for processing")
```

### `print_quality_report()`

Print a formatted identification quality report to the terminal.

Reports condition number, overall torque residual statistics, per-joint residuals (when available), base-parameter uncertainty, held-out validation metrics (if configured), and optional physical-consistency / reconstruction status.

Source code in `src/figaroh/identification/base_identification.py`

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

    Reports condition number, overall torque residual statistics,
    per-joint residuals (when available), base-parameter uncertainty,
    held-out validation metrics (if configured), and optional
    physical-consistency / reconstruction status.
    """
    if self.result is None:
        logger.warning("No identification results to report. Run solve() first.")
        return

    result = self.result
    per_joint = self._compute_per_joint_stats()

    print()
    print("=" * 70)
    print("  IDENTIFICATION QUALITY REPORT")
    print("=" * 70)
    from figaroh.tools.stages import stages_line

    print(f"  Stages:          {stages_line(self)}")
    sel = getattr(self, "selected", None)
    if sel is not None:
        print(
            f"  Reported estimate: "
            f"{sel.stage if sel.accepted else 'none'}"
            f" (requested {sel.requested}: {sel.status}; {sel.reason})"
        )

    cond_num = result.get("condition number", float("nan"))
    n_base = len(result.get("base parameters names", []))
    print(
        f"  Base parameters: {n_base}    "
        f"Samples: {result.get('num samples', 0)}"
    )
    if not np.isnan(cond_num):
        cond_label = (
            "well-conditioned"
            if cond_num < 100
            else "moderately-conditioned" if cond_num < 1000 else "ill-conditioned"
        )
        print(f"  Condition:    {cond_num:.1f} ({cond_label})")
    else:
        print("  Condition:    unavailable")
    rmse_label = "RMSE:" if sel is None else "RMSE (base fit):"
    print(
        f"  {rmse_label:<13s} {result.get('rmse norm (N/m)', float('nan')):.4f}    "
        f"Correlation: {self.correlation:.4f}"
    )
    selected_info = result.get("selected")
    if isinstance(selected_info, dict) and "effort_rmse_fit" in selected_info:
        print(
            f"  RMSE ({selected_info['stage']} estimate): "
            f"{selected_info['effort_rmse_fit']:.4f}"
        )

    if per_joint is not None:
        print("-" * 70)
        heading = "Per-Joint Torque Residuals (training set)"
        if sel is not None:
            heading = "Base fit residuals, per joint (training set)"
        print(f"  {heading}")
        names = per_joint["joint_names"]
        print(
            f"  {'Joint':<22s} {'Mean':>10s} {'Std':>10s} "
            f"{'RMSE':>10s} {'Max':>10s}"
        )
        print(f"  {'-'*22} {'-'*10} {'-'*10} {'-'*10} {'-'*10}")
        for i in range(len(names)):
            m = f"{per_joint['mean'][i]:10.4f}"
            s = f"{per_joint['std'][i]:10.4f}"
            r = f"{per_joint['rmse'][i]:10.4f}"
            x = f"{per_joint['max_abs'][i]:10.4f}"
            print(f"  {names[i]:<22s} {m} {s} {r} {x}")
        per_joint_sel = (selected_info or {}).get("effort_rmse_fit_per_joint")
        if per_joint_sel:
            print(f"  Selected estimate ({selected_info['stage']}) RMSE per joint")
            for name, value in per_joint_sel.items():
                print(f"  {name:<22s} {value:10.4f}")
    else:
        print("-" * 70)
        print("  Per-joint residuals: unavailable")

    # ── Base-parameter uncertainty ──
    print("-" * 70)
    std_relative = getattr(self, "std_relative", None)
    base_names = result.get("base parameters names", [])
    if std_relative is not None and len(std_relative) == len(base_names):
        order = sorted(range(len(base_names)), key=lambda i: -abs(std_relative[i]))
        print("  Base-Parameter Uncertainty (top 5 by relative std-dev)")
        for i in order[:5]:
            print(f"    {base_names[i]:<50s} {std_relative[i]:8.1f}%")
    else:
        print("  Base-parameter uncertainty: unavailable")

    # ── Validation ──
    print("-" * 70)
    val = result.get("validation_metrics")
    if val is not None:
        if val.get("validation_source") == "identification_data_fallback":
            print(
                "  ⚠ WARNING: no separate validation data "
                "provided — falling back to identification data. "
                "These are NOT an independent generalization test."
            )
            print(
                f"  Validation (identification set, " f"n={val['n_val_samples']})"
            )
        else:
            print(f"  Validation (separate set, n={val['n_val_samples']})")
        print(
            f"    RMSE nominal:    {val['rmse_nominal']:.4f}    "
            f"RMSE identified: {val['rmse_identified']:.4f}"
        )
        print(
            f"    Improvement:     {val['improvement_pct']:.1f}%    "
            "Correlation (per-joint normalised): "
            f"{val.get('correlation_normalised', float('nan')):.4f}"
        )
        for name, m in val.get("per_joint", {}).items():
            flag = "" if m["predictive"] else "  <- not better than a constant"
            print(
                f"    {name:24s} RMSE {m['rmse_identified']:.4g} {m['unit']}"
                f"  std {m['std_measured']:.4g}  R² {m['r2']:.3f}{flag}"
            )
    else:
        print("  Validation: no separate validation data provided.")
        print(
            "    Set validation_data_file in the identification config "
            "to enable held-out FK/torque validation."
        )

    # ── Optional physical consistency / reconstruction ──
    pc = result.get("physical consistency")
    if pc is not None:
        print("-" * 70)
        print(f"  Physical consistency: {pc.get('status', 'unknown')}")
    recon = result.get("reconstruction")
    if recon is not None:
        print("-" * 70)
        print(f"  Full-parameter reconstruction: {recon.get('status', 'unknown')}")

    print("=" * 70)
```

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

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

Parameters:

| Name          | Type  | Description                                                                             | Default     |
| ------------- | ----- | --------------------------------------------------------------------------------------- | ----------- |
| `output_path` | `str` | Explicit output file path. If None, writes to \<output_dir>/identification_report.html. | `None`      |
| `output_dir`  | `str` | Directory used when output_path is not given (created if missing).                      | `'results'` |

Returns:

| Type  | Description                         |
| ----- | ----------------------------------- |
| `str` | The path the report was written to. |

Raises:

| Type             | Description                      |
| ---------------- | -------------------------------- |
| `AttributeError` | If solve() has not been run yet. |

Source code in `src/figaroh/identification/base_identification.py`

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

    Args:
        output_path: Explicit output file path. If None, writes to
            ``<output_dir>/identification_report.html``.
        output_dir: Directory used when output_path is not given
            (created if missing).

    Returns:
        The path the report was written to.

    Raises:
        AttributeError: If solve() has not been run yet.
    """
    if self.result is None:
        raise AttributeError(
            "No identification results available. Run solve() first."
        )

    from os import makedirs
    from os.path import join

    from figaroh.tools.identification_report import (
        generate_identification_report,
    )

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

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

### `verify(thresholds=None, scope='prediction')`

Return scoped acceptance evidence with explicit incomplete states.

`scope="execution"` checks finite numerical fit outputs; it does not certify prediction, physical parameters or export. `scope="prediction"` (default) additionally requires independent validation and explicit application error limits. No universal improvement, correlation, conditioning or prediction-error gate is imposed.

`thresholds` maps metric names to `threshold`, `comparison` (min/max), and optional `required` (default True). Missing required evidence produces not_evaluated, not PASS. Nonfinite evidence fails. `passed` is True only when all required checks in scope pass.

Source code in `src/figaroh/identification/base_identification.py`

```
def verify(
    self,
    thresholds: Optional[Dict[str, Dict[str, Any]]] = None,
    scope: str = "prediction",
):
    """Return scoped acceptance evidence with explicit incomplete states.

    ``scope="execution"`` checks finite numerical fit outputs;
    it does not certify prediction, physical parameters or export.
    ``scope="prediction"`` (default) additionally requires independent validation
    and explicit application error limits. No universal improvement,
    correlation, conditioning or prediction-error gate is imposed.

    ``thresholds`` maps metric names to ``threshold``, ``comparison``
    (min/max), and optional ``required`` (default True). Missing required
    evidence produces not_evaluated, not PASS. Nonfinite evidence fails.
    ``passed`` is True only when all required checks in scope pass.
    """
    if self.result is None:
        raise AttributeError(
            "No identification results available. Run solve() first."
        )

    from figaroh.tools._report_common import (
        IDENTIFICATION_DEFAULT_THRESHOLDS,
        scoped_verification,
    )
    from figaroh.tools.identification_report import _build_insights
    from figaroh.tools.provenance import collect_run_provenance

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

    result = self.result
    base_names = result.get("base parameters names", [])
    std_relative_raw = getattr(self, "std_relative", None)
    std_relative = list(std_relative_raw) if std_relative_raw is not None else []
    validation = result.get("validation_metrics")

    # with a non-default select_stage the thresholds judge the selected
    # estimate, not the base fit it was derived from (#61)
    fit_rmse = result.get("rmse norm (N/m)", float("nan"))
    selected_info = result.get("selected")
    if (
        isinstance(selected_info, dict)
        and selected_info.get("effort_rmse_fit") is not None
    ):
        fit_rmse = selected_info["effort_rmse_fit"]
    metrics: Dict[str, float] = {
        "condition_number": result.get("condition number", float("nan")),
        "rmse": fit_rmse,
    }
    if validation is not None:
        # per-joint normalised: a pooled correlation is dominated by the
        # largest (gravity) torques and mixes N with N·m (#103)
        metrics["validation_correlation"] = validation.get(
            "correlation_normalised", validation.get("correlation", float("nan"))
        )
        metrics["validation_unpredictable_joints"] = float(
            len(validation.get("unpredictable_joints", []))
        )
        metrics["validation_improvement_pct"] = validation.get(
            "improvement_pct", float("nan")
        )

    joint_names = (validation or {}).get("joint_names", [])
    independent = (validation or {}).get("validation_source") == "validation_data"
    for joint in joint_names:
        observed = np.asarray(
            validation.get("tau_measured_per_joint", {}).get(joint, [])
        )
        predicted = np.asarray(
            validation.get("tau_identified_per_joint", {}).get(joint, [])
        )
        if observed.size and observed.shape == predicted.shape:
            prefix = "validation" if independent else "training"
            metrics[f"{prefix}_rmse:{joint}"] = float(
                np.sqrt(np.mean((observed - predicted) ** 2))
            )
            metrics[f"{prefix}_abs_bias:{joint}"] = float(
                abs(np.mean(observed - predicted))
            )
            metrics[f"{prefix}_peak_error:{joint}"] = float(
                np.max(np.abs(observed - predicted))
            )
    sel = getattr(self, "selected", None)
    finite_parameters = result.get("base parameters values")
    finite_prediction = result.get("torque estimated")
    facts_extra = {}
    if sel is not None:
        # the candidate that was asked for, not the fit (#61)
        if sel.values is not None:
            finite_parameters = sel.values
            if sel.space == "standard":
                eliminated = set(self._idx_eliminated or [])
                keep = [i for i in range(len(sel.names)) if i not in eliminated]
                finite_prediction = self._solve_W @ np.asarray(sel.values)[keep]
        facts_extra["selected_stage_accepted"] = (
            bool(sel.accepted),
            f"The requested {sel.requested} estimate was rejected: {sel.reason}",
        )
    verdict = scoped_verification(
        metrics,
        thresholds,
        scope,
        {
            "finite_parameters": finite_parameters,
            "finite_prediction": finite_prediction,
            "finite_measurements": result.get("torque processed"),
            "finite_fit_rmse": (
                fit_rmse
                if isinstance(selected_info, dict)
                else result.get("rmse norm (N/m)")
            ),
        },
        independent,
        [f"validation_rmse:{j}" for j in joint_names],
        facts={
            "requested_validation_loaded": (
                not self.identif_config.get("validation_data_file")
                or bool(getattr(self, "_val_available", False))
            ),
            "matching_torque_shapes": (
                np.shape(result["torque estimated"])
                == np.shape(result["torque processed"])
                if result.get("torque estimated") is not None
                and result.get("torque processed") is not None
                else None
            ),
            **facts_extra,
        },
    )
    verdict.insights = [
        i["text"]
        for i in _build_insights(result, std_relative, base_names, validation)
    ]
    verdict.metadata = getattr(
        self, "_run_provenance", None
    ) or collect_run_provenance(self, "identification")

    active_joints = self.identif_config.get("active_joints", [])
    if validation is not None and "tau_nominal_per_joint" in validation:
        verdict.series = {
            # every sample (masked ones too), like the per-joint series
            "time": list(
                range(
                    len(next(iter(validation["tau_measured_per_joint"].values())))
                )
            ),
            "joint_names": validation.get("joint_names", active_joints),
            "nominal": validation["tau_nominal_per_joint"],
            "fitted": validation["tau_identified_per_joint"],
            "measured": validation["tau_measured_per_joint"],
        }
    verdict.compat = {
        "active_joints": active_joints,
        "decimate": bool(getattr(self, "_decimate_used", False)),
        "sample_count": result.get("num samples", 0),
        "config_sha256": verdict.metadata.get("config", {}).get("sha256"),
    }
    from figaroh.tools.stages import apply_to_verdict

    # the reported parameters come from the fit (phi_base / var_) unless
    # select_stage asked for another estimate (#61)
    apply_to_verdict(
        verdict,
        self,
        selected_stage=(
            "fit" if sel is None else (sel.stage if sel.accepted else "none")
        ),
    )
    return verdict
```

### `export_urdf(nominal_urdf, output_path=None, merged_bodies='refuse')`

Write the selected estimate's link inertials into a URDF.

Needs `select_stage` `reconstruction` or `physical_fit` and an *accepted* estimate: the base-parameter fit has no per-link inertials, and a rejected estimate is not exported. Either case records stage `export` as failed and raises. On success the URDF is written, reloaded with Pinocchio as a check and `export` is recorded `ok` with the file as artifact.

Parameters:

| Name            | Type            | Description                                                                                                                                           | Default    |
| --------------- | --------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `nominal_urdf`  | `str`           | Path of the nominal URDF the run was based on.                                                                                                        | *required* |
| `output_path`   | `Optional[str]` | Where to write; default <stem>\_modified.urdf beside the nominal one.                                                                                 | `None`     |
| `merged_bodies` | `str`           | "refuse" (default) or "subtract_fixed" for joints whose Pinocchio body holds fixed-attached links, see :func:figaroh.tools.urdf_exporter.export_urdf. | `'refuse'` |

Returns:

| Name  | Type  | Description                        |
| ----- | ----- | ---------------------------------- |
| `str` | `str` | Absolute path of the written URDF. |

Raises:

| Type         | Description                                                                  |
| ------------ | ---------------------------------------------------------------------------- |
| `ValueError` | no accepted standard-space estimate, or the exporter refused the parameters. |

Source code in `src/figaroh/identification/base_identification.py`

```
def export_urdf(
    self,
    nominal_urdf: str,
    output_path: Optional[str] = None,
    merged_bodies: str = "refuse",
) -> str:
    """Write the selected estimate's link inertials into a URDF.

    Needs ``select_stage`` ``reconstruction`` or ``physical_fit`` and an
    *accepted* estimate: the base-parameter fit has no per-link
    inertials, and a rejected estimate is not exported. Either case
    records stage ``export`` as failed and raises. On success the URDF is
    written, reloaded with Pinocchio as a check and ``export`` is
    recorded ``ok`` with the file as artifact.

    Args:
        nominal_urdf: Path of the nominal URDF the run was based on.
        output_path: Where to write; default ``<stem>_modified.urdf``
            beside the nominal one.
        merged_bodies: ``"refuse"`` (default) or ``"subtract_fixed"`` for
            joints whose Pinocchio body holds fixed-attached links, see
            :func:`figaroh.tools.urdf_exporter.export_urdf`.

    Returns:
        str: Absolute path of the written URDF.

    Raises:
        ValueError: no accepted standard-space estimate, or the exporter
            refused the parameters.
    """
    from figaroh.tools.stages import record_stage
    from figaroh.tools.urdf_exporter import export_urdf

    sel = getattr(self, "selected", None)
    try:
        if sel is None or sel.space != "standard":
            raise ValueError(
                "the base-parameter fit has no link inertials to export; "
                "set select_stage to 'reconstruction' or 'physical_fit'"
            )
        if not sel.accepted:
            raise ValueError(
                f"the requested {sel.requested} estimate was rejected "
                f"({sel.reason}); nothing to export"
            )
        params = {}
        # only joints whose inertials were identified: the free-flyer
        # body has no URDF joint and non-identified joints keep their
        # nominal values (also when a fixed link is merged into them)
        for joint, p10 in sel.link_p10(self._identified_joints()).items():
            for key, value in zip(_INERTIAL_KEYS, p10):
                params[f"{key}_{joint}"] = float(value)
        out = export_urdf(
            nominal_urdf,
            params,
            output_path=output_path,
            merged_bodies=merged_bodies,
            psd_eig_tol=sel.psd_eig_tol,
        )
        import pinocchio as pin

        try:
            pin.buildModelFromUrdf(out)  # the file must load
        except Exception:
            os.remove(out)
            raise
    except (ValueError, KeyError) as exc:
        record_stage(self, "export", "failed", str(exc))
        self._refresh_result_stages()
        raise ValueError(f"export failed: {exc}") from exc
    except Exception as exc:  # I/O, Pinocchio reload, ...
        record_stage(self, "export", "failed", f"{type(exc).__name__}: {exc}")
        self._refresh_result_stages()
        raise
    record_stage(
        self,
        "export",
        "ok",
        f"{sel.stage} link inertials written",
        artifacts=[out],
    )
    self._refresh_result_stages()
    return out
```

### `export_verification_report(output_path=None, output_dir='results', thresholds=None, scope='prediction')`

Write this identification's :meth:`verify` verdict as JSON.

Parameters:

| Name          | Type                                  | Description                                                                                | Default        |
| ------------- | ------------------------------------- | ------------------------------------------------------------------------------------------ | -------------- |
| `output_path` | `str`                                 | Explicit file path. If omitted, defaults to {output_dir}/identification_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`         |
| `scope`       | `str`                                 | Forwarded to :meth:verify; default prediction acceptance.                                  | `'prediction'` |

Returns:

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

Source code in `src/figaroh/identification/base_identification.py`

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

    Args:
        output_path: Explicit file path. If omitted, defaults to
            ``{output_dir}/identification_verification.json``.
        output_dir: Directory used when ``output_path`` is omitted.
        thresholds: Forwarded to :meth:`verify`.
        scope: Forwarded to :meth:`verify`; default prediction acceptance.

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

    verdict = self.verify(thresholds=thresholds, scope=scope)
    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, "identification_verification.json")

    from figaroh.tools._report_common import verification_json_data
    from figaroh.tools.stages import with_schema as _with_schema

    with open(output_path, "w") as f:
        json.dump(
            verification_json_data(_with_schema(self, verdict_dict)),
            f,
            indent=2,
            allow_nan=False,
        )

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

### `plot_results()`

Plot identification results using unified results manager.

Source code in `src/figaroh/identification/base_identification.py`

```
def plot_results(self):
    """Plot identification results using unified results manager."""
    if not hasattr(self, "result") or self.result is None:
        logger.warning("No identification results to plot. Run solve() first.")
        return

    def _basic_plots():
        try:
            import matplotlib.pyplot as plt

            # Extract data from self.result dictionary
            tau_measured = self.result.get("torque processed", np.array([]))
            tau_identified = self.result.get("torque estimated", np.array([]))
            parameter_values = self.result.get(
                "base parameters values", np.array([])
            )

            if len(tau_measured) == 0 or len(tau_identified) == 0:
                logger.warning("No torque data available for plotting")
                return

            plt.figure(figsize=(12, 8))

            plt.subplot(2, 1, 1)
            plt.plot(tau_measured, label="Measured (with noise)", alpha=0.7)
            plt.plot(tau_identified, label="Identified", alpha=0.7)
            plt.xlabel("Sample")
            plt.ylabel("Torque (Nm)")
            plt.title(
                f"{self.__class__.__name__} Torque Comparison"
                + ("" if self.selected is None else " (base fit)")
            )
            plt.legend()
            plt.grid(True, alpha=0.3)

            plt.subplot(2, 1, 2)
            if len(parameter_values) > 0:
                plt.bar(
                    range(len(parameter_values)),
                    parameter_values,
                    alpha=0.7,
                    label="Base Parameters",
                )
                plt.xlabel("Parameter Index")
                plt.ylabel("Parameter Value")
                plt.title("Identified Base Parameters")
                plt.legend()
                plt.grid(True, alpha=0.3)

            plt.tight_layout()
            plt.show()

        except ImportError:
            logger.warning("matplotlib not available for plotting")
        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_identification_results(
                n_joints=len(self._fit_blocks()),
                joint_names=self.fit_joints(),
            ),
            _basic_plots,
            logger,
            "identification",
        )
    else:
        _basic_plots()
```

### `save_results(output_dir='results')`

Save identification results using unified results manager.

Source code in `src/figaroh/identification/base_identification.py`

```
def save_results(self, output_dir="results"):
    """Save identification results using unified results manager."""
    if not hasattr(self, "result") or self.result is None:
        logger.warning("No identification 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("Identification 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...")

    # Fallback to basic saving if ResultsManager not available
    try:
        import os
        import yaml
        import datetime

        os.makedirs(output_dir, exist_ok=True)

        # Extract data from self.result dictionary
        parameter_values = self.result.get("base parameters values", np.array([]))
        parameter_names = self.result.get("base parameters names", [])
        condition_number = self.result.get("condition number", 0)
        rmse_norm = self.result.get("rmse norm (N/m)", 0)
        std_dev_param = self.result.get("std dev of estimated param", np.array([]))

        results_dict = {
            "base_parameters": (
                parameter_values.tolist()
                if hasattr(parameter_values, "tolist")
                else parameter_values
            ),
            "parameter_names": [str(p) for p in parameter_names],
            "condition_number": float(condition_number),
            "rmse_norm": float(rmse_norm),
            "standard_deviation": (
                std_dev_param.tolist()
                if hasattr(std_dev_param, "tolist")
                else std_dev_param
            ),
        }

        robot_name = self.__class__.__name__.lower().replace("identification", "")
        timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
        filename = f"{robot_name}_identification_results_{timestamp}.yaml"

        with open(os.path.join(output_dir, filename), "w") as f:
            yaml.dump(results_dict, f, default_flow_style=False)

        logger.info(f"Results saved to {output_dir}/{filename}")
        return {filename: os.path.join(output_dir, filename)}

    except Exception as e:
        logger.error(f"Error in fallback saving: {e}")
        return None
```

## identification_tools

## `base_param_from_standard(phi_standard, params_base)`

Convert standard parameters to base parameters.

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

Parameters:

| Name           | Type   | Description                         | Default    |
| -------------- | ------ | ----------------------------------- | ---------- |
| `phi_standard` | `dict` | Standard parameters from model/URDF | *required* |
| `params_base`  | `list` | Analytical parameter relationships  | *required* |

Returns:

| Name   | Type | Description                                               |
| ------ | ---- | --------------------------------------------------------- |
| `list` |      | Base parameter values calculated from standard parameters |

Source code in `src/figaroh/identification/identification_tools.py`

```
def base_param_from_standard(phi_standard, params_base):
    """Convert standard parameters to base parameters.

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

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

    Returns:
        list: Base parameter values calculated from standard parameters
    """
    phi_base = []
    ops = {"+": operator.add, "-": operator.sub}
    for ii in range(len(params_base)):
        param_base_i = params_base[ii].split(" ")
        values = []
        list_ops = []
        for jj in range(len(param_base_i)):
            param_base_j = param_base_i[jj].split("*")
            if len(param_base_j) == 2:
                value = float(param_base_j[0]) * phi_standard[param_base_j[1]]
                values.append(value)
            elif param_base_j[0] != "+" and param_base_j[0] != "-":
                value = phi_standard[param_base_j[0]]
                values.append(value)
            else:
                list_ops.append(ops[param_base_j[0]])
        value_phi_base = values[0]
        for kk in range(len(list_ops)):
            value_phi_base = list_ops[kk](value_phi_base, values[kk + 1])
        phi_base.append(value_phi_base)
    return phi_base
```

## `relative_stdev(W_b, phi_b, tau)`

Calculate relative standard deviation of identified parameters.

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

Parameters:

| Name    | Type      | Description                   | Default    |
| ------- | --------- | ----------------------------- | ---------- |
| `W_b`   | `ndarray` | Base regressor matrix         | *required* |
| `phi_b` | `list`    | Base parameter values         | *required* |
| `tau`   | `ndarray` | Measured joint torques/forces | *required* |

Returns:

| Name      | Type | Description                                             |
| --------- | ---- | ------------------------------------------------------- |
| `ndarray` |      | Relative standard deviation (%) for each base parameter |

Source code in `src/figaroh/identification/identification_tools.py`

```
def relative_stdev(W_b, phi_b, tau):
    """Calculate relative standard deviation of identified parameters.

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

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

    Returns:
        ndarray: Relative standard deviation (%) for each base parameter
    """
    # stdev of residual error ro
    sig_ro_sqr = np.linalg.norm((tau - np.dot(W_b, phi_b))) ** 2 / (
        W_b.shape[0] - phi_b.shape[0]
    )

    # covariance matrix of estimated parameters
    C_x = sig_ro_sqr * np.linalg.inv(np.dot(W_b.T, W_b))

    # relative stdev of estimated parameters
    std_x_sqr = np.diag(C_x)
    std_xr = np.zeros(std_x_sqr.shape[0])
    for i in range(std_x_sqr.shape[0]):
        std_xr[i] = np.round(100 * np.sqrt(std_x_sqr[i]) / np.abs(phi_b[i]), 2)

    return std_xr
```

## `index_in_base_params(params, id_segments)`

Map segment IDs to their base parameters.

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

Parameters:

| Name          | Type   | Description                | Default    |
| ------------- | ------ | -------------------------- | ---------- |
| `params`      | `list` | Base parameter expressions | *required* |
| `id_segments` | `list` | Segment IDs to map         | *required* |

Returns:

| Name   | Type | Description                                         |
| ------ | ---- | --------------------------------------------------- |
| `dict` |      | Maps segment IDs to lists of base parameter indices |

Source code in `src/figaroh/identification/identification_tools.py`

```
def index_in_base_params(params, id_segments):
    """Map segment IDs to their base parameters.

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

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

    Returns:
        dict: Maps segment IDs to lists of base parameter indices
    """
    base_index = []
    params_name = [
        "Ixx",
        "Ixy",
        "Ixz",
        "Iyy",
        "Iyz",
        "Izz",
        "mx",
        "my",
        "mz",
        "m",
    ]

    id_segments_new = [i for i in range(len(id_segments))]

    for id in id_segments:
        for ii in range(len(params)):
            param_base_i = params[ii].split(" ")
            for jj in range(len(param_base_i)):
                param_base_j = param_base_i[jj].split("*")
                for ll in range(len(param_base_j)):
                    for kk in params_name:
                        if kk + str(id) == param_base_j[ll]:
                            base_index.append((id, ii))

    base_index[:] = list(set(base_index))
    base_index = sorted(base_index)

    dictio = {}

    for i in base_index:
        dictio.setdefault(i[0], []).append(i[1])

    values = []
    for ii in dictio:
        values.append(dictio[ii])

    return dict(zip(id_segments_new, values))
```

## `weigthed_least_squares(robot, phi_b, W_b, tau_meas, tau_est, identif_config)`

Compute weighted least squares solution for parameter identification.

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

Parameters:

| Name       | Type      | Description                     | Default    |
| ---------- | --------- | ------------------------------- | ---------- |
| `robot`    | `Robot`   | Robot model                     | *required* |
| `phi_b`    | `ndarray` | Initial base parameters         | *required* |
| `W_b`      | `ndarray` | Base regressor matrix           | *required* |
| `tau_meas` | `ndarray` | Measured joint torques          | *required* |
| `tau_est`  | `ndarray` | Estimated joint torques         | *required* |
| `param`    | `dict`    | Settings including idx_tau_stop | *required* |

Returns:

| Name      | Type | Description                |
| --------- | ---- | -------------------------- |
| `ndarray` |      | Identified base parameters |

Source code in `src/figaroh/identification/identification_tools.py`

```
def weigthed_least_squares(robot, phi_b, W_b, tau_meas, tau_est, identif_config):
    """Compute weighted least squares solution for parameter identification.

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

    Args:
        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:
        ndarray: Identified base parameters
    """
    sigma = np.zeros(robot.model.nq)  # For ground reaction force model
    P = np.zeros((len(tau_meas), len(tau_meas)))
    nb_samples = int(identif_config["idx_tau_stop"][0])
    start_idx = int(0)
    for ii in range(robot.model.nq):
        tau_slice = slice(int(start_idx), int(identif_config["idx_tau_stop"][ii]))
        diff = tau_meas[tau_slice] - tau_est[tau_slice]
        denom = len(tau_meas[tau_slice]) - len(phi_b)
        sigma[ii] = np.linalg.norm(diff) / denom

        start_idx = identif_config["idx_tau_stop"][ii]

        for jj in range(nb_samples):
            idx = jj + ii * nb_samples
            P[idx, idx] = 1 / sigma[ii]

        phi_b = np.matmul(np.linalg.pinv(np.matmul(P, W_b)), np.matmul(P, tau_meas))

    return phi_b  # full precision (#142)
```

## `calculate_first_second_order_differentiation(model, q, identif_config, dt=None, backend=None)`

Estimate interval tangent velocities and their coordinate derivatives.

Configuration differences use the model/backend Lie-group operation, so positions have width `nq` and velocities/accelerations have width `nv`. Effort-selection flags do not determine these dimensions.

Parameters:

| Name             | Type               | Description                                                                                                                                                              | Default    |
| ---------------- | ------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ---------- |
| `model`          | `Model`            | Robot model; unused when backend is supplied.                                                                                                                            | *required* |
| `q`              | `ndarray`          | Finite configurations of shape (n_samples, nq), with at least three samples. Manifold configurations must be valid for the selected model (e.g. normalized quaternions). | *required* |
| `identif_config` | `dict`             | Contains positive finite ts when dt is None.                                                                                                                             | *required* |
| `dt`             | `float or ndarray` | Positive finite sample interval, or one interval per consecutive configuration pair (n_samples - 1). Pass timestamp differences, not absolute timestamps.                | `None`     |
| `backend`        | `DynamicsBackend`  | Supplies nq, nv and compute_difference instead of the Pinocchio model.                                                                                                   | `None`     |

Returns:

| Name    | Type | Description                                                      |
| ------- | ---- | ---------------------------------------------------------------- |
| `tuple` |      | Positions (n_samples - 2, nq), velocities and accelerations      |
|         |      | (n_samples - 2, nv). All tangent coordinates are differentiated. |

Raises:

| Type         | Description                                        |
| ------------ | -------------------------------------------------- |
| `ValueError` | Invalid configurations, sample count or timesteps. |

Note

Preserves the historical alignment: q[:-2] and the first n_samples-2 forward interval velocities. Velocities represent interval midpoints, not the returned position timestamps. Acceleration is the gradient of those tangent components at the midpoint times (one-sided at endpoints). No interpolation to position timestamps or moving-frame transport is performed. Both discarded position samples are at the end.

Source code in `src/figaroh/identification/identification_tools.py`

```
def calculate_first_second_order_differentiation(
    model, q, identif_config, dt=None, backend=None
):
    """Estimate interval tangent velocities and their coordinate derivatives.

    Configuration differences use the model/backend Lie-group operation, so
    positions have width ``nq`` and velocities/accelerations have width ``nv``.
    Effort-selection flags do not determine these dimensions.

    Args:
        model (pin.Model): Robot model; unused when backend is supplied.
        q (ndarray): Finite configurations of shape (n_samples, nq), with at
            least three samples. Manifold configurations must be valid for
            the selected model (e.g. normalized quaternions).
        identif_config (dict): Contains positive finite ``ts`` when dt is None.
        dt (float or ndarray, optional): Positive finite sample interval, or
            one interval per consecutive configuration pair (n_samples - 1).
            Pass timestamp differences, not absolute timestamps.
        backend (DynamicsBackend, optional): Supplies nq, nv and
            compute_difference instead of the Pinocchio model.

    Returns:
        tuple: Positions (n_samples - 2, nq), velocities and accelerations
        (n_samples - 2, nv). All tangent coordinates are differentiated.

    Raises:
        ValueError: Invalid configurations, sample count or timesteps.

    Note:
        Preserves the historical alignment: q[:-2] and the first n_samples-2
        forward interval velocities. Velocities represent interval midpoints,
        not the returned position timestamps. Acceleration is the gradient of
        those tangent components at the midpoint times (one-sided at endpoints).
        No interpolation to position timestamps or moving-frame transport is
        performed. Both discarded position samples are at the end.
    """
    nq = backend.nq if backend is not None else model.nq
    nv = backend.nv if backend is not None else model.nv
    q = np.asarray(q, dtype=float)
    if q.ndim != 2 or q.shape[1] != nq:
        raise ValueError(f"q must have shape (n_samples, {nq})")
    if q.shape[0] < 3:
        raise ValueError("q must contain at least three samples")
    if not np.all(np.isfinite(q)):
        raise ValueError("q must contain only finite configurations")

    intervals = np.asarray(identif_config["ts"] if dt is None else dt, dtype=float)
    constant_dt = intervals.ndim == 0
    if constant_dt:
        intervals = np.full(q.shape[0] - 1, float(intervals))
    if intervals.shape != (q.shape[0] - 1,):
        raise ValueError("dt must be scalar or have one timestep per sample pair")
    if not np.all(np.isfinite(intervals)) or np.any(intervals <= 0):
        raise ValueError("Every timestep must be finite and strictly positive")

    dq = np.empty((q.shape[0] - 1, nv))
    for ii, timestep in enumerate(intervals):
        if backend is not None:
            difference = backend.compute_difference(q[ii], q[ii + 1])
        else:
            difference = pin.difference(model, q[ii], q[ii + 1])
        dq[ii] = difference / timestep

    if constant_dt:
        ddq = np.gradient(dq, axis=0, edge_order=1) / intervals[0]
    else:
        midpoint_times = np.cumsum(intervals) - 0.5 * intervals
        if not np.all(np.isfinite(midpoint_times)) or np.any(
            np.diff(midpoint_times) <= 0
        ):
            raise ValueError("dt produces invalid interval midpoint times")
        ddq = np.gradient(dq, midpoint_times, axis=0, edge_order=1)

    return q[:-2].copy(), dq[:-1], ddq[:-1]
```

## `low_pass_filter_data(data, identif_config, nbutter=5)`

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:

| Name      | Type      | Description                                                                                             | Default    |
| --------- | --------- | ------------------------------------------------------------------------------------------------------- | ---------- |
| `data`    | `ndarray` | Raw measurement data to filter                                                                          | *required* |
| `param`   | `dict`    | Filter parameters containing: - ts: Sample time - cut_off_frequency_butterworth: Cutoff frequency in Hz | *required* |
| `nbutter` | `int`     | Filter order. Higher order gives sharper frequency cutoff. Defaults to 5.                               | `5`        |

Returns:

| Name      | Type | Description                               |
| --------- | ---- | ----------------------------------------- |
| `ndarray` |      | Filtered data with border regions removed |

Note

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

Source code in `src/figaroh/identification/identification_tools.py`

```
def low_pass_filter_data(data, identif_config, nbutter=5):
    """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.

    Args:
        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:
        ndarray: Filtered data with border regions removed

    Note:
        Border effects are handled by removing nborder = 5*nbutter samples
        from start and end of filtered signal.
    """
    cutoff = identif_config["ts"] * identif_config["cut_off_frequency_butterworth"] / 2
    b, a = signal.butter(nbutter, cutoff, "low")

    padlen = 3 * (max(len(b), len(a)) - 1)
    data = signal.filtfilt(b, a, data, axis=0, padtype="odd", padlen=padlen)

    # Remove border effects
    nbord = 5 * nbutter
    data = np.delete(data, np.s_[0:nbord], axis=0)
    end_slice = slice(data.shape[0] - nbord, data.shape[0])
    data = np.delete(data, end_slice, axis=0)

    return data
```

## config

Configuration parsing and parameter management for robot identification.

This module handles all configuration-related functionality including:

- YAML configuration file parsing
- Unified to legacy config format conversion
- Parameter extraction and validation
- Signal processing and mechanical parameter management

## `unified_to_legacy_identif_config(robot, unified_identif_config)`

Convert unified identification format to legacy identif_config format.

Maps the unified identification configuration structure to the `identif_config` dict the identification classes consume.

Parameters:

| Name                     | Type           | Description                              | Default    |
| ------------------------ | -------------- | ---------------------------------------- | ---------- |
| `robot`                  | `RobotWrapper` | Robot instance containing model and data | *required* |
| `unified_identif_config` | `dict`         | Configuration from create_task_config    | *required* |

Returns:

| Name   | Type   | Description                                   |
| ------ | ------ | --------------------------------------------- |
| `dict` | `dict` | Identification configuration (identif_config) |

Example

> > > unified_config = create_task_config(robot, parsed_config, ... "identification") legacy_config = unified_to_legacy_identif_config(robot, ... unified_config)
> > >
> > > ### legacy_config has the identif_config keys

Source code in `src/figaroh/identification/config.py`

```
def unified_to_legacy_identif_config(robot, unified_identif_config) -> dict:
    """Convert unified identification format to legacy identif_config format.

    Maps the unified identification configuration structure to the
    ``identif_config`` dict the identification classes consume.

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

    Returns:
        dict: Identification configuration (``identif_config``)

    Example:
        >>> unified_config = create_task_config(robot, parsed_config,
        ...                                    "identification")
        >>> legacy_config = unified_to_legacy_identif_config(robot,
        ...                                                  unified_config)
        >>> # legacy_config has the identif_config keys
    """
    # Initialize output configuration
    identif_config = {}

    # Extract unified config sections
    mechanics = unified_identif_config.get("mechanics", {})
    joints = unified_identif_config.get("joints", {})
    problem = unified_identif_config.get("problem", {})
    coupling = unified_identif_config.get("coupling", {})
    signal_processing = unified_identif_config.get("signal_processing", {})

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

    # 2. Extract signal processing parameters
    _extract_signal_processing_params(identif_config, signal_processing)

    # 3. Extract joint limits
    _extract_joint_limits(identif_config, joints)

    # 4. Extract problem configuration
    _extract_problem_config(identif_config, problem)

    # 5. Extract mechanical parameters
    _extract_mechanical_params(identif_config, mechanics)

    # 6. Extract coupling parameters
    _extract_coupling_params(identif_config, coupling)

    # 7. Extract load parameters (defaults)
    _extract_load_params(identif_config)

    # 8. Optional physical consistency configuration (v0.4.1, default-off)
    physical_consistency = unified_identif_config.get("physical_consistency", {})
    if isinstance(physical_consistency, dict) and physical_consistency:
        identif_config["physical_consistency"] = physical_consistency

    # 9. Optional reconstruction configuration (v0.4.2, default-off)
    reconstruction = unified_identif_config.get("reconstruction", {})
    if isinstance(reconstruction, dict) and reconstruction:
        identif_config["reconstruction"] = reconstruction

    _extract_selection_config(identif_config, unified_identif_config)

    # 10. Optional held-out validation dataset (separate file/directory,
    # never a split of the training data). Mirrors calibration's
    # tasks.calibration.data.validation_data_file.
    data_section = unified_identif_config.get("data", {})
    if not isinstance(data_section, dict):
        data_section = {}
    identif_config["validation_data_file"] = data_section.get(
        "validation_data_file", ""
    )

    return identif_config
```

## parameter

Parameter management utilities for robot identification.

This module handles parameter extraction, reordering, and management including:

- Inertial parameter extraction and reordering
- Standard additional parameters (friction, actuator inertia, offsets)
- Custom parameter support
- Parameter information queries

## `reorder_inertial_parameters(pinocchio_params)`

Reorder inertial parameters from Pinocchio format to desired format.

Parameters:

| Name               | Type | Description                                                                 | Default    |
| ------------------ | ---- | --------------------------------------------------------------------------- | ---------- |
| `pinocchio_params` |      | Parameters in Pinocchio order [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz] | *required* |

Returns:

| Name   | Type | Description                                                               |
| ------ | ---- | ------------------------------------------------------------------------- |
| `list` |      | Parameters in desired order [Ixx, Ixy, Ixz, Iyy, Iyz, Izz, mx, my, mz, m] |

Source code in `src/figaroh/identification/parameter.py`

```
def reorder_inertial_parameters(pinocchio_params):
    """Reorder inertial parameters from Pinocchio format to desired format.

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

    Returns:
        list: Parameters in desired order
            [Ixx, Ixy, Ixz, Iyy, Iyz, Izz, mx, my, mz, m]
    """
    if len(pinocchio_params) != 10:
        raise ValueError(
            f"Expected 10 inertial parameters, got {len(pinocchio_params)}"
        )

    # Mapping from Pinocchio indices to desired indices
    reordered = np.zeros_like(pinocchio_params)
    reordered[0] = pinocchio_params[4]  # Ixx
    reordered[1] = pinocchio_params[5]  # Ixy
    reordered[2] = pinocchio_params[7]  # Ixz
    reordered[3] = pinocchio_params[6]  # Iyy
    reordered[4] = pinocchio_params[8]  # Iyz
    reordered[5] = pinocchio_params[9]  # Izz
    reordered[6] = pinocchio_params[1]  # mx
    reordered[7] = pinocchio_params[2]  # my
    reordered[8] = pinocchio_params[3]  # mz
    reordered[9] = pinocchio_params[0]  # m

    return reordered.tolist()
```

## `add_standard_additional_parameters(model, identif_config)`

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

Parameters:

| Name             | Type   | Description                  | Default    |
| ---------------- | ------ | ---------------------------- | ---------- |
| `model`          |        | Robot model                  | *required* |
| `identif_config` | `dict` | Identification configuration | *required* |

Returns:

| Name   | Type | Description                             |
| ------ | ---- | --------------------------------------- |
| `dict` |      | Additional parameters with their values |

Source code in `src/figaroh/identification/parameter.py`

```
def add_standard_additional_parameters(model, identif_config):
    """Add standard additional parameters (actuator inertia, friction,
    offsets).

    Args:
        model: Robot model
        identif_config (dict): Identification configuration

    Returns:
        dict: Additional parameters with their values
    """
    phi = []
    params = []

    # Standard additional parameters configuration
    additional_params = [
        {
            "name": "fv",
            "enabled_key": "has_friction",
            "values_key": "fv",
            "default": 0.0,
            "description": "viscous friction",
        },
        {
            "name": "fs",
            "enabled_key": "has_friction",
            "values_key": "fs",
            "default": 0.0,
            "description": "static friction",
        },
        {
            "name": "Ia",
            "enabled_key": "has_actuator_inertia",
            "values_key": "Ia",
            "default": 0.0,
            "description": "actuator inertia",
        },
        {
            "name": "off",
            "enabled_key": "has_joint_offset",
            "values_key": "off",
            "default": 0.0,
            "description": "joint offset",
        },
    ]

    for param_def in additional_params:
        # Disabled blocks get no columns in the regressor, so they get no
        # keys here either; keys and regressor columns must line up.
        if not identif_config.get(param_def["enabled_key"], False):
            continue
        for link_idx, jname in enumerate(model.names[1:]):  # Skip world link
            param_name = f"{param_def['name']}_{jname}"
            params.append(param_name)

            # Get parameter value
            try:
                values_list = identif_config.get(param_def["values_key"], [])
                if len(values_list) >= link_idx + 1:
                    value = values_list[link_idx]
                else:
                    value = param_def["default"]
                    logger.warning(
                        f"Missing {param_def['description']} "
                        f"for joint {jname}, using default: {value}"
                    )
            except (KeyError, IndexError, TypeError) as e:
                value = param_def["default"]
                logger.warning(
                    f"Error getting {param_def['description']} "
                    f"for joint {jname}: {e}, using default: {value}"
                )
            phi.append(value)

    return dict(zip(params, phi))
```

## `add_custom_parameters(model, custom_params)`

Add custom user-defined parameters.

Parameters:

| Name            | Type   | Description                                                                                        | Default    |
| --------------- | ------ | -------------------------------------------------------------------------------------------------- | ---------- |
| `model`         |        | Robot model                                                                                        | *required* |
| `custom_params` | `dict` | Custom parameter definitions Format: {param_name: {values: list, per_joint: bool, default: float}} | *required* |

Returns:

| Name   | Type | Description                         |
| ------ | ---- | ----------------------------------- |
| `dict` |      | Custom parameters with their values |

Source code in `src/figaroh/identification/parameter.py`

```
def add_custom_parameters(model, custom_params):
    """Add custom user-defined parameters.

    Args:
        model: Robot model
        custom_params (dict): Custom parameter definitions
            Format: {param_name: {values: list, per_joint: bool,
            default: float}}

    Returns:
        dict: Custom parameters with their values
    """
    phi = []
    params = []

    for param_name, param_def in custom_params.items():
        if not isinstance(param_def, dict):
            logger.warning(
                f"Invalid custom parameter definition for " f"'{param_name}', skipping"
            )
            continue

        values = param_def.get("values", [])
        per_joint = param_def.get("per_joint", True)
        default_value = param_def.get("default", 0.0)

        if per_joint:
            # Add parameter for each joint
            for link_idx, jname in enumerate(model.names[1:]):
                param_full_name = f"{param_name}_{jname}"
                params.append(param_full_name)

                try:
                    if len(values) >= link_idx + 1:
                        value = values[link_idx]
                    else:
                        value = default_value
                        # Only warn if values were provided but insufficient
                        if values:
                            logger.warning(
                                f"Missing value for custom "
                                f"parameter '{param_name}' joint "
                                f"{jname}, using default: {value}"
                            )
                except (IndexError, TypeError):
                    value = default_value
                    logger.warning(
                        f"Error accessing custom parameter "
                        f"'{param_name}' for joint {jname}, "
                        f"using default: {value}"
                    )

                phi.append(value)
        else:
            # Global parameter (not per joint)
            params.append(param_name)
            try:
                value = values[0] if values else default_value
            except (IndexError, TypeError):
                value = default_value
                logger.warning(
                    f"Error accessing global custom parameter "
                    f"'{param_name}', using default: {value}"
                )

            phi.append(value)

    return dict(zip(params, phi))
```

## `get_standard_parameters(model, identif_config=None)`

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

Parameters:

| Name             | Type   | Description                      | Default    |
| ---------------- | ------ | -------------------------------- | ---------- |
| `model`          |        | Robot model (Pinocchio model)    | *required* |
| `identif_config` | `dict` | Dictionary of parameter settings | `None`     |

Returns:

| Name   | Type | Description                            |
| ------ | ---- | -------------------------------------- |
| `dict` |      | Parameter names mapped to their values |

Source code in `src/figaroh/identification/parameter.py`

```
def get_standard_parameters(model, identif_config=None):
    """Get standard inertial parameters from robot model with extensible
    parameter support.

    Args:
        model: Robot model (Pinocchio model)
        identif_config (dict, optional): Dictionary of parameter settings

    Returns:
        dict: Parameter names mapped to their values
    """
    if identif_config is None:
        identif_config = {}

    phi = []
    params = []

    # Standard inertial parameter names in desired order
    # inertial_params = [
    #     "Ixx", "Ixy", "Ixz", "Iyy", "Iyz", "Izz",
    #     "mx", "my", "mz", "m"
    # ]
    inertial_params = [
        "m",
        "mx",
        "my",
        "mz",
        "Ixx",
        "Ixy",
        "Iyy",
        "Ixz",
        "Iyz",
        "Izz",
    ]

    # Extract and rearrange inertial parameters for each link
    assert len(model.inertias) == model.njoints, "Inertia count mismatch with joints"
    for link_idx, jname in enumerate(model.names[1:]):
        # Get dynamic parameters from Pinocchio (in Pinocchio order)
        # Returns the representation of the matrix as a vector of dynamic
        # parameters. The parameters are given as 𝑣=[𝑚,𝑚𝑐𝑥,𝑚𝑐𝑦,𝑚𝑐𝑧,
        # 𝐼𝑥𝑥,𝐼𝑥𝑦,𝐼𝑦𝑦,𝐼𝑥𝑧,𝐼𝑦𝑧,𝐼𝑧𝑧]^𝑇 where 𝑐 is the center
        # of mass, 𝐼=𝐼𝐶+𝑚𝑆𝑇(𝑐)𝑆(𝑐) and 𝐼𝐶 has its origin at the
        # barycenter and 𝑆(𝑐) is the the skew matrix representation of the
        # cross product operator from Vector of spatial inertias supported by
        # each joint.
        pinocchio_params = model.inertias[link_idx + 1].toDynamicParameters()

        # Rearrange from Pinocchio order [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz,
        # Iyz, Izz] to desired order [Ixx, Ixy, Ixz, Iyy, Iyz, Izz, mx, my,
        # mz, m]
        # reordered_params = reorder_inertial_parameters(pinocchio_params)
        reordered_params = pinocchio_params
        # Add parameter names and values
        for param_name in inertial_params:
            params.append(f"{param_name}_{jname}")
        phi.extend(reordered_params)

    return dict(zip(params, phi))
```

## `get_parameter_info()`

Get information about available parameter types.

Returns:

| Name   | Type | Description                                           |
| ------ | ---- | ----------------------------------------------------- |
| `dict` |      | Information about standard and custom parameter types |

Source code in `src/figaroh/identification/parameter.py`

```
def get_parameter_info():
    """Get information about available parameter types.

    Returns:
        dict: Information about standard and custom parameter types
    """
    return {
        "inertial_parameters": [
            # "Ixx", "Ixy", "Ixz", "Iyy", "Iyz", "Izz",
            # "mx", "my", "mz", "m"
            "m",
            "mx",
            "my",
            "mz",
            "Ixx",
            "Ixy",
            "Iyy",
            "Ixz",
            "Iyz",
            "Izz",
        ],
        "standard_additional": {
            "viscous_friction": {
                "name": "fv",
                "enabled_key": "has_friction",
                "values_key": "fv",
                "description": "Viscous friction coefficient",
            },
            "static_friction": {
                "name": "fs",
                "enabled_key": "has_friction",
                "values_key": "fs",
                "description": "Static friction coefficient",
            },
            "actuator_inertia": {
                "name": "Ia",
                "enabled_key": "has_actuator_inertia",
                "values_key": "Ia",
                "description": "Actuator/rotor inertia",
            },
            "joint_offset": {
                "name": "off",
                "enabled_key": "has_joint_offset",
                "values_key": "off",
                "description": "Joint position offset",
            },
        },
        "custom_parameters_format": {
            "parameter_name": {
                "values": "list of values",
                "per_joint": "boolean - if True, creates param for each joint",
                "default": "default value if not enough values provided",
            }
        },
    }
```

## physical_fit

Direct LMI effort fit over all standard parameters (public since #61; `figaroh.identification._physical_comparator` remains as an alias).

Physically consistent parameter recovery by convex (LMI) effort fits.

Three *different* objectives are available on one shared problem; none of them silently falls back to another:

1. :func:`solve_exact_reconstruction` - closest-to-prior theta that reproduces the OLS base parameters exactly (`M_full theta = phi_ols`) with a pseudo-inertia LMI per link.
1. :func:`solve_direct_effort_fit` - regularised effort fit with the same LMIs and bounds, no base equality. This is the estimator that `select_stage: physical_fit` selects in :class:`BaseIdentification`.
1. :func:`solve_per_link_projection` - nullspace representative of `phi_ols` followed by an independent LMI projection per link.

:func:`diagnose_exact` runs staged diagnostics (D0..D3) to explain why objective 1 fails on a given problem.

Typical use::

```
problem = build_problem(Y, tau, params_std, joint_names, prior=prior)
record = solve_direct_effort_fit(problem, solver="cvxopt")
if record.accepted:
    theta = record.theta  # aligned to ``problem.params_std``
```

Every solve returns a :class:`SolveRecord` that states the solver status, the base residual and per-link feasibility, so a caller can decide whether the estimate is usable. The module was promoted from the private `_physical_comparator`; that name remains as a re-export shim.

## `FixedExtras(names=(), values=(), source='zero')`

Parameters held fixed (e.g. friction); their effort is subtracted.

`names` must be column names of `Y_std`; `source` records where `values` came from (`truth`, `zero` or `declared`).

## `PhysicalPolicy(mass_min=1e-06, mass_ratio_bounds=None, feas_tol=-1e-08, base_rel_tol=1e-06, prior_weight=1e-06)`

Bounds and tolerances shared by all objectives.

`mass_min` lower-bounds every link mass, `feas_tol` is the pseudo-inertia eigenvalue tolerance of the acceptance test, `base_rel_tol` the relative base residual allowed for the exact objective and `prior_weight` the Tikhonov weight towards the prior.

## `ComparatorProblem(params_std, joint_names, Y, tau, row_weight, theta_prior, coord_scale, M_full, phi_ols, R, Qt_tau, extras, policy, theta_truth=None, base_indices=(), removed_columns=())`

Shared problem data built by :func:`build_problem`.

`Y`/`tau` are the (unweighted) regressor and effort over `params_std`; `row_weight` the per-row weights; `M_full`/`phi_ols` the base mapping and OLS base parameters used by the exact objective.

## `SolveRecord(objective, solver, solver_status, picos_status, exception, runtime_s, objective_value, theta, fallback_used, fallback_candidate, base_residual, effort_rmse_fit_per_joint, feasibility, accepted, truth_error, inputs_hash, notes=dict())`

Outcome of one solve: status, estimate, residuals and acceptance.

`accepted` is true only for an optimal solve whose every link passes the pseudo-inertia feasibility test (and, for the exact objective, whose relative base residual is within tolerance). `theta` is `None` for a non-optimal solve; no estimate is substituted.

## `build_problem(Y_std, tau, params_std, joint_names, *, prior, extras=None, policy=None, row_weight=None, qr_tol=1e-06, theta_truth=None)`

Assemble the shared problem.

`prior`/`theta_truth` are given over `params_std`. Columns named in `extras` are removed from the unknowns and their effort is subtracted from `tau`. Columns removed by the QR step stay in `M_full` as zeros so every link keeps its LMI.

Source code in `src/figaroh/identification/physical_fit.py`

```
def build_problem(
    Y_std: np.ndarray,
    tau: np.ndarray,
    params_std: Sequence[str],
    joint_names: Sequence[str],
    *,
    prior: np.ndarray,
    extras: Optional[FixedExtras] = None,
    policy: Optional[PhysicalPolicy] = None,
    row_weight: Optional[np.ndarray] = None,
    qr_tol: float = 1e-6,
    theta_truth: Optional[np.ndarray] = None,
) -> ComparatorProblem:
    """Assemble the shared problem.

    ``prior``/``theta_truth`` are given over ``params_std``. Columns named in
    ``extras`` are removed from the unknowns and their effort is subtracted
    from ``tau``. Columns removed by the QR step stay in ``M_full`` as zeros
    so every link keeps its LMI.
    """
    extras = extras or FixedExtras()
    policy = policy or PhysicalPolicy()
    Y_std = np.asarray(Y_std, dtype=float)
    tau = np.asarray(tau, dtype=float).reshape(-1)
    names = list(params_std)
    prior = np.asarray(prior, dtype=float).reshape(len(names))
    truth = (
        None
        if theta_truth is None
        else np.asarray(theta_truth, dtype=float).reshape(len(names))
    )
    if Y_std.shape != (tau.size, len(names)):
        raise ValueError(
            f"Y_std shape {Y_std.shape} inconsistent with tau {tau.shape} "
            f"and {len(names)} parameters"
        )

    # Freeze extras
    tau = tau.copy()
    keep = np.ones(len(names), dtype=bool)
    for nm, val in zip(extras.names, extras.values):
        j = names.index(nm)
        tau = tau - Y_std[:, j] * float(val)
        keep[j] = False
    names = [n for n, k in zip(names, keep) if k]
    Y = Y_std[:, keep]
    prior = prior[keep]
    truth = None if truth is None else truth[keep]

    w = (
        np.ones(tau.size)
        if row_weight is None
        else np.asarray(row_weight, dtype=float).reshape(tau.size)
    )
    Yw = w[:, None] * Y
    tw = w * tau
    n = len(names)

    # Core QR decomposer on the non-zero columns; removed columns stay zero.
    colnorm = np.linalg.norm(Yw, axis=0)
    thresh = qr_tol * max(float(colnorm.max()), 1e-300)
    kept = [i for i in range(n) if colnorm[i] > thresh]
    removed = tuple(i for i in range(n) if i not in kept)
    params_r = [names[i] for i in kept]
    dec = QRDecomposer(tolerance=qr_tol)
    M_r, _, base_r, _ = dec.get_base_mapping_matrix_double(Yw[:, kept], params_r)
    M_full = QRDecomposer.expand_mapping_matrix_to_full(M_r, params_r, names)
    base_full = tuple(kept[i] for i in base_r)
    phi_ols, *_ = np.linalg.lstsq(Yw[:, list(base_full)], tw, rcond=None)

    Q, R = np.linalg.qr(Yw, mode="reduced")
    return ComparatorProblem(
        params_std=tuple(names),
        joint_names=tuple(joint_names),
        Y=Y,
        tau=tau,
        row_weight=w,
        theta_prior=prior,
        coord_scale=_coord_scale(names, joint_names, prior),
        M_full=M_full,
        phi_ols=phi_ols,
        R=R,
        Qt_tau=Q.T @ tw,
        extras=extras,
        policy=policy,
        theta_truth=truth,
        base_indices=base_full,
        removed_columns=removed,
    )
```

## `solve_exact_reconstruction(p, *, solver='cvxopt', **opts)`

Objective 1: min ||D(theta-theta0)|| s.t. M_full theta = phi_ols, LMIs.

Source code in `src/figaroh/identification/physical_fit.py`

```
def solve_exact_reconstruction(
    p: ComparatorProblem, *, solver: str = "cvxopt", **opts: Any
) -> SolveRecord:
    """Objective 1: min ||D(theta-theta0)|| s.t. M_full theta = phi_ols, LMIs."""
    raw = _solve_core(p, "exact", solver=solver, **opts)
    return _make_record(p, "exact_reconstruction", solver=solver, raw=raw, exact=True)
```

## `solve_direct_effort_fit(p, *, solver='cvxopt', **opts)`

Objective 2: regularised effort fit with the same LMIs, no equality.

Source code in `src/figaroh/identification/physical_fit.py`

```
def solve_direct_effort_fit(
    p: ComparatorProblem, *, solver: str = "cvxopt", **opts: Any
) -> SolveRecord:
    """Objective 2: regularised effort fit with the same LMIs, no equality."""
    raw = _solve_core(p, "direct", solver=solver, **opts)
    return _make_record(p, "direct_effort_fit", solver=solver, raw=raw)
```

## `solve_per_link_projection(p, *, solver='cvxopt')`

Objective 3: nullspace representative, then per-link LMI projection.

Source code in `src/figaroh/identification/physical_fit.py`

```
def solve_per_link_projection(
    p: ComparatorProblem, *, solver: str = "cvxopt"
) -> SolveRecord:
    """Objective 3: nullspace representative, then per-link LMI projection."""
    t0 = time.perf_counter()
    raw: Dict[str, Any] = {
        "status": "error",
        "picos_status": None,
        "exception": None,
        "objective_value": None,
        "theta": None,
    }
    try:
        D = p.coord_scale
        A = p.M_full * D[None, :]
        z = np.linalg.pinv(A) @ (p.phi_ols - p.M_full @ p.theta_prior)
        theta = p.theta_prior + D * z
        total = 0.0
        for j, idx in _link_indices(p).items():
            cols = [idx[k] for k in _INERTIAL_KEYS]
            p10, rep = project_p10_lmi(
                theta[cols],
                mass_min=p.policy.mass_min,
                psd_eig_tol=p.policy.feas_tol,
                solver=solver,
                mass_bounds=_mass_bounds(p, idx),
            )
            if rep.status == "error":
                raise RuntimeError(f"link {j}: {rep.message}")
            theta[cols] = p10
            total += float(rep.objective or 0.0)
        raw.update(status="optimal", picos_status="optimal", theta=theta)
        raw["objective_value"] = total
    except Exception as exc:
        raw["exception"] = f"{type(exc).__name__}: {exc}"
        raw["theta"] = None
    raw["runtime_s"] = time.perf_counter() - t0
    return _make_record(p, "per_link_projection", solver=solver, raw=raw)
```

## `diagnose_exact(p, *, solver='cvxopt')`

Staged diagnostics for objective 1 (D0 reproduce, D1 phase-I, D2 one-change variants, D3 relaxed equality).

Source code in `src/figaroh/identification/physical_fit.py`

```
def diagnose_exact(
    p: ComparatorProblem, *, solver: str = "cvxopt"
) -> List[SolveRecord]:
    """Staged diagnostics for objective 1 (D0 reproduce, D1 phase-I,
    D2 one-change variants, D3 relaxed equality)."""
    records: List[SolveRecord] = []

    # D0: reproduce with the production reconstruction entry point.
    t0 = time.perf_counter()
    raw: Dict[str, Any] = {
        "status": "error",
        "picos_status": None,
        "exception": None,
        "theta": None,
    }
    try:
        res = reconstruct_full_parameters(
            (p.M_full, p.phi_ols, list(p.params_std)),
            method="sdp",
            theta0=p.theta_prior,
            weights=1.0 / p.coord_scale,
            joint_names=list(p.joint_names),
            mass_min=p.policy.mass_min,
            solver=solver,
        )
        raw["objective_value"] = res.objective
        if res.status == "ok":
            raw.update(status="optimal", theta=res.theta_r)
        else:
            # Legacy code silently fell back; keep it only as a candidate.
            raw["status"] = "error"
            raw["exception"] = f"reconstruct_full_parameters status={res.status}"
            raw["fallback_candidate"] = res.theta_r
    except Exception as exc:
        raw["exception"] = f"{type(exc).__name__}: {exc}"
    raw["runtime_s"] = time.perf_counter() - t0
    records.append(
        _make_record(
            p, "D0:reconstruct_full_parameters_sdp", solver=solver, raw=raw, exact=True
        )
    )

    # D1: phase-I feasibility.
    raw = _solve_core(p, "phase1", solver=solver, bounds=False)
    s_star = raw.get("s_star")
    notes: Dict[str, Any] = {"s_star": s_star}
    if raw["status"] == "optimal" and s_star is not None:
        notes["label"] = "infeasible (certificate s*<0)" if s_star < 0 else "feasible"
        if s_star < 0:
            raw["theta"] = None  # a strictly infeasible point is not a solution
    else:
        notes["label"] = "solver failure"
    rec = _make_record(p, "D1:phase1", solver=solver, raw=raw, exact=True, notes=notes)
    rec.objective_value = s_star
    rec.accepted = False
    records.append(rec)

    # D2: one change at a time.
    variants = {
        "schur_norm": {"norm": "schur"},
        "scaling_off": {"scaling": False},
        "params_r_only": {"variables": "params_r"},
        "bounds_off": {"bounds": False},
    }
    for name, opts in variants.items():
        raw = _solve_core(p, "exact", solver=solver, **opts)
        records.append(
            _make_record(p, f"D2:{name}", solver=solver, raw=raw, exact=True)
        )

    # D3: relaxed equality.
    raw = _solve_core(p, "relaxed", solver=solver)
    records.append(_make_record(p, "D3:relaxed_equality", solver=solver, raw=raw))
    return records
```

## selection

Which estimate an identification run reports (#61).

`tasks.identification.select_stage` picks the estimate that validation, `verify()`, the reports, the run archive and the URDF export use:

- `fit` (default): the base-parameter least-squares fit, unchanged.
- `reconstruction`: the full parameter vector reconstructed from the fit, accepted only when it is physically consistent and was produced by the requested method.
- `physical_fit`: the direct LMI effort fit (:func:`figaroh.identification.physical_fit.solve_direct_effort_fit`).

A requested stage that does not meet its acceptance conditions is *rejected*: nothing else is substituted, the selected stage is `none` and the verdict fails. The fit numbers stay under their usual result keys.

## `SelectedEstimate(stage, requested, status, reason, space, names=list(), values=None, phi_base_equivalent=None, base_residual_rel=None, feasibility=dict(), effective_method=None, solvers=list(), psd_eig_tol=-1e-10, extra=dict())`

The estimate chosen by `select_stage` and the verdict on it.

Attributes:

| Name                  | Type                        | Description                                                                                              |
| --------------------- | --------------------------- | -------------------------------------------------------------------------------------------------------- |
| `stage`               | `str`                       | Stage whose numbers are reported: fit, reconstruction or physical_fit when accepted, none when rejected. |
| `requested`           | `str`                       | The stage that was asked for.                                                                            |
| `status`              | `str`                       | accepted or rejected.                                                                                    |
| `reason`              | `str`                       | Why, in one line (names the solver(s) and the failed test).                                              |
| `space`               | `str`                       | standard (values are the full standard parameter vector).                                                |
| `names`               | `List[str]`                 | Parameter names aligned with values.                                                                     |
| `values`              | `Optional[ndarray]`         | The candidate vector; kept for diagnosis when rejected.                                                  |
| `phi_base_equivalent` | `Optional[ndarray]`         | M @ theta for a standard candidate (diagnostic only, never used for prediction).                         |
| `base_residual_rel`   | `Optional[float]`           | Relative distance of phi_base_equivalent from the fitted base parameters.                                |
| `feasibility`         | `Dict[str, Dict[str, Any]]` | Per link {mass, min_eig, ok} for standard candidates.                                                    |
| `effective_method`    | `Optional[str]`             | Method that produced a reconstruction.                                                                   |
| `solvers`             | `List[str]`                 | Solver names tried for a physical fit.                                                                   |
| `psd_eig_tol`         | `float`                     | Pseudo-inertia eigenvalue tolerance the links were judged feasible with; export uses the same one.       |

### `predict(W_full)`

Effort predicted by this estimate: the full regressor times the standard parameter vector (never `phi_base_equivalent`).

Source code in `src/figaroh/identification/selection.py`

```
def predict(self, W_full: np.ndarray) -> np.ndarray:
    """Effort predicted by this estimate: the full regressor times the
    standard parameter vector (never ``phi_base_equivalent``)."""
    if self.values is None:
        raise ValueError("no estimate values to predict with")
    return W_full @ self.values
```

### `parameter_dict()`

`{name: value}` of the candidate (empty when no values).

Source code in `src/figaroh/identification/selection.py`

```
def parameter_dict(self) -> Dict[str, float]:
    """``{name: value}`` of the candidate (empty when no values)."""
    if self.values is None:
        return {}
    return {n: float(v) for n, v in zip(self.names, self.values)}
```

### `link_p10(joint_names)`

Per-link 10-vectors of a standard-space estimate.

Source code in `src/figaroh/identification/selection.py`

```
def link_p10(self, joint_names: Sequence[str]) -> Dict[str, np.ndarray]:
    """Per-link 10-vectors of a standard-space estimate."""
    if self.space != "standard":
        raise ValueError("link inertias need a standard-space estimate")
    from figaroh.identification.physical_consistency import (
        p10_by_joint_from_param_dict,
    )

    return p10_by_joint_from_param_dict(
        parameter_dict=self.parameter_dict(), joint_names=joint_names
    )
```

## `requested_stage(identif)`

The configured `select_stage` (`fit` when absent).

Source code in `src/figaroh/identification/selection.py`

```
def requested_stage(identif) -> str:
    """The configured ``select_stage`` (``fit`` when absent)."""
    cfg = getattr(identif, "identif_config", None) or {}
    return str(cfg.get("select_stage") or DEFAULT_STAGE)
```

## `physical_fit_config(identif)`

The `physical_fit` settings with every key present.

The config parser already merges the defaults; a programmatic config may hold a partial mapping (or none), which is completed here.

Source code in `src/figaroh/identification/selection.py`

```
def physical_fit_config(identif) -> Dict[str, Any]:
    """The ``physical_fit`` settings with every key present.

    The config parser already merges the defaults; a programmatic config
    may hold a partial mapping (or none), which is completed here.
    """
    from figaroh.identification.config import PHYSICAL_FIT_DEFAULTS

    raw = (getattr(identif, "identif_config", None) or {}).get("physical_fit")
    if isinstance(raw, dict) and PHYSICAL_FIT_DEFAULTS.keys() <= raw.keys():
        return dict(raw)
    cfg = dict(PHYSICAL_FIT_DEFAULTS)
    if isinstance(raw, dict):
        cfg.update(raw)
    return cfg
```

## `run_physical_fit(identif)`

Solve the direct LMI effort fit on the data the fit used.

Returns an outcome dict (never raises): `record` (a :class:`~figaroh.identification.physical_fit.SolveRecord` or `None`), `solvers` (names tried), `reason` and `accepted`. The processed regressor is scattered into all standard columns (zeros where columns were eliminated); friction and offsets are free parameters and the prior is the nominal standard parameter vector.

Source code in `src/figaroh/identification/selection.py`

```
def run_physical_fit(identif) -> Dict[str, Any]:
    """Solve the direct LMI effort fit on the data the fit used.

    Returns an outcome dict (never raises): ``record`` (a
    :class:`~figaroh.identification.physical_fit.SolveRecord` or ``None``),
    ``solvers`` (names tried), ``reason`` and ``accepted``. The processed
    regressor is scattered into all standard columns (zeros where columns
    were eliminated); friction and offsets are free parameters and the
    prior is the nominal standard parameter vector.
    """
    cfg = physical_fit_config(identif)
    solvers = [str(cfg["solver"])]
    outcome: Dict[str, Any] = {
        "record": None,
        "solvers": solvers,
        "reason": "",
        "accepted": False,
        "config": cfg,
    }
    try:
        from figaroh.identification import physical_fit as pf

        names = list(identif.standard_parameter.keys())
        n = len(names)
        W = np.asarray(identif._solve_W, dtype=float)
        tau = np.asarray(identif._solve_tau, dtype=float).reshape(-1)
        eliminated = set(int(i) for i in (identif._idx_eliminated or []))
        keep = [i for i in range(n) if i not in eliminated]
        if W.shape[1] != len(keep):
            raise ValueError(
                f"processed regressor has {W.shape[1]} columns, expected {len(keep)}"
            )
        Y = np.zeros((W.shape[0], n))
        Y[:, keep] = W
        problem = pf.build_problem(
            Y,
            tau,
            names,
            list(identif.model.names[1:]),
            prior=_nominal_vector(identif),
            policy=pf.PhysicalPolicy(
                mass_min=float(cfg["mass_min"]),
                prior_weight=float(cfg["prior_weight"]),
            ),
            row_weight=getattr(identif, "_wls_row_weight", None),
        )
        opts = {}
        if cfg["max_seconds"] is not None:
            opts["max_seconds"] = float(cfg["max_seconds"])
        record = pf.solve_direct_effort_fit(problem, solver=solvers[0], **opts)
        steps = [f"{solvers[0]}: {record.solver_status}"]
        if record.solver_status != "optimal" and cfg["second_solver"]:
            second = str(cfg["second_solver"])
            solvers.append(second)
            record = pf.solve_direct_effort_fit(problem, solver=second, **opts)
            steps.append(f"{second}: {record.solver_status}")
        outcome["record"] = record
        outcome["psd_eig_tol"] = problem.policy.feas_tol
        outcome["accepted"] = bool(
            record.solver_status == "optimal" and record.accepted
        )
        verdict = (
            "all links feasible"
            if outcome["accepted"]
            else (
                "a link is not physically feasible"
                if record.solver_status == "optimal"
                else "no optimal solution"
            )
        )
        outcome["reason"] = "; ".join(steps) + f"; {verdict}"
    except Exception as exc:  # solver stack missing, shape problems, ...
        logger.warning("physical fit failed: %s", exc)
        outcome["reason"] = f"{type(exc).__name__}: {exc}"
    return outcome
```

## `select_estimate(identif)`

Build the :class:`SelectedEstimate` for the configured stage.

Source code in `src/figaroh/identification/selection.py`

```
def select_estimate(identif) -> SelectedEstimate:
    """Build the :class:`SelectedEstimate` for the configured stage."""
    stage = requested_stage(identif)
    if stage == DEFAULT_STAGE:
        raise ValueError("select_stage='fit' reports the base fit; nothing to select")
    if stage == "reconstruction":
        return _select_reconstruction(identif)
    if stage == "physical_fit":
        return _select_physical_fit(identif)
    raise ValueError(f"select_stage={stage!r} is not selectable")
```

## Physical inertial conventions

Physical-consistency utilities use Pinocchio dynamic-parameter order: `[m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz]`. The first moments are `h = m*c`; the six tensor entries describe rotational inertia `I_O` about the link-frame origin. They are distinct from inertia `I_C` about the centre of mass: `I_O = I_C + m * ((c.T*c)*eye(3) - c*c.T)`.

The physical constraint is positive semidefiniteness of the pseudo-inertia `P = [[Sigma, h], [h.T, m]]`, with `Sigma = 0.5*trace(I_O)*eye(3) - I_O`. Projection uses this second-moment block as its decision variable and converts back using `I_O = trace(Sigma)*eye(3) - Sigma`. Its objective and weights apply to the ten dynamic parameters. SDP reconstruction imposes the same pseudo-inertia constraint while preserving its base-parameter equalities.

The existing CAD `com_bounds` option bounds first moments `h`, in kg\*m, rather than CoM coordinates in metres. Physical feasibility does not establish identifiability or complete inertial URDF export.

# Integration

High-level, one-line API for identification workflows. See [Concepts → Integration API](https://thanhndv212.github.io/figaroh-plus/concepts/integration/index.md) for usage and when to reach for this instead of subclassing `BaseIdentification` directly.

## api

Integration API for FIGAROH identification workflows.

Provides the high-level RobotIdentificationSystem class that wraps the backend abstraction and BaseIdentification workflow into a simple, one-line API.

## `IdentificationResult(phi_base=None, params_base=None, phi_standard=None, rms_error=None, correlation=None, backend='', model_path='', config=None, raw=None)`

Container for identification results.

Attributes:

| Name           | Type                | Description                                                   |
| -------------- | ------------------- | ------------------------------------------------------------- |
| `phi_base`     | `Optional[ndarray]` | Identified base parameters                                    |
| `params_base`  | `Optional[list]`    | Base parameter names                                          |
| `phi_standard` | `Optional[ndarray]` | Full standard parameters (if reconstruction enabled)          |
| `rms_error`    | `Optional[float]`   | RMS error of the identification                               |
| `correlation`  | `Optional[float]`   | Correlation between measured and estimated torques            |
| `backend`      | `str`               | Name of the backend used                                      |
| `model_path`   | `str`               | Path to the robot model                                       |
| `config`       | `Optional[dict]`    | Configuration used                                            |
| `raw`          | `Optional[dict]`    | Raw result dict from BaseIdentification (for advanced access) |

## `RobotIdentificationSystem(robot, backend_name='pinocchio', **kwargs)`

High-level API for robot dynamic parameter identification.

Provides a simple interface that wraps the backend abstraction and BaseIdentification workflow. Supports switching between dynamics backends (Pinocchio, MuJoCo) with minimal code changes.

Example

> > > ### Create system from URDF
> > >
> > > system = RobotIdentificationSystem.from_urdf("robot.urdf")
> > >
> > > ### Or with specific backend
> > >
> > > system = RobotIdentificationSystem.from_urdf( ... "robot.urdf", backend="mujoco" ... )
> > >
> > > ### Run identification
> > >
> > > results = system.identify_parameters( ... config="config/robot_config.yaml", ... data_dir="data/", ... plotting=False ... ) print(f"Identified {len(results.phi_base)} base parameters") print(f"RMS error: {results.rms_error}")

Initialize identification system.

Parameters:

| Name           | Type | Description                                             | Default       |
| -------------- | ---- | ------------------------------------------------------- | ------------- |
| `robot`        |      | Robot model (figaroh.tools.robot.Robot or RobotWrapper) | *required*    |
| `backend_name` |      | Name of the dynamics backend ('pinocchio', 'mujoco')    | `'pinocchio'` |
| `**kwargs`     |      | Additional configuration                                | `{}`          |

Source code in `src/figaroh/integration/api.py`

```
def __init__(self, robot, backend_name="pinocchio", **kwargs):
    """Initialize identification system.

    Args:
        robot: Robot model (figaroh.tools.robot.Robot or RobotWrapper)
        backend_name: Name of the dynamics backend ('pinocchio', 'mujoco')
        **kwargs: Additional configuration
    """
    self.robot = robot
    self.backend_name = backend_name
    self._identification = None
    self._config = kwargs
```

### `backend`

Access the underlying dynamics backend.

### `nq`

Number of position variables.

### `nv`

Number of velocity variables.

### `from_urdf(urdf_path, backend='pinocchio', package_dirs=None, free_flyer=False, **kwargs)`

Create identification system from URDF file.

Parameters:

| Name           | Type | Description                          | Default       |
| -------------- | ---- | ------------------------------------ | ------------- |
| `urdf_path`    |      | Path to URDF file                    | *required*    |
| `backend`      |      | Backend name ('pinocchio', 'mujoco') | `'pinocchio'` |
| `package_dirs` |      | Package directories for mesh files   | `None`        |
| `free_flyer`   |      | Whether to add free-flyer joint      | `False`       |
| `**kwargs`     |      | Additional arguments for load_robot  | `{}`          |

Returns:

| Type | Description                        |
| ---- | ---------------------------------- |
|      | RobotIdentificationSystem instance |

Source code in `src/figaroh/integration/api.py`

```
@classmethod
def from_urdf(
    cls,
    urdf_path,
    backend="pinocchio",
    package_dirs=None,
    free_flyer=False,
    **kwargs,
):
    """Create identification system from URDF file.

    Args:
        urdf_path: Path to URDF file
        backend: Backend name ('pinocchio', 'mujoco')
        package_dirs: Package directories for mesh files
        free_flyer: Whether to add free-flyer joint
        **kwargs: Additional arguments for load_robot

    Returns:
        RobotIdentificationSystem instance
    """
    from ..tools.load_robot import load_robot

    robot = load_robot(
        urdf_path,
        package_dirs=package_dirs,
        isFext=free_flyer,
        loader="figaroh",
        **kwargs,
    )
    return cls(robot, backend_name=backend, **kwargs)
```

### `from_mjcf(mjcf_path, backend='mujoco', **kwargs)`

Create identification system from MJCF file.

.. note:: MJCF support is not yet implemented. The identification workflow requires a Robot object (URDF-based). Full MJCF support requires refactoring BaseIdentification to accept a DynamicsBackend directly — planned for a future release.

Parameters:

| Name        | Type | Description                                  | Default    |
| ----------- | ---- | -------------------------------------------- | ---------- |
| `mjcf_path` |      | Path to MJCF file                            | *required* |
| `backend`   |      | Backend name (defaults to 'mujoco' for MJCF) | `'mujoco'` |
| `**kwargs`  |      | Additional arguments                         | `{}`       |

Raises:

| Type                  | Description                            |
| --------------------- | -------------------------------------- |
| `NotImplementedError` | Always raised — MJCF not yet supported |

Source code in `src/figaroh/integration/api.py`

```
@classmethod
def from_mjcf(cls, mjcf_path, backend="mujoco", **kwargs):
    """Create identification system from MJCF file.

    .. note::
        MJCF support is not yet implemented. The identification workflow
        requires a Robot object (URDF-based). Full MJCF support requires
        refactoring BaseIdentification to accept a DynamicsBackend
        directly — planned for a future release.

    Args:
        mjcf_path: Path to MJCF file
        backend: Backend name (defaults to 'mujoco' for MJCF)
        **kwargs: Additional arguments

    Raises:
        NotImplementedError: Always raised — MJCF not yet supported
    """
    raise NotImplementedError(
        "from_mjcf is not yet supported. The identification workflow "
        "requires a Robot object (URDF-based). Use from_urdf instead. "
        "Full MJCF support requires refactoring BaseIdentification to "
        "accept a DynamicsBackend directly — planned for a future release."
    )
```

### `identify_parameters(config, data_dir=None, truncate=None, decimate=True, decimation_factor=10, zero_tolerance=0.001, plotting=False, save_results=False, **kwargs)`

Run parameter identification.

This is the main entry point. It:

1. Creates a BaseIdentification subclass instance
1. Loads configuration from YAML
1. Processes trajectory data from CSV files
1. Computes the regressor
1. Solves for base parameters
1. Returns structured results

Parameters:

| Name                | Type | Description                                             | Default    |
| ------------------- | ---- | ------------------------------------------------------- | ---------- |
| `config`            |      | Path to YAML config file                                | *required* |
| `data_dir`          |      | Directory containing trajectory data (overrides config) | `None`     |
| `truncate`          |      | Optional truncation indices for data                    | `None`     |
| `decimate`          |      | Whether to decimate the regressor                       | `True`     |
| `decimation_factor` |      | Decimation factor (default: 10)                         | `10`       |
| `zero_tolerance`    |      | Tolerance for zero column elimination (default: 0.001)  | `0.001`    |
| `plotting`          |      | Whether to generate plots (default: False)              | `False`    |
| `save_results`      |      | Whether to save results to file (default: False)        | `False`    |
| `**kwargs`          |      | Additional solver arguments                             | `{}`       |

Returns:

| Type | Description                                                 |
| ---- | ----------------------------------------------------------- |
|      | IdentificationResult with identified parameters and metrics |

Raises:

| Type                | Description                       |
| ------------------- | --------------------------------- |
| `ValueError`        | If no data source is configured   |
| `FileNotFoundError` | If config or data files not found |

Source code in `src/figaroh/integration/api.py`

```
def identify_parameters(
    self,
    config,
    data_dir=None,
    truncate=None,
    decimate=True,
    decimation_factor=10,
    zero_tolerance=0.001,
    plotting=False,
    save_results=False,
    **kwargs,
):
    """Run parameter identification.

    This is the main entry point. It:
    1. Creates a BaseIdentification subclass instance
    2. Loads configuration from YAML
    3. Processes trajectory data from CSV files
    4. Computes the regressor
    5. Solves for base parameters
    6. Returns structured results

    Args:
        config: Path to YAML config file
        data_dir: Directory containing trajectory data (overrides config)
        truncate: Optional truncation indices for data
        decimate: Whether to decimate the regressor
        decimation_factor: Decimation factor (default: 10)
        zero_tolerance: Tolerance for zero column elimination (default: 0.001)
        plotting: Whether to generate plots (default: False)
        save_results: Whether to save results to file (default: False)
        **kwargs: Additional solver arguments

    Returns:
        IdentificationResult with identified parameters and metrics

    Raises:
        ValueError: If no data source is configured
        FileNotFoundError: If config or data files not found
    """
    # Override data_dir in config if provided
    _created_temp_config = False
    if data_dir is not None:
        import tempfile
        import yaml

        # Read existing config and add/override data_dir
        if isinstance(config, str):
            with open(config, "r") as f:
                cfg = yaml.safe_load(f)
        else:
            cfg = config

        # Inject data_dir into the config
        if "identification" in cfg:
            if isinstance(cfg["identification"], dict):
                cfg["identification"]["data_dir"] = data_dir
            elif isinstance(cfg["identification"], list):
                cfg["identification"][0]["data_dir"] = data_dir
        elif "calibration" in cfg:
            if isinstance(cfg["calibration"], dict):
                cfg["calibration"]["data_dir"] = data_dir
            elif isinstance(cfg["calibration"], list):
                cfg["calibration"][0]["data_dir"] = data_dir
        else:
            cfg["data_dir"] = data_dir

        # Write modified config to a temp file
        tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False)
        yaml.dump(cfg, tmp)
        tmp.close()
        config_file = tmp.name
        _created_temp_config = True
    elif isinstance(config, str):
        config_file = config
    else:
        raise ValueError(
            "config must be a file path (string). "
            "Use data_dir= to specify the trajectory data directory."
        )

    # Extract data_dir from config if not passed explicitly
    if data_dir is None and isinstance(config, str):
        import yaml

        try:
            with open(config_file, "r") as f:
                cfg = yaml.safe_load(f)
            # unified configs nest the task sections under ``tasks:``
            sections = cfg.get("tasks", cfg)
            for section in ("identification", "calibration"):
                if section in sections and isinstance(sections[section], dict):
                    data_dir = sections[section].get("data_dir", data_dir)
        except Exception:
            pass
    elif data_dir is None and not isinstance(config, str):
        if isinstance(config, dict):
            sections = config.get("tasks", config)
            for section in ("identification", "calibration"):
                if section in sections and isinstance(sections[section], dict):
                    data_dir = sections[section].get("data_dir", data_dir)

    # Create the concrete identification instance
    identification = _SimpleIdentification(
        self.robot, config_file, data_dir=data_dir
    )

    try:
        # Initialize: process data, compute regressor, compute reference torque
        identification.initialize(truncate=truncate)

        # Solve for base parameters
        phi_base = identification.solve(
            decimate=decimate,
            decimation_factor=decimation_factor,
            zero_tolerance=zero_tolerance,
            plotting=plotting,
            save_results=save_results,
        )

        # Extract results
        result_dict = identification.result or {}

        return IdentificationResult(
            phi_base=phi_base,
            params_base=result_dict.get("params_base"),
            phi_standard=result_dict.get("phi_standard"),
            rms_error=identification.rms_error,
            correlation=identification.correlation,
            backend=self.backend_name,
            model_path=getattr(self.robot, "robot_urdf", ""),
            config=identification.identif_config,
            raw=result_dict,
        )
    finally:
        # Clean up temp config file if we created one
        if _created_temp_config:
            try:
                os.unlink(config_file)
            except OSError:
                pass
```

# Measurements

## `Measurement(name, joint, frame, type, value)`

Class for handling different types of measurements.

Initialize measurement.

Parameters:

| Name    | Type | Description                                | Default    |
| ------- | ---- | ------------------------------------------ | ---------- |
| `name`  |      | Name of measurement                        | *required* |
| `joint` |      | Joint where measurement is expressed       | *required* |
| `frame` |      | Closest frame from measurement             | *required* |
| `type`  |      | Type of measurement (SE3, wrench, current) | *required* |
| `value` |      | 6D measurement value wrt joint placement   | *required* |

Source code in `src/figaroh/measurements/measurement.py`

```
def __init__(self, name, joint, frame, type, value):
    """Initialize measurement.

    Args:
        name: Name of measurement
        joint: Joint where measurement is expressed
        frame: Closest frame from measurement
        type: Type of measurement (SE3, wrench, current)
        value: 6D measurement value wrt joint placement
    """
    self.name = name
    self.joint = joint
    self.frame = frame
    self.type = type
    if self.type == "SE3":
        self.SE3_value = pin.SE3(
            pin.rpyToMatrix(np.array([value[3], value[4], value[5]])),
            np.array([value[0], value[1], value[2]]),
        )
    elif self.type == "wrench":
        self.wrench_value = pin.Force(value)
    elif self.type == "current":
        self.current_value = value
    else:
        raise TypeError("The type of your measurement is not valid")
```

### `add_SE3_measurement(model)`

Add SE3 measurement to model.

Parameters:

| Name    | Type | Description                           | Default    |
| ------- | ---- | ------------------------------------- | ---------- |
| `model` |      | Pinocchio model to add measurement to | *required* |

Returns:

| Name    | Type | Description            |
| ------- | ---- | ---------------------- |
| `tuple` |      | Updated model and data |

Source code in `src/figaroh/measurements/measurement.py`

```
def add_SE3_measurement(self, model):
    """Add SE3 measurement to model.

    Args:
        model: Pinocchio model to add measurement to

    Returns:
        tuple: Updated model and data
    """
    if self.type == "SE3":
        self.frame_index = model.addFrame(
            pin.Frame(
                self.name,
                model.getJointId(self.joint),
                model.getFrameId(self.frame),
                self.SE3_value,
                pin.OP_FRAME,
            ),
            "False",
        )
    data = model.createData()
    return model, data
```

# Optimal

Optimal calibration and trajectory optimization module.

This module provides base classes for optimal calibration and trajectory optimization for robotic systems.

## `BaseOptimalCalibration(robot, config_file='config/robot_config.yaml')`

Bases: `ABC`

Base class for robot optimal configuration generation for calibration.

This class implements the framework for generating optimal robot configurations that maximize the observability of kinematic parameters during calibration. It uses Second-Order Cone Programming (SOCP) to solve the D-optimal design problem for parameter estimation.

The class provides a Template Method pattern where the main workflow is defined, but specific optimization strategies can be customized by derived classes for different robot types.

Workflow

1. Load candidate configurations from file (CSV or YAML)
1. Calculate kinematic regressors for all candidates
1. Compute information matrices for each configuration
1. Solve SOCP optimization to find optimal subset
1. Select configurations with significant weights
1. Visualize and save results

Key Features

- D-optimal experimental design for calibration
- Support for multiple calibration models (full_params, joint_offset)
- Automatic minimum configuration calculation
- SOCP-based optimization with convex relaxation
- Comprehensive visualization and analysis tools
- File I/O for configuration management

Mathematical Background

The method maximizes the determinant of the Fisher Information Matrix: max det(Σᵢ wᵢ Rᵢᵀ Rᵢ) subject to Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

Where:

- Rᵢ is the kinematic regressor for configuration i
- wᵢ is the weight assigned to configuration i
- The objective maximizes parameter estimation precision

Attributes:

| Name                     | Type      | Description                                    |
| ------------------------ | --------- | ---------------------------------------------- |
| `robot`                  |           | Robot model instance loaded with FIGAROH       |
| `model`                  |           | Pinocchio robot model                          |
| `data`                   |           | Pinocchio robot data                           |
| `calib_config`           | `dict`    | Calibration parameters from configuration file |
| `optimal_configurations` | `dict`    | Selected optimal configurations                |
| `optimal_weights`        | `ndarray` | Weights assigned to configurations             |
| `minNbChosen`            | `int`     | Minimum number of configurations required      |
| `R_rearr`                | `ndarray` | Rearranged kinematic regressor matrix          |
| `detroot_whole`          | `float`   | Determinant root of full information matrix    |
| `w_list`                 | `list`    | Solution weights from SOCP optimization        |
| `w_dict_sort`            | `dict`    | Sorted weights by configuration index          |

Example

> > > ### Basic usage for TIAGo robot
> > >
> > > from figaroh.robots import TiagoRobot robot = TiagoRobot()
> > >
> > > ### Create optimal calibration instance
> > >
> > > opt_calib = TiagoOptimalCalibration(robot, "config/tiago.yaml")
> > >
> > > ### Generate optimal configurations
> > >
> > > opt_calib.solve(save_file=True)
> > >
> > > ### Access results
> > >
> > > print(f"Selected {len(opt_calib.optimal_configurations)} configs") print(f"Minimum required: {opt_calib.minNbChosen}")

See Also

BaseCalibration: Main calibration framework SOCPOptimizer: Second-order cone programming solver TiagoOptimalCalibration: TIAGo-specific implementation UR10OptimalCalibration: UR10-specific implementation

Initialize optimal calibration with robot model and configuration.

Sets up the optimal calibration framework by loading robot parameters, initializing optimization attributes, and calculating the minimum number of configurations required based on the calibration model.

The minimum number of configurations is computed to ensure the optimization problem is well-posed and identifiable:

- For full_params: considers all kinematic parameters
- For joint_offset: considers only joint offset parameters

Parameters:

| Name          | Type  | Description                                                                                                                                 | Default                      |
| ------------- | ----- | ------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------- |
| `robot`       |       | Robot model instance loaded with FIGAROH. Must have 'model' and 'data' attributes for Pinocchio integration.                                | *required*                   |
| `config_file` | `str` | Path to YAML configuration file containing calibration parameters, sample file paths, and optimization settings. Defaults to standard path. | `'config/robot_config.yaml'` |

Raises:

| Type                | Description                                       |
| ------------------- | ------------------------------------------------- |
| `FileNotFoundError` | If config_file does not exist                     |
| `KeyError`          | If required parameters missing from configuration |
| `ValueError`        | If calibration model type is unsupported          |

Side Effects

- Loads and stores calibration parameters in self.calib_config
- Calculates minimum required configurations (self.minNbChosen)
- Initializes optimization result attributes to None
- Prints initialization confirmation message

Example

> > > robot = TiagoRobot() opt_calib = BaseOptimalCalibration(robot, "config/tiago.yaml") TiagoOptimalCalibration initialized

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def __init__(self, robot, config_file="config/robot_config.yaml"):
    """Initialize optimal calibration with robot model and configuration.

    Sets up the optimal calibration framework by loading robot parameters,
    initializing optimization attributes, and calculating the minimum
    number of configurations required based on the calibration model.

    The minimum number of configurations is computed to ensure the
    optimization problem is well-posed and identifiable:
    - For full_params: considers all kinematic parameters
    - For joint_offset: considers only joint offset parameters

    Args:
        robot: Robot model instance loaded with FIGAROH. Must have
              'model' and 'data' attributes for Pinocchio integration.
        config_file (str): Path to YAML configuration file containing
                         calibration parameters, sample file paths, and
                         optimization settings. Defaults to standard path.

    Raises:
        FileNotFoundError: If config_file does not exist
        KeyError: If required parameters missing from configuration
        ValueError: If calibration model type is unsupported

    Side Effects:
        - Loads and stores calibration parameters in self.calib_config
        - Calculates minimum required configurations (self.minNbChosen)
        - Initializes optimization result attributes to None
        - Prints initialization confirmation message

    Example:
        >>> robot = TiagoRobot()
        >>> opt_calib = BaseOptimalCalibration(robot, "config/tiago.yaml")
        TiagoOptimalCalibration initialized
    """
    self.robot = robot
    self.model = robot.model
    self.data = robot.data
    self._config_file = config_file
    self.load_param(config_file)

    # Initialize attributes for optimal calibration
    self.optimal_configurations = None
    self.optimal_weights = None
    self._sampleConfigs_file = self.calib_config.get("sample_configs_file")
    # load_param() only reads the "calibration" task, so the optimal task's
    # own output settings are picked up separately here.
    self._optimal_output_file = self._load_optimal_output_file(config_file)

    # Calculate minimum number of configurations needed
    if self.calib_config["calib_model"] == "full_params":
        self.minNbChosen = (
            int(
                len(self.calib_config["actJoint_idx"])
                * 6
                / self.calib_config["calibration_index"]
            )
            + 1
        )
    elif self.calib_config["calib_model"] == "joint_offset":
        self.minNbChosen = (
            int(
                len(self.calib_config["actJoint_idx"])
                / self.calib_config["calibration_index"]
            )
            + 1
        )

    logger.info(f"{self.__class__.__name__} initialized")
```

### `initialize()`

Initialize the optimization process by preparing all required data.

This method orchestrates the initialization sequence required before optimization can begin. It ensures all mathematical components are properly computed and cached for efficient optimization.

The initialization sequence:

1. Load candidate configurations from external files
1. Calculate kinematic regressors for all configurations
1. Compute determinant root of the full information matrix

Prerequisites

- Robot model and parameters must be loaded
- Configuration file must specify valid sample data paths

Side Effects

- Sets self.q_measured with candidate joint configurations
- Sets self.R_rearr with rearranged kinematic regressor
- Sets self.\_subX_dict and self.\_subX_list with info matrices
- Sets self.detroot_whole with full matrix determinant root

Raises:

| Type             | Description                                        |
| ---------------- | -------------------------------------------------- |
| `ValueError`     | If sample configuration file is invalid or missing |
| `AssertionError` | If regressor calculation fails                     |

See Also

load_candidate_configurations: Configuration data loading calculate_regressor: Kinematic regressor computation calculate_detroot_whole: Information matrix analysis

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def initialize(self):
    """Initialize the optimization process by preparing all required data.

    This method orchestrates the initialization sequence required before
    optimization can begin. It ensures all mathematical components are
    properly computed and cached for efficient optimization.

    The initialization sequence:
    1. Load candidate configurations from external files
    2. Calculate kinematic regressors for all configurations
    3. Compute determinant root of the full information matrix

    Prerequisites:
        - Robot model and parameters must be loaded
        - Configuration file must specify valid sample data paths

    Side Effects:
        - Sets self.q_measured with candidate joint configurations
        - Sets self.R_rearr with rearranged kinematic regressor
        - Sets self._subX_dict and self._subX_list with info matrices
        - Sets self.detroot_whole with full matrix determinant root

    Raises:
        ValueError: If sample configuration file is invalid or missing
        AssertionError: If regressor calculation fails

    See Also:
        load_candidate_configurations: Configuration data loading
        calculate_regressor: Kinematic regressor computation
        calculate_detroot_whole: Information matrix analysis
    """
    self.load_candidate_configurations()
    self.calculate_regressor()
    self.calculate_detroot_whole()
```

### `solve(save_file=False, selection_method='threshold', **selection_kwargs)`

Solve the optimal configuration selection problem.

This is the main entry point that orchestrates the complete optimal configuration generation workflow. It automatically handles initialization if not already performed, solves the SOCP optimization, and provides comprehensive results analysis.

The method implements the complete D-optimal design workflow:

1. Initialize data and regressors (if needed)
1. Solve SOCP optimization for optimal weights
1. Select configurations with significant weights
1. Optionally save results to files
1. Generate visualization plots

Parameters:

| Name                 | Type   | Description                                                                                                    | Default       |
| -------------------- | ------ | -------------------------------------------------------------------------------------------------------------- | ------------- |
| `save_file`          | `bool` | Whether to save optimal configurations to YAML file in results directory. Default False.                       | `False`       |
| `selection_method`   | `str`  | "threshold" (default) or "iroc" -- see calculate_optimal_configurations for what each one does.                | `'threshold'` |
| `**selection_kwargs` |        | Forwarded to calculate_optimal_configurations (e.g. iroc_rel_tol, iroc_patience when selection_method="iroc"). | `{}`          |

Side Effects

- Updates self.optimal_configurations with selected configs
- Updates self.optimal_weights with optimization weights
- Creates visualization plots
- May create output files if save_file=True
- Prints progress and results to console

Raises:

| Type             | Description                                  |
| ---------------- | -------------------------------------------- |
| `AssertionError` | If minimum configuration requirement not met |
| `ValueError`     | If optimization problem is infeasible        |
| `IOError`        | If file saving fails (logged as warning)     |

Example

> > > opt_calib = TiagoOptimalCalibration(robot) opt_calib.solve(save_file=True) 12 configs are chosen: [0, 5, 12, 18, ...] Optimal configurations written to file successfully

See Also

initialize: Data preparation workflow calculate_optimal_configurations: Core optimization solver plot: Results visualization save_results: File output management

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def solve(self, save_file=False, selection_method="threshold", **selection_kwargs):
    """Solve the optimal configuration selection problem.

    This is the main entry point that orchestrates the complete optimal
    configuration generation workflow. It automatically handles
    initialization if not already performed, solves the SOCP optimization,
    and provides comprehensive results analysis.

    The method implements the complete D-optimal design workflow:
    1. Initialize data and regressors (if needed)
    2. Solve SOCP optimization for optimal weights
    3. Select configurations with significant weights
    4. Optionally save results to files
    5. Generate visualization plots

    Args:
        save_file (bool): Whether to save optimal configurations to YAML
                        file in results directory. Default False.
        selection_method (str): "threshold" (default) or "iroc" -- see
                        calculate_optimal_configurations for what each
                        one does.
        **selection_kwargs: Forwarded to calculate_optimal_configurations
                        (e.g. iroc_rel_tol, iroc_patience when
                        selection_method="iroc").

    Side Effects:
        - Updates self.optimal_configurations with selected configs
        - Updates self.optimal_weights with optimization weights
        - Creates visualization plots
        - May create output files if save_file=True
        - Prints progress and results to console

    Raises:
        AssertionError: If minimum configuration requirement not met
        ValueError: If optimization problem is infeasible
        IOError: If file saving fails (logged as warning)

    Example:
        >>> opt_calib = TiagoOptimalCalibration(robot)
        >>> opt_calib.solve(save_file=True)
        12 configs are chosen: [0, 5, 12, 18, ...]
        Optimal configurations written to file successfully

    See Also:
        initialize: Data preparation workflow
        calculate_optimal_configurations: Core optimization solver
        plot: Results visualization
        save_results: File output management
    """
    if not hasattr(self, "R_rearr"):
        self.initialize()
    self.calculate_optimal_configurations(
        selection_method=selection_method, **selection_kwargs
    )
    if save_file:
        try:
            self.save_results()
            logger.info("Optimal configurations written to file successfully")
        except Exception as e:
            logger.warning(f"Could not write to file: {e}")
    self.plot()
```

### `load_param(config_file, setting_type='calibration')`

Load calibration parameters from YAML configuration file.

Only the unified configuration format is supported; a legacy flat config raises an error naming `figaroh.utils.config_migration`.

Parameters:

| Name           | Type  | Description                          | Default         |
| -------------- | ----- | ------------------------------------ | --------------- |
| `config_file`  | `str` | Path to a unified configuration file | *required*      |
| `setting_type` | `str` | Configuration section to load        | `'calibration'` |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def load_param(self, config_file: str, setting_type: str = "calibration"):
    """Load calibration parameters from YAML configuration file.

    Only the unified configuration format is supported; a legacy flat
    config raises an error naming ``figaroh.utils.config_migration``.

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

        # The legacy flat format was removed in 1.0
        require_unified_config(config_file)
        parser = UnifiedConfigParser(config_file)
        unified_config = parser.parse()
        unified_calib_config = create_task_config(
            self.robot, unified_config, setting_type
        )
        # Convert unified format to the calib_config dict
        self.calib_config = unified_to_legacy_config(
            self.robot, unified_calib_config
        )

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

### `load_candidate_configurations()`

Load candidate joint configurations from external data files.

Reads robot joint configurations from CSV or YAML files that serve as the candidate pool for optimization. The method supports multiple file formats and automatically updates the sample count parameter.

Supported formats:

- CSV: Standard measurement data format with joint configurations
- YAML: Structured format with named joints and configurations

The YAML format expects:

```
calibration_joint_names: [joint1, joint2, ...]
calibration_joint_configurations: [[q1_1, q1_2, ...], [q2_1, ...]]
```

Side Effects

- Sets self.q_measured with loaded joint configurations
- Updates self.calib_config["NbSample"] with actual sample count
- May load self.\_configs for YAML format data

Raises:

| Type                | Description                                                                              |
| ------------------- | ---------------------------------------------------------------------------------------- |
| `ValueError`        | If sample_configs_file not specified in configuration or if file format is not supported |
| `FileNotFoundError` | If specified data file does not exist                                                    |

Example

> > > #### Assuming config specifies "data/candidate_configs.yaml"
> > >
> > > opt_calib.load_candidate_configurations() print(opt_calib.q_measured.shape) # (1000, 7) for TIAGo

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

````
def load_candidate_configurations(self):
    """Load candidate joint configurations from external data files.

    Reads robot joint configurations from CSV or YAML files that serve
    as the candidate pool for optimization. The method supports multiple
    file formats and automatically updates the sample count parameter.

    Supported formats:
    - CSV: Standard measurement data format with joint configurations
    - YAML: Structured format with named joints and configurations

    The YAML format expects:
    ```yaml
    calibration_joint_names: [joint1, joint2, ...]
    calibration_joint_configurations: [[q1_1, q1_2, ...], [q2_1, ...]]
    ```

    Side Effects:
        - Sets self.q_measured with loaded joint configurations
        - Updates self.calib_config["NbSample"] with actual sample count
        - May load self._configs for YAML format data

    Raises:
        ValueError: If sample_configs_file not specified in configuration
                   or if file format is not supported
        FileNotFoundError: If specified data file does not exist

    Example:
        >>> # Assuming config specifies "data/candidate_configs.yaml"
        >>> opt_calib.load_candidate_configurations()
        >>> print(opt_calib.q_measured.shape)  # (1000, 7) for TIAGo
    """
    from figaroh.calibration.calibration_tools import get_idxq_from_jname

    if self._sampleConfigs_file is None:
        raise ValueError("sample_configs_file not specified in " "configuration")

    if "csv" in self._sampleConfigs_file:
        _, self.q_measured = load_data(
            self._data_path, self.model, self.calib_config, []
        )
    elif "yaml" in self._sampleConfigs_file:
        with open(self._sampleConfigs_file, "r") as file:
            self._configs = yaml.load(file, Loader=yaml.SafeLoader)

        q_jointNames = self._configs["calibration_joint_names"]
        q_jointConfigs = np.array(
            self._configs["calibration_joint_configurations"]
        ).T

        df = pd.DataFrame.from_dict(dict(zip(q_jointNames, q_jointConfigs)))

        q = np.zeros([len(df), self.robot.q0.shape[0]])
        for i in range(len(df)):
            for j, name in enumerate(q_jointNames):
                jointidx = get_idxq_from_jname(self.model, name)
                q[i, jointidx] = df[name][i]
        self.q_measured = q

        # update number of samples
        self.calib_config["NbSample"] = self.q_measured.shape[0]
    else:
        raise ValueError("Data file format not supported. Use CSV or YAML format.")
````

### `calculate_regressor()`

Calculate kinematic regressors and information matrices.

Computes the kinematic regressor matrices that relate kinematic parameter variations to end-effector pose changes. This is the mathematical foundation for the optimization problem.

The method performs several key computations:

1. Calculate base kinematic regressors for all configurations
1. Rearrange regressor matrix by sample order for efficiency
1. Compute individual information matrices for each configuration
1. Store results for optimization access

Mathematical Background

For each configuration i, the regressor Rᵢ satisfies: δx = Rᵢ δθ where δx is pose variation and δθ is parameter variation.

The information matrix is: Xᵢ = RᵢᵀRᵢ

Side Effects

- Sets self.R_rearr with rearranged kinematic regressor
- Sets self.\_subX_list with list of information matrices
- Sets self.\_subX_dict with indexed information matrices
- Prints parameter names for verification

Returns:

| Name   | Type | Description                    |
| ------ | ---- | ------------------------------ |
| `bool` |      | True if calculation successful |

Prerequisites

- Joint configurations must be loaded (self.q_measured)
- Robot model and parameters must be initialized

See Also

calculate_base_kinematics_regressor: Core regressor computation rearrange_rb: Matrix rearrangement for optimization sub_info_matrix: Information matrix decomposition

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_regressor(self):
    """Calculate kinematic regressors and information matrices.

    Computes the kinematic regressor matrices that relate kinematic
    parameter variations to end-effector pose changes. This is the
    mathematical foundation for the optimization problem.

    The method performs several key computations:
    1. Calculate base kinematic regressors for all configurations
    2. Rearrange regressor matrix by sample order for efficiency
    3. Compute individual information matrices for each configuration
    4. Store results for optimization access

    Mathematical Background:
        For each configuration i, the regressor Rᵢ satisfies:
        δx = Rᵢ δθ
        where δx is pose variation and δθ is parameter variation.

        The information matrix is: Xᵢ = RᵢᵀRᵢ

    Side Effects:
        - Sets self.R_rearr with rearranged kinematic regressor
        - Sets self._subX_list with list of information matrices
        - Sets self._subX_dict with indexed information matrices
        - Prints parameter names for verification

    Returns:
        bool: True if calculation successful

    Prerequisites:
        - Joint configurations must be loaded (self.q_measured)
        - Robot model and parameters must be initialized

    See Also:
        calculate_base_kinematics_regressor: Core regressor computation
        rearrange_rb: Matrix rearrangement for optimization
        sub_info_matrix: Information matrix decomposition
    """
    (
        Rrand_b,
        R_b,
        R_e,
        paramsrand_base,
        paramsrand_e,
    ) = calculate_base_kinematics_regressor(
        self.q_measured, self.model, self.data, self.calib_config
    )

    # Rearrange the kinematic regressor by sample numbered order
    self.R_rearr = self.rearrange_rb(R_b, self.calib_config)
    subX_list, subX_dict = self.sub_info_matrix(self.R_rearr, self.calib_config)
    self._subX_dict = subX_dict
    self._subX_list = subX_list
    return True
```

### `calculate_detroot_whole()`

Calculate determinant root of complete information matrix.

Computes the determinant root of the full Fisher Information Matrix formed by all candidate configurations. This serves as the theoretical upper bound for the D-optimality criterion and is used for performance comparison.

Mathematical Background

M_full = R^T R (full regressor) detroot_whole = det(M_full)^(1/n) / sqrt(n)

This represents the geometric mean of eigenvalues, normalized by matrix dimension for scale independence.

Side Effects

- Sets self.detroot_whole with computed determinant root
- Prints the computed value for verification

Prerequisites

- Kinematic regressor must be calculated (self.R_rearr)
- Requires picos library for determinant computation

Raises:

| Type             | Description                                  |
| ---------------- | -------------------------------------------- |
| `AssertionError` | If regressor calculation not performed first |
| `ImportError`    | If picos library not available               |

See Also

calculate_regressor: Prerequisites for this computation plot: Uses this value for performance comparison

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_detroot_whole(self):
    """Calculate determinant root of complete information matrix.

    Computes the determinant root of the full Fisher Information Matrix
    formed by all candidate configurations. This serves as the theoretical
    upper bound for the D-optimality criterion and is used for
    performance comparison.

    Mathematical Background:
        M_full = R^T R  (full regressor)
        detroot_whole = det(M_full)^(1/n) / sqrt(n)

        This represents the geometric mean of eigenvalues, normalized
        by matrix dimension for scale independence.

    Side Effects:
        - Sets self.detroot_whole with computed determinant root
        - Prints the computed value for verification

    Prerequisites:
        - Kinematic regressor must be calculated (self.R_rearr)
        - Requires picos library for determinant computation

    Raises:
        AssertionError: If regressor calculation not performed first
        ImportError: If picos library not available

    See Also:
        calculate_regressor: Prerequisites for this computation
        plot: Uses this value for performance comparison
    """
    import picos as pc

    assert self.calculate_regressor(), "Calculate regressor first."
    M_whole = np.matmul(self.R_rearr.T, self.R_rearr)
    self.detroot_whole = pc.DetRootN(M_whole) / np.sqrt(M_whole.shape[0])
    logger.info(f"detrootn of whole matrix: {self.detroot_whole}")
```

### `rearrange_rb(R_b, calib_config)`

rearrange the kinematic regressor by sample numbered order

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def rearrange_rb(self, R_b, calib_config):
    """rearrange the kinematic regressor by sample numbered order"""
    Rb_rearr = np.empty_like(R_b)
    for i in range(calib_config["calibration_index"]):
        for j in range(calib_config["NbSample"]):
            Rb_rearr[j * calib_config["calibration_index"] + i, :] = R_b[
                i * calib_config["NbSample"] + j
            ]
    return Rb_rearr
```

### `sub_info_matrix(R, calib_config)`

Decompose regressor into individual configuration info matrices.

Creates separate information matrices for each configuration by extracting the corresponding rows from the full regressor matrix. This decomposition enables individual configuration evaluation in the optimization process.

Parameters:

| Name           | Type      | Description                                                         | Default    |
| -------------- | --------- | ------------------------------------------------------------------- | ---------- |
| `R`            | `ndarray` | Full rearranged kinematic regressor matrix                          | *required* |
| `calib_config` | `dict`    | Calibration parameters including sample count and calibration index | *required* |

Returns:

| Name    | Type | Description                                                                                                                            |
| ------- | ---- | -------------------------------------------------------------------------------------------------------------------------------------- |
| `tuple` |      | (subX_list, subX_dict) where: - subX_list: List of information matrices (RᵢᵀRᵢ) - subX_dict: Dictionary mapping config index to matrix |

Mathematical Details

For configuration i: Rᵢ = R[i\*idx:(i+1)\*idx, :] (extract rows) Xᵢ = RᵢᵀRᵢ (information matrix)

Example

> > > R_full = np.random.rand(6000, 42) # 1000 configs, 6 DOF subX_list, subX_dict = self.sub_info_matrix(R_full, calib_config) print(len(subX_list)) # 1000 print(subX_dict[0].shape) # (42, 42)

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def sub_info_matrix(self, R, calib_config):
    """Decompose regressor into individual configuration info matrices.

    Creates separate information matrices for each configuration by
    extracting the corresponding rows from the full regressor matrix.
    This decomposition enables individual configuration evaluation
    in the optimization process.

    Args:
        R (ndarray): Full rearranged kinematic regressor matrix
        calib_config (dict): Calibration parameters including sample count
                     and calibration index

    Returns:
        tuple: (subX_list, subX_dict) where:
            - subX_list: List of information matrices (RᵢᵀRᵢ)
            - subX_dict: Dictionary mapping config index to matrix

    Mathematical Details:
        For configuration i:
        Rᵢ = R[i*idx:(i+1)*idx, :]  (extract rows)
        Xᵢ = RᵢᵀRᵢ  (information matrix)

    Example:
        >>> R_full = np.random.rand(6000, 42)  # 1000 configs, 6 DOF
        >>> subX_list, subX_dict = self.sub_info_matrix(R_full, calib_config)
        >>> print(len(subX_list))  # 1000
        >>> print(subX_dict[0].shape)  # (42, 42)
    """
    subX_list = []
    idex = calib_config["calibration_index"]
    for it in range(calib_config["NbSample"]):
        sub_R = R[it * idex : (it * idex + idex), :]
        subX = np.matmul(sub_R.T, sub_R)
        subX_list.append(subX)
    subX_dict = dict(
        zip(
            np.arange(
                calib_config["NbSample"],
            ),
            subX_list,
        )
    )
    return subX_list, subX_dict
```

### `calculate_optimal_configurations(selection_method='threshold', iroc_rel_tol=0.001, iroc_patience=3)`

Solve SOCP optimization to find optimal configuration subset.

This is the core optimization method that solves the D-optimal experimental design problem using Second-Order Cone Programming. The method finds weights for each candidate configuration that maximize the determinant of the Fisher Information Matrix.

Optimization Problem

maximize det(Σᵢ wᵢ Xᵢ)^(1/n) subject to: Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

Where Xᵢ are information matrices and wᵢ are configuration weights.

Selection Process

1. Solve SOCP optimization for optimal (continuous-relaxation) weights
1. Select a discrete subset from the weight ranking, via one of two interchangeable strategies (`selection_method`):
1. `"threshold"` (default, original behavior): keep every configuration whose weight exceeds `eps_opt` (1e-5). Simple, but says nothing about *how many* configurations are actually enough -- it only discards the ones the SOCP relaxation assigned essentially zero weight.
1. `"iroc"`: rank-and-incrementally-test against the normalized D-optimality criterion (the same quantity :meth:`plot` already computes for visualization, here used as a stopping rule instead): grow the selected subset one configuration at a time in weight-rank order and stop at the first point where the criterion's relative gain stays below `iroc_rel_tol` for `iroc_patience` consecutive configurations in a row (a plateau) -- i.e. an automatic, data-driven minimal configuration count, rather than a fixed threshold on individual weights. See :meth:`_select_by_iroc`.
1. Verify minimum configuration requirement is met
1. Store selected configurations and weights

Parameters:

| Name               | Type    | Description                                                                                                      | Default       |
| ------------------ | ------- | ---------------------------------------------------------------------------------------------------------------- | ------------- |
| `selection_method` | `str`   | "threshold" or "iroc" (see above).                                                                               | `'threshold'` |
| `iroc_rel_tol`     | `float` | Relative-gain plateau tolerance, only used when selection_method="iroc".                                         | `0.001`       |
| `iroc_patience`    | `int`   | Number of consecutive sub-tolerance gains required to declare a plateau, only used when selection_method="iroc". | `3`           |

Side Effects

- Sets self.w_list with optimization solution weights
- Sets self.w_dict_sort with sorted weight dictionary
- Sets self.o1_history with the incremental D-optimality criterion trace when selection_method="iroc" (for inspection/plotting)
- Sets self.optimal_configurations with selected configs
- Sets self.optimal_weights with final weight values
- Sets self.nb_chosen with number of selected configurations
- Prints timing information and selection results

Returns:

| Name   | Type | Description                                  |
| ------ | ---- | -------------------------------------------- |
| `bool` |      | True if optimization successful and feasible |

Raises:

| Type             | Description                                                                                |
| ---------------- | ------------------------------------------------------------------------------------------ |
| `AssertionError` | If regressor not calculated or if insufficient configurations selected (infeasible design) |
| `ValueError`     | If selection_method is not "threshold" or "iroc"                                           |

Example

> > > opt_calib.calculate_optimal_configurations() solve time of socp: 2.35 seconds 12 configs are chosen: [0, 5, 12, 18, 23, ...] opt_calib.calculate_optimal_configurations( ... selection_method="iroc" ... ) 9 configs are chosen: [0, 5, 12, 18, 23, 7, 14, 2, 9]

See Also

SOCPOptimizer: The optimization solver implementation calculate_regressor: Required prerequisite computation \_select_by_iroc: The O1-plateau automatic count selection

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_optimal_configurations(
    self,
    selection_method="threshold",
    iroc_rel_tol=1e-3,
    iroc_patience=3,
):
    """Solve SOCP optimization to find optimal configuration subset.

    This is the core optimization method that solves the D-optimal
    experimental design problem using Second-Order Cone Programming.
    The method finds weights for each candidate configuration that
    maximize the determinant of the Fisher Information Matrix.

    Optimization Problem:
        maximize det(Σᵢ wᵢ Xᵢ)^(1/n)
        subject to: Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

        Where Xᵢ are information matrices and wᵢ are configuration weights.

    Selection Process:
        1. Solve SOCP optimization for optimal (continuous-relaxation)
           weights
        2. Select a discrete subset from the weight ranking, via one of
           two interchangeable strategies (``selection_method``):

           - ``"threshold"`` (default, original behavior): keep every
             configuration whose weight exceeds ``eps_opt`` (1e-5).
             Simple, but says nothing about *how many* configurations
             are actually enough -- it only discards the ones the SOCP
             relaxation assigned essentially zero weight.
           - ``"iroc"``: rank-and-incrementally-test against the
             normalized D-optimality criterion (the same quantity
             :meth:`plot` already computes for visualization, here used
             as a stopping rule instead): grow the selected subset one
             configuration at a time in weight-rank order and stop at
             the first point where the criterion's relative gain stays
             below ``iroc_rel_tol`` for ``iroc_patience`` consecutive
             configurations in a row (a plateau) -- i.e. an automatic,
             data-driven minimal configuration count, rather than a
             fixed threshold on individual weights. See
             :meth:`_select_by_iroc`.
        3. Verify minimum configuration requirement is met
        4. Store selected configurations and weights

    Args:
        selection_method (str): "threshold" or "iroc" (see above).
        iroc_rel_tol (float): Relative-gain plateau tolerance, only
            used when ``selection_method="iroc"``.
        iroc_patience (int): Number of consecutive sub-tolerance gains
            required to declare a plateau, only used when
            ``selection_method="iroc"``.

    Side Effects:
        - Sets self.w_list with optimization solution weights
        - Sets self.w_dict_sort with sorted weight dictionary
        - Sets self.o1_history with the incremental D-optimality
          criterion trace when selection_method="iroc" (for
          inspection/plotting)
        - Sets self.optimal_configurations with selected configs
        - Sets self.optimal_weights with final weight values
        - Sets self.nb_chosen with number of selected configurations
        - Prints timing information and selection results

    Returns:
        bool: True if optimization successful and feasible

    Raises:
        AssertionError: If regressor not calculated or if insufficient
                      configurations selected (infeasible design)
        ValueError: If selection_method is not "threshold" or "iroc"

    Example:
        >>> opt_calib.calculate_optimal_configurations()
        solve time of socp: 2.35 seconds
        12 configs are chosen: [0, 5, 12, 18, 23, ...]
        >>> opt_calib.calculate_optimal_configurations(
        ...     selection_method="iroc"
        ... )
        9 configs are chosen: [0, 5, 12, 18, 23, 7, 14, 2, 9]

    See Also:
        SOCPOptimizer: The optimization solver implementation
        calculate_regressor: Required prerequisite computation
        _select_by_iroc: The O1-plateau automatic count selection
    """
    import time

    assert self.calculate_regressor(), "Calculate regressor first."

    # Picos optimization (A-optimality, C-optimality, D-optimality)
    prev_time = time.time()
    SOCP_algo = SOCPOptimizer(self._subX_dict, self.calib_config)
    self.w_list, self.w_dict_sort = SOCP_algo.solve()
    solve_time = time.time() - prev_time
    logger.info(f"solve time of socp: {solve_time}")

    if selection_method == "threshold":
        chosen_config = self._select_by_threshold()
    elif selection_method == "iroc":
        chosen_config = self._select_by_iroc(iroc_rel_tol, iroc_patience)
    else:
        raise ValueError(
            "selection_method must be 'threshold' or 'iroc', got "
            f"{selection_method!r}."
        )

    assert (
        len(chosen_config) >= self.minNbChosen
    ), "Infeasible design, try to increase NbSample."

    logger.info(f"{len(chosen_config)} configs are chosen: {chosen_config}")
    self.nb_chosen = len(chosen_config)

    # Store optimal configurations and weights
    opt_ids = chosen_config
    opt_configs_values = []
    for opt_id in opt_ids:
        opt_configs_values.append(
            self._configs["calibration_joint_configurations"][opt_id]
        )
    self.optimal_configurations = self._configs.copy()
    self.optimal_configurations["calibration_joint_configurations"] = list(
        opt_configs_values
    )
    self.optimal_weights = self.w_list
    return True
```

### `plot()`

Generate comprehensive visualization of optimization results.

Creates dual-panel plots that provide insight into the optimization quality and configuration selection process. The visualizations help assess the efficiency of the selected configuration subset.

Plot Components:

1. D-optimality criterion vs. number of configurations

- Shows how information matrix determinant improves with additional configurations
- Normalized against theoretical maximum (all configurations)
- Helps identify diminishing returns point

1. Configuration weights in logarithmic scale
1. Displays weight assigned to each candidate configuration
1. Configurations above threshold (eps_opt) are selected
1. Shows selection boundary and weight distribution

Prerequisites

- Optimization must be completed (optimal_configurations available)
- Information matrices must be computed

Side Effects

- Creates matplotlib figure with two subplots
- Displays plots using plt.show()
- May block execution until plots are closed

Returns:

| Name   | Type | Description                 |
| ------ | ---- | --------------------------- |
| `bool` |      | True if plotting successful |

Mathematical Details

D-optimality ratio = detroot_whole / det(selected_subset) This ratio approaches 1.0 as selected subset approaches optimality.

Example

> > > opt_calib.solve()
> > >
> > > #### Plot is automatically generated, or call manually:
> > >
> > > opt_calib.plot()

See Also

calculate_optimal_configurations: Generates data for plotting calculate_detroot_whole: Provides normalization reference

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def plot(self):
    """Generate comprehensive visualization of optimization results.

    Creates dual-panel plots that provide insight into the optimization
    quality and configuration selection process. The visualizations help
    assess the efficiency of the selected configuration subset.

    Plot Components:
    1. D-optimality criterion vs. number of configurations
       - Shows how information matrix determinant improves with
         additional configurations
       - Normalized against theoretical maximum (all configurations)
       - Helps identify diminishing returns point

    2. Configuration weights in logarithmic scale
       - Displays weight assigned to each candidate configuration
       - Configurations above threshold (eps_opt) are selected
       - Shows selection boundary and weight distribution

    Prerequisites:
        - Optimization must be completed (optimal_configurations available)
        - Information matrices must be computed

    Side Effects:
        - Creates matplotlib figure with two subplots
        - Displays plots using plt.show()
        - May block execution until plots are closed

    Returns:
        bool: True if plotting successful

    Mathematical Details:
        D-optimality ratio = detroot_whole / det(selected_subset)
        This ratio approaches 1.0 as selected subset approaches optimality.

    Example:
        >>> opt_calib.solve()
        >>> # Plot is automatically generated, or call manually:
        >>> opt_calib.plot()

    See Also:
        calculate_optimal_configurations: Generates data for plotting
        calculate_detroot_whole: Provides normalization reference
    """
    import picos as pc

    assert (
        hasattr(self, "optimal_configurations")
        and self.optimal_configurations is not None
    ), "Calculate optimal configurations first."

    # Plotting
    det_root_list = []
    n_key_list = []

    # Calculate det_root_list and n_key_list
    for nbc in range(self.minNbChosen, self.calib_config["NbSample"] + 1):
        n_key = list(self.w_dict_sort.keys())[0:nbc]
        n_key_list.append(n_key)
        M_i = pc.sum(self.w_dict_sort[i] * self._subX_list[i] for i in n_key)
        det_root_list.append(pc.DetRootN(M_i) / np.sqrt(nbc))

    # Create subplots
    fig, ax = plt.subplots(2)

    # Plot D-optimality criterion
    ratio = self.detroot_whole / det_root_list[-1]
    plot_range = self.calib_config["NbSample"] - self.minNbChosen
    ax[0].set_ylabel("D-optimality criterion", fontsize=20)
    ax[0].tick_params(axis="y", labelsize=18)
    ax[0].plot(ratio * np.array(det_root_list[:plot_range]))
    ax[0].spines["top"].set_visible(False)
    ax[0].spines["right"].set_visible(False)
    ax[0].grid(True, linestyle="--")

    # Plot quality of estimation
    ax[1].set_ylabel("Weight values (log)", fontsize=20)
    ax[1].set_xlabel("Data sample", fontsize=20)
    ax[1].tick_params(axis="both", labelsize=18)
    ax[1].tick_params(axis="y", labelrotation=30)
    ax[1].scatter(
        np.arange(len(list(self.w_dict_sort.values()))),
        list(self.w_dict_sort.values()),
    )
    ax[1].set_yscale("log")
    ax[1].spines["top"].set_visible(False)
    ax[1].spines["right"].set_visible(False)
    ax[1].grid(True, linestyle="--")
    plt.show()

    return True
```

### `plot_results()`

Plot optimal calibration results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def plot_results(self):
    """Plot optimal calibration results using unified results manager."""
    if (
        not hasattr(self, "optimal_configurations")
        or self.optimal_configurations is None
    ):
        logger.warning(
            "No optimal configuration results to plot. Run solve() first."
        )
        return

    def _basic_plots():
        fig, ax = plt.subplots(1, 2, figsize=(14, 6))

        ax[0].bar(
            list(self.w_dict_sort.keys()),
            list(self.w_dict_sort.values()),
        )
        ax[0].set_xlabel("Configuration indices")
        ax[0].set_ylabel("Weights")
        ax[0].set_title("Chosen configurations")
        ax[0].spines["top"].set_visible(False)
        ax[0].spines["right"].set_visible(False)
        ax[0].grid(True, linestyle="--")

        ax[1].bar(
            list(self.w_dict_sort.keys()),
            list(self.w_dict_sort.values()),
        )
        ax[1].set_yscale("log")
        ax[1].spines["top"].set_visible(False)
        ax[1].spines["right"].set_visible(False)
        ax[1].grid(True, linestyle="--")
        plt.show()

    def _managed_plot():
        robot_name = self.calib_config.get("robot_name", self.model.name)
        results_manager = ResultsManager("optimal_calibration", robot_name)
        weights = (
            np.array(list(self.w_dict_sort.values()))
            if hasattr(self, "w_dict_sort")
            else np.array([])
        )
        results_manager.plot_optimal_calibration_results(
            configurations=self.optimal_configurations,
            weights=weights,
            title="Optimal Calibration Configuration Results",
        )

    plot_with_fallback(_managed_plot, _basic_plots, logger, "optimal_calibration")
```

### `get_optimal_output_dir(default='results')`

Return the directory optimal-configuration results are written to.

Honours the directory component of the configured `tasks.optimal_configuration.output.output_file`. The *filename* is managed by :class:`ResultsManager` (timestamped, one file per format), so only the directory part of that setting is used.

Parameters:

| Name      | Type  | Description                                  | Default     |
| --------- | ----- | -------------------------------------------- | ----------- |
| `default` | `str` | Directory to use when nothing is configured. | `'results'` |

Returns:

| Name  | Type | Description            |
| ----- | ---- | ---------------------- |
| `str` |      | Output directory path. |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def get_optimal_output_dir(self, default="results"):
    """Return the directory optimal-configuration results are written to.

    Honours the directory component of the configured
    ``tasks.optimal_configuration.output.output_file``. The *filename* is
    managed by :class:`ResultsManager` (timestamped, one file per format),
    so only the directory part of that setting is used.

    Args:
        default (str): Directory to use when nothing is configured.

    Returns:
        str: Output directory path.
    """
    configured = getattr(self, "_optimal_output_file", None)
    if not configured:
        return default
    return os.path.dirname(configured) or default
```

### `count_optimal_configurations()`

Return the number of selected configurations.

`optimal_configurations` is a *dict* holding the joint names alongside the selected joint configurations, so `len()` on it counts keys (2), not configurations. Always go through this helper.

Returns:

| Name  | Type | Description                                         |
| ----- | ---- | --------------------------------------------------- |
| `int` |      | Number of selected joint configurations, 0 if none. |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def count_optimal_configurations(self):
    """Return the number of selected configurations.

    ``optimal_configurations`` is a *dict* holding the joint names alongside
    the selected joint configurations, so ``len()`` on it counts keys (2),
    not configurations. Always go through this helper.

    Returns:
        int: Number of selected joint configurations, 0 if none.
    """
    configs = getattr(self, "optimal_configurations", None)
    if not configs:
        return 0
    return len(configs.get("calibration_joint_configurations", []))
```

### `save_results(output_dir=None)`

Save optimal configuration results using unified results manager.

Parameters:

| Name         | Type  | Description                                                                                                                                                                            | Default |
| ------------ | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `output_dir` | `str` | Directory to write results into. When omitted, the directory configured under the unified config's tasks.optimal_configuration.output.output_file is used, and failing that "results". | `None`  |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def save_results(self, output_dir=None):
    """Save optimal configuration results using unified results manager.

    Args:
        output_dir (str, optional): Directory to write results into. When
            omitted, the directory configured under the unified config's
            ``tasks.optimal_configuration.output.output_file`` is used, and
            failing that ``"results"``.
    """
    if (
        not hasattr(self, "optimal_configurations")
        or self.optimal_configurations is None
    ):
        logger.warning(
            "No optimal configuration results to save. Run solve() first."
        )
        return

    if output_dir is None:
        output_dir = self.get_optimal_output_dir()

    try:
        # Initialize results manager
        robot_name = self.calib_config.get("robot_name", self.model.name)
        results_manager = ResultsManager("optimal_calibration", robot_name)

        # Prepare results dictionary
        results_dict = {
            "optimal_configurations": self.optimal_configurations,
            "selected_weights": (
                self.w_dict_sort if hasattr(self, "w_dict_sort") else {}
            ),
            "minimum_configurations": getattr(self, "minNbChosen", 0),
            "configuration_count": self.count_optimal_configurations(),
            "calibration_config": self.calib_config,
        }

        # D-optimality determinant root of the full information matrix.
        # Named after what it is: this is NOT a condition number.
        if hasattr(self, "detroot_whole"):
            results_dict["d_optimality_detroot"] = float(self.detroot_whole)

        # Save using unified manager
        saved_files = results_manager.save_results(
            results_dict, output_dir, save_formats=["yaml", "csv"]
        )

        return saved_files

    except ImportError:
        # Fallback to existing saving
        import os
        import yaml

        os.makedirs(output_dir, exist_ok=True)

        robot_name = self.calib_config.get("robot_name", self.model.name)
        filename = f"{robot_name}_optimal_configurations.yaml"

        with open(os.path.join(output_dir, filename), "w") as stream:
            try:
                yaml.dump(
                    self.optimal_configurations,
                    stream,
                    sort_keys=False,
                    default_flow_style=True,
                )
            except yaml.YAMLError as exc:
                logger.error(exc)
        logger.info(f"Results saved to {output_dir}/{filename}")

        return {"yaml": os.path.join(output_dir, filename)}
```

## `BaseOptimalTrajectory(robot, active_joints, config_file='config/robot_config.yaml')`

Base class for IPOPT-based optimal trajectory generation.

Features:

- Modular design with separated concerns
- Better error handling and logging
- Configuration validation
- Cleaner interfaces

This base class can be extended for specific robots by implementing robot-specific configuration loading and constraint handling.

Initialize the optimal trajectory generator.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def __init__(
    self,
    robot,
    active_joints: List[str],
    config_file: str = "config/robot_config.yaml",
):
    """Initialize the optimal trajectory generator."""
    self.robot = robot
    self.model = self.robot.model
    self.active_joints = active_joints

    # Set up logger (configuration should be done by application, not library)
    self.logger = logging.getLogger(__name__)

    # Load configuration
    self.trajectory_config, self.identif_config = load_param(
        self.robot, config_file
    )

    # # Initialize components
    # self.initialize()

    # Results storage
    self.results = {
        "T_F": [],
        "P_F": [],
        "V_F": [],
        "A_F": [],
        "iteration_data": [],
        "final_regressor_shape": None,
    }
```

### `initialize()`

Initialize trajectory generation components.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def initialize(self):
    """Initialize trajectory generation components."""
    # Create soft limit pool
    n_active_joints = len(self.active_joints)
    self.soft_lim_pool = np.full(
        (3, n_active_joints), self.trajectory_config["soft_lim"]
    )

    # Initialize cubic spline and waypoint generation
    self.CB = CubicSpline(
        self.robot,
        self.trajectory_config["n_wps"],
        self.active_joints,
        self.trajectory_config["soft_lim"],
    )
    self.WP = WaypointsGeneration(
        self.robot,
        self.trajectory_config["n_wps"],
        self.active_joints,
        self.trajectory_config["soft_lim"],
    )

    # Initialize specialized components
    self.base_computer = BaseParameterComputer(
        self.robot, self.identif_config, self.active_joints, self.soft_lim_pool
    )
    self.constraint_manager = TrajectoryConstraintManager(
        self.robot, self.CB, self.trajectory_config, self.identif_config
    )

    # Compute base parameters
    self.idx_e, self.idx_b = self.base_computer.compute_base_indices()

    self.logger.info(
        f"BaseOptimalTrajectory initialized with {len(self.idx_b)} base parameters"
    )
```

### `solve(stack_reps=2)`

Solve the optimal trajectory generation problem.

Parameters:

| Name         | Type  | Description                            | Default |
| ------------ | ----- | -------------------------------------- | ------- |
| `stack_reps` | `int` | Number of trajectory segments to stack | `2`     |

Returns:

| Type             | Description                                        |
| ---------------- | -------------------------------------------------- |
| `Dict[str, Any]` | Dict containing trajectories and optimization info |

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def solve(self, stack_reps: int = 2) -> Dict[str, Any]:
    """
    Solve the optimal trajectory generation problem.

    Args:
        stack_reps: Number of trajectory segments to stack

    Returns:
        Dict containing trajectories and optimization info
    """
    self.logger.info(
        f"Starting optimal trajectory generation with {stack_reps} segments..."
    )

    try:
        # Initialize
        self.WP.gen_rand_pool(self.soft_lim_pool)
        wp_init = np.zeros(len(self.CB.act_idxq))
        vel_wp_init = np.zeros(len(self.CB.act_idxv))
        acc_wp_init = np.zeros(len(self.CB.act_idxv))

        # Random initial position
        for idx in range(len(self.CB.act_idxq)):
            wp_init[idx] = np.random.choice(self.WP.pool_q[:, idx], 1)[0]

        W_stack = None

        for s_rep in range(stack_reps):
            self.logger.info(f"Optimizing segment {s_rep + 1}/{stack_reps}")
            self.logger.info(f"Initial waypoint: {wp_init}")

            success = self._solve_segment(
                s_rep, wp_init, vel_wp_init, acc_wp_init, W_stack
            )

            if not success:
                self.logger.error(f"Failed to solve segment {s_rep + 1}")
                break

            # Update for next segment
            if s_rep < stack_reps - 1:  # Not the last segment
                wp_init, W_stack = self._prepare_next_segment()

        self.logger.info(
            f"Completed! Generated {len(self.results['T_F'])} trajectory segments"
        )
        self.results["final_regressor_shape"] = (
            W_stack.shape if W_stack is not None else None
        )

        return self.results

    except Exception as e:
        self.logger.error(f"Error in solve: {e}")
        raise
```

### `objective_function(X, opt_cb, tps, vel_wps, acc_wps, wp_init, W_stack=None)`

Objective function: condition number of base regressor matrix.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def objective_function(
    self, X, opt_cb, tps, vel_wps, acc_wps, wp_init, W_stack=None
):
    """Objective function: condition number of base regressor matrix."""
    try:
        # Reshape and arrange waypoints
        X = np.array(X)
        wps_X = np.reshape(
            X,
            (self.trajectory_config["n_wps"] - 1, len(self.active_joints)),
        )
        wps = np.vstack((wp_init, wps_X))
        wps = wps.transpose()

        # Generate full trajectory configuration
        t_f, p_f, v_f, a_f = self.CB.get_full_config(
            self.trajectory_config["freq"], tps, wps, vel_wps, acc_wps
        )

        # Store in callback dictionary
        opt_cb.update({"t_f": t_f, "p_f": p_f, "v_f": v_f, "a_f": a_f})

        # Build stacked base regressor and return condition number
        W_b = self._stack_base_regressors(p_f, v_f, a_f, W_stack=W_stack)
        return np.linalg.cond(W_b)

    except Exception as e:
        self.logger.error(f"Error in objective function: {e}")
        return 1e10  # Return large penalty value
```

### `create_ipopt_problem(n_joints, n_wps, Ns, tps, vel_wps, acc_wps, wp_init, vel_wp_init, acc_wp_init, W_stack)`

Create IPOPT problem instance. Should be implemented by subclasses.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
@abstractmethod
def create_ipopt_problem(
    self,
    n_joints,
    n_wps,
    Ns,
    tps,
    vel_wps,
    acc_wps,
    wp_init,
    vel_wp_init,
    acc_wp_init,
    W_stack,
):
    """Create IPOPT problem instance. Should be implemented by subclasses."""
    raise NotImplementedError("Subclasses must implement create_ipopt_problem")
```

### `plot_results()`

Plot optimal trajectory results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def plot_results(self):
    """Plot optimal trajectory results using unified results manager."""
    if not self.results["T_F"]:
        self.logger.warning("No trajectory data to plot")
        return

    def _basic_plots():
        try:
            # Create subplots
            n_joints = len(self.CB.act_Jid)
            fig, axes = plt.subplots(
                n_joints, 3, sharex=True, figsize=(15, 2 * n_joints)
            )
            if n_joints == 1:
                axes = axes.reshape(1, -1)

            fig.suptitle("Optimal Trajectory Results", fontsize=16)

            # Plot each segment
            colors = plt.cm.tab10(np.linspace(0, 1, len(self.results["T_F"])))

            for seg_idx, (T, P, V, A) in enumerate(
                zip(
                    self.results["T_F"],
                    self.results["P_F"],
                    self.results["V_F"],
                    self.results["A_F"],
                )
            ):
                color = colors[seg_idx]
                label = f"Segment {seg_idx + 1}"

                # P spans nq columns, V and A span nv: index each by
                # the active joint's own idx_q / idx_v (#149).
                for joint_idx, (iq, iv) in enumerate(
                    zip(self.CB.act_idxq, self.CB.act_idxv)
                ):
                    axes[joint_idx, 0].plot(T, P[:, iq], color=color, label=label)
                    axes[joint_idx, 1].plot(T, V[:, iv], color=color, label=label)
                    axes[joint_idx, 2].plot(T, A[:, iv], color=color, label=label)

            # Set labels and formatting
            for joint_idx, name in enumerate(self.CB.act_Jname):
                axes[joint_idx, 0].set_ylabel(f"{name}\nPosition (rad)")
                axes[joint_idx, 1].set_ylabel(f"{name}\nVelocity (rad/s)")
                axes[joint_idx, 2].set_ylabel(f"{name}\nAcceleration (rad/s²)")

                if joint_idx == 0:
                    for col in range(3):
                        axes[joint_idx, col].legend()

                for col in range(3):
                    axes[joint_idx, col].grid(True, alpha=0.3)

            axes[-1, 0].set_xlabel("Time (s)")
            axes[-1, 1].set_xlabel("Time (s)")
            axes[-1, 2].set_xlabel("Time (s)")

            plt.tight_layout()
            plt.show()

        except Exception as e:
            self.logger.error(f"Error plotting results: {e}")

    def _managed_plot():
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        results_manager = ResultsManager("optimal_trajectory", robot_name)

        # Calculate overall condition number
        condition_number = getattr(self, "final_condition_number", 0.0)
        if (
            condition_number == 0.0
            and hasattr(self, "results")
            and "condition_numbers" in self.results
        ):
            condition_number = (
                self.results["condition_numbers"][-1]
                if self.results["condition_numbers"]
                else 0.0
            )

        results_manager.plot_optimal_trajectory_results(
            trajectories=self.results,
            condition_number=condition_number,
            joint_names=self.CB.act_Jname,
            title="Optimal Trajectory Generation Results",
            q_indices=self.CB.act_idxq,
            v_indices=self.CB.act_idxv,
        )

    plot_with_fallback(
        _managed_plot, _basic_plots, self.logger, "optimal_trajectory"
    )
```

### `save_results(output_dir='results')`

Save optimal trajectory results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def save_results(self, output_dir="results"):
    """Save optimal trajectory results using unified results manager."""
    if not self.results["T_F"]:
        self.logger.warning("No trajectory data to save")
        return

    try:
        # Initialize results manager
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        results_manager = ResultsManager("optimal_trajectory", robot_name)

        # Calculate overall condition number
        condition_number = getattr(self, "final_condition_number", 0.0)
        if (
            condition_number == 0.0
            and hasattr(self, "results")
            and "condition_numbers" in self.results
        ):
            condition_number = (
                self.results["condition_numbers"][-1]
                if self.results["condition_numbers"]
                else 0.0
            )

        # Prepare results dictionary
        results_dict = {
            "trajectory_segments": len(self.results["T_F"]),
            "condition_number": float(condition_number),
            "joint_names": [f"Joint {i+1}" for i in range(len(self.CB.act_Jid))],
            "configuration": self.CB.identif_config,
            "time_segments": [t.tolist() for t in self.results["T_F"]],
            "position_segments": [p.tolist() for p in self.results["P_F"]],
            "velocity_segments": [v.tolist() for v in self.results["V_F"]],
            "acceleration_segments": [a.tolist() for a in self.results["A_F"]],
        }

        # Add condition number history if available
        if "condition_numbers" in self.results:
            results_dict["condition_number_history"] = [
                float(c) for c in self.results["condition_numbers"]
            ]

        # Save using unified manager
        saved_files = results_manager.save_results(
            results_dict, output_dir, save_formats=["yaml", "npz"]
        )

        self.logger.info("Trajectory results saved successfully")
        return saved_files

    except ImportError:
        # Fallback to basic saving
        import os
        import yaml

        os.makedirs(output_dir, exist_ok=True)

        # Basic results dictionary
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        filename = f"{robot_name}_optimal_trajectory.yaml"

        condition_number = getattr(self, "final_condition_number", 0.0)
        results_dict = {
            "trajectory_segments": len(self.results["T_F"]),
            "condition_number": float(condition_number),
            "joint_count": len(self.CB.act_Jid),
        }

        with open(os.path.join(output_dir, filename), "w") as f:
            yaml.dump(results_dict, f, default_flow_style=False)

        self.logger.info(f"Basic results saved to {output_dir}/{filename}")
        return {"yaml": os.path.join(output_dir, filename)}
```

## `BaseTrajectoryIPOPTProblem(opt_traj, n_joints, n_wps, Ns, tps, vel_wps, acc_wps, wp_init, vel_wp_init, acc_wp_init, W_stack, problem_name='TrajectoryOptimization')`

Bases: `BaseOptimizationProblem`

Base IPOPT problem formulation for trajectory optimization.

This class provides a base implementation for trajectory optimization that can be extended for specific robots.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def __init__(
    self,
    opt_traj,
    n_joints,
    n_wps,
    Ns,
    tps,
    vel_wps,
    acc_wps,
    wp_init,
    vel_wp_init,
    acc_wp_init,
    W_stack,
    problem_name="TrajectoryOptimization",
):
    super().__init__(problem_name)

    self.opt_traj = opt_traj
    self.n_joints = n_joints
    self.n_wps = n_wps
    self.Ns = Ns
    self.tps = tps
    self.vel_wps = vel_wps
    self.acc_wps = acc_wps
    self.wp_init = wp_init
    self.vel_wp_init = vel_wp_init
    self.acc_wp_init = acc_wp_init
    self.W_stack = W_stack

    # Storage for optimization callback (inherits callback_data from base)
    self.opt_cb = {"t_f": None, "p_f": None, "v_f": None, "a_f": None}
```

### `get_variable_bounds()`

Get variable bounds for optimization.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_variable_bounds(self) -> Tuple[List[float], List[float]]:
    """Get variable bounds for optimization."""
    return self.opt_traj.constraint_manager.get_variable_bounds()
```

### `get_constraint_bounds()`

Get constraint bounds for optimization.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_constraint_bounds(self) -> Tuple[List[float], List[float]]:
    """Get constraint bounds for optimization."""
    return self.opt_traj.constraint_manager.get_constraint_bounds(self.Ns)
```

### `get_initial_guess()`

Get initial guess from waypoints.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_initial_guess(self) -> List[float]:
    """Get initial guess from waypoints."""
    # This will be set when solve() is called with waypoints
    if not hasattr(self, "_initial_wps"):
        # Return zeros as fallback
        return [0.0] * (self.n_joints * (self.n_wps - 1))

    X0 = self._initial_wps[:, range(1, self.n_wps)]
    return np.reshape(X0.transpose(), (self.n_joints * (self.n_wps - 1),)).tolist()
```

### `objective(X)`

Objective function: condition number of base regressor matrix.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def objective(self, X: np.ndarray) -> float:
    """Objective function: condition number of base regressor matrix."""
    return self.opt_traj.objective_function(
        X,
        self.opt_cb,
        self.tps,
        self.vel_wps,
        self.acc_wps,
        self.wp_init,
        self.W_stack,
    )
```

### `constraints(X)`

Constraint function for IPOPT.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def constraints(self, X: np.ndarray) -> np.ndarray:
    """Constraint function for IPOPT."""
    return self.opt_traj.constraint_manager.evaluate_constraints(
        self.Ns, X, self.opt_cb, self.tps, self.vel_wps, self.acc_wps, self.wp_init
    )
```

### `gradient(X)`

Gradient of the objective by forward finite differences.

The default numdifftools gradient uses Richardson extrapolation over many step sizes, which costs tens of seconds per call here because every objective evaluation rebuilds the full regressor.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def gradient(self, X: np.ndarray) -> np.ndarray:
    """
    Gradient of the objective by forward finite differences.

    The default numdifftools gradient uses Richardson extrapolation over
    many step sizes, which costs tens of seconds per call here because
    every objective evaluation rebuilds the full regressor.
    """
    X = np.asarray(X, dtype=float)
    f0 = self.objective(X)
    eps = 1e-6
    grad = np.zeros_like(X)
    for i in range(len(X)):
        X_plus = X.copy()
        X_plus[i] += eps
        grad[i] = (self.objective(X_plus) - f0) / eps
    return grad
```

### `jacobian(X)`

Jacobian of constraints - Custom implementation for better performance.

For trajectory optimization, we can use sparse finite differences instead of full automatic differentiation which is too slow.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def jacobian(self, X: np.ndarray) -> np.ndarray:
    """
    Jacobian of constraints - Custom implementation for better performance.

    For trajectory optimization, we can use sparse finite differences
    instead of full automatic differentiation which is too slow.
    """
    try:
        # Get current constraint values
        c0 = self.constraints(X)
        n_constraints = len(c0)
        n_vars = len(X)

        # Use finite differences with smaller step size for efficiency
        eps = 1e-6
        jac = np.zeros((n_constraints, n_vars))

        # Compute Jacobian column by column (forward differences)
        for i in range(n_vars):
            X_plus = X.copy()
            X_plus[i] += eps
            c_plus = self.constraints(X_plus)
            jac[:, i] = (c_plus - c0) / eps

        self.logger.debug(f"Constraint jacobian shape: {jac.shape}")
        return jac

    except Exception as e:
        self.logger.warning(f"Error computing jacobian: {e}")
        # Return sparse identity matrix as fallback
        n_constraints = len(self.constraints(X))
        n_vars = len(X)
        # Create a sparse jacobian approximation
        jac = np.zeros((n_constraints, n_vars))
        min_dim = min(n_constraints, n_vars)
        jac[:min_dim, :min_dim] = np.eye(min_dim)
        return jac
```

### `solve_with_waypoints(wps)`

Solve the optimization problem with given initial waypoints.

Parameters:

| Name  | Type | Description       | Default    |
| ----- | ---- | ----------------- | ---------- |
| `wps` |      | Initial waypoints | *required* |

Returns:

| Type                          | Description                      |
| ----------------------------- | -------------------------------- |
| `Tuple[bool, Dict[str, Any]]` | Tuple of (success, results_dict) |

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def solve_with_waypoints(self, wps) -> Tuple[bool, Dict[str, Any]]:
    """
    Solve the optimization problem with given initial waypoints.

    Args:
        wps: Initial waypoints

    Returns:
        Tuple of (success, results_dict)
    """
    try:
        # Store initial waypoints for get_initial_guess
        self._initial_wps = wps

        # Create solver with trajectory optimization config
        config = IPOPTConfig.for_trajectory_optimization()
        # Adjust settings for this complex problem
        config.tolerance = 1e-3
        config.acceptable_tolerance = 1e-2
        config.max_iterations = self.opt_traj.trajectory_config.get(
            "max_iterations", 200
        )
        config.print_level = 3  # Reduce output
        config.custom_options = {
            b"mu_strategy": b"adaptive",
        }
        solver = RobotIPOPTSolver(self, config)

        # Solve the problem
        success, results = solver.solve()
        converged = success

        # The condition-number objective is non-smooth, so IPOPT often
        # stalls on dual infeasibility after reaching a feasible plateau.
        # Keep such an iterate, but only if it satisfies every constraint.
        if not success and results.get("status") == -1:
            success = self._is_feasible(results["x_opt"])
            if success:
                self.logger.warning(
                    "IPOPT reached the iteration limit (%d) without "
                    "converging; keeping the feasible final iterate "
                    "(objective %.4g)",
                    config.max_iterations,
                    results["obj_val"],
                )

        # Collision is constrained at check points only; accept the
        # segment only if the whole spline is clear (#143).
        if success and not self._is_collision_free(results["x_opt"]):
            self.logger.error(
                "Optimised segment collides between collision check points"
            )
            return False, results

        if success:
            # Extract final waypoint for next segment
            X_opt = results["x_opt"]
            wps_X = np.reshape(np.array(X_opt), (self.n_wps - 1, self.n_joints))
            final_waypoint = wps_X[-1, :]
            # The last callback may have been a finite-difference probe;
            # rebuild the stored trajectory at the returned solution.
            self.objective(np.asarray(X_opt))

            # Update results with trajectory-specific data
            results.update(
                {
                    "t_f": self.opt_cb["t_f"],
                    "p_f": self.opt_cb["p_f"],
                    "v_f": self.opt_cb["v_f"],
                    "a_f": self.opt_cb["a_f"],
                    "iter_data": {
                        "iterations": self.iteration_data["iterations"],
                        "obj_values": self.iteration_data["obj_values"],
                        "solve_time": results["solve_time"],
                        "status": results["status"],
                        "converged": converged,
                        "final_waypoint": final_waypoint,
                    },
                }
            )

            return True, results
        else:
            self.logger.error("Optimization failed")
            return False, results

    except Exception as e:
        self.logger.error(f"Error in IPOPT solve: {e}")
        return False, {"error": str(e)}
```

## base_optimal_calibration

Base class for robot optimal configuration generation for calibration. This module provides a generalized framework for optimal configuration generation that can be inherited by any robot type (TIAGo, UR10, MATE, etc.).

## `BaseOptimalCalibration(robot, config_file='config/robot_config.yaml')`

Bases: `ABC`

Base class for robot optimal configuration generation for calibration.

This class implements the framework for generating optimal robot configurations that maximize the observability of kinematic parameters during calibration. It uses Second-Order Cone Programming (SOCP) to solve the D-optimal design problem for parameter estimation.

The class provides a Template Method pattern where the main workflow is defined, but specific optimization strategies can be customized by derived classes for different robot types.

Workflow

1. Load candidate configurations from file (CSV or YAML)
1. Calculate kinematic regressors for all candidates
1. Compute information matrices for each configuration
1. Solve SOCP optimization to find optimal subset
1. Select configurations with significant weights
1. Visualize and save results

Key Features

- D-optimal experimental design for calibration
- Support for multiple calibration models (full_params, joint_offset)
- Automatic minimum configuration calculation
- SOCP-based optimization with convex relaxation
- Comprehensive visualization and analysis tools
- File I/O for configuration management

Mathematical Background

The method maximizes the determinant of the Fisher Information Matrix: max det(Σᵢ wᵢ Rᵢᵀ Rᵢ) subject to Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

Where:

- Rᵢ is the kinematic regressor for configuration i
- wᵢ is the weight assigned to configuration i
- The objective maximizes parameter estimation precision

Attributes:

| Name                     | Type      | Description                                    |
| ------------------------ | --------- | ---------------------------------------------- |
| `robot`                  |           | Robot model instance loaded with FIGAROH       |
| `model`                  |           | Pinocchio robot model                          |
| `data`                   |           | Pinocchio robot data                           |
| `calib_config`           | `dict`    | Calibration parameters from configuration file |
| `optimal_configurations` | `dict`    | Selected optimal configurations                |
| `optimal_weights`        | `ndarray` | Weights assigned to configurations             |
| `minNbChosen`            | `int`     | Minimum number of configurations required      |
| `R_rearr`                | `ndarray` | Rearranged kinematic regressor matrix          |
| `detroot_whole`          | `float`   | Determinant root of full information matrix    |
| `w_list`                 | `list`    | Solution weights from SOCP optimization        |
| `w_dict_sort`            | `dict`    | Sorted weights by configuration index          |

Example

> > > ### Basic usage for TIAGo robot
> > >
> > > from figaroh.robots import TiagoRobot robot = TiagoRobot()
> > >
> > > ### Create optimal calibration instance
> > >
> > > opt_calib = TiagoOptimalCalibration(robot, "config/tiago.yaml")
> > >
> > > ### Generate optimal configurations
> > >
> > > opt_calib.solve(save_file=True)
> > >
> > > ### Access results
> > >
> > > print(f"Selected {len(opt_calib.optimal_configurations)} configs") print(f"Minimum required: {opt_calib.minNbChosen}")

See Also

BaseCalibration: Main calibration framework SOCPOptimizer: Second-order cone programming solver TiagoOptimalCalibration: TIAGo-specific implementation UR10OptimalCalibration: UR10-specific implementation

Initialize optimal calibration with robot model and configuration.

Sets up the optimal calibration framework by loading robot parameters, initializing optimization attributes, and calculating the minimum number of configurations required based on the calibration model.

The minimum number of configurations is computed to ensure the optimization problem is well-posed and identifiable:

- For full_params: considers all kinematic parameters
- For joint_offset: considers only joint offset parameters

Parameters:

| Name          | Type  | Description                                                                                                                                 | Default                      |
| ------------- | ----- | ------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------- |
| `robot`       |       | Robot model instance loaded with FIGAROH. Must have 'model' and 'data' attributes for Pinocchio integration.                                | *required*                   |
| `config_file` | `str` | Path to YAML configuration file containing calibration parameters, sample file paths, and optimization settings. Defaults to standard path. | `'config/robot_config.yaml'` |

Raises:

| Type                | Description                                       |
| ------------------- | ------------------------------------------------- |
| `FileNotFoundError` | If config_file does not exist                     |
| `KeyError`          | If required parameters missing from configuration |
| `ValueError`        | If calibration model type is unsupported          |

Side Effects

- Loads and stores calibration parameters in self.calib_config
- Calculates minimum required configurations (self.minNbChosen)
- Initializes optimization result attributes to None
- Prints initialization confirmation message

Example

> > > robot = TiagoRobot() opt_calib = BaseOptimalCalibration(robot, "config/tiago.yaml") TiagoOptimalCalibration initialized

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def __init__(self, robot, config_file="config/robot_config.yaml"):
    """Initialize optimal calibration with robot model and configuration.

    Sets up the optimal calibration framework by loading robot parameters,
    initializing optimization attributes, and calculating the minimum
    number of configurations required based on the calibration model.

    The minimum number of configurations is computed to ensure the
    optimization problem is well-posed and identifiable:
    - For full_params: considers all kinematic parameters
    - For joint_offset: considers only joint offset parameters

    Args:
        robot: Robot model instance loaded with FIGAROH. Must have
              'model' and 'data' attributes for Pinocchio integration.
        config_file (str): Path to YAML configuration file containing
                         calibration parameters, sample file paths, and
                         optimization settings. Defaults to standard path.

    Raises:
        FileNotFoundError: If config_file does not exist
        KeyError: If required parameters missing from configuration
        ValueError: If calibration model type is unsupported

    Side Effects:
        - Loads and stores calibration parameters in self.calib_config
        - Calculates minimum required configurations (self.minNbChosen)
        - Initializes optimization result attributes to None
        - Prints initialization confirmation message

    Example:
        >>> robot = TiagoRobot()
        >>> opt_calib = BaseOptimalCalibration(robot, "config/tiago.yaml")
        TiagoOptimalCalibration initialized
    """
    self.robot = robot
    self.model = robot.model
    self.data = robot.data
    self._config_file = config_file
    self.load_param(config_file)

    # Initialize attributes for optimal calibration
    self.optimal_configurations = None
    self.optimal_weights = None
    self._sampleConfigs_file = self.calib_config.get("sample_configs_file")
    # load_param() only reads the "calibration" task, so the optimal task's
    # own output settings are picked up separately here.
    self._optimal_output_file = self._load_optimal_output_file(config_file)

    # Calculate minimum number of configurations needed
    if self.calib_config["calib_model"] == "full_params":
        self.minNbChosen = (
            int(
                len(self.calib_config["actJoint_idx"])
                * 6
                / self.calib_config["calibration_index"]
            )
            + 1
        )
    elif self.calib_config["calib_model"] == "joint_offset":
        self.minNbChosen = (
            int(
                len(self.calib_config["actJoint_idx"])
                / self.calib_config["calibration_index"]
            )
            + 1
        )

    logger.info(f"{self.__class__.__name__} initialized")
```

### `initialize()`

Initialize the optimization process by preparing all required data.

This method orchestrates the initialization sequence required before optimization can begin. It ensures all mathematical components are properly computed and cached for efficient optimization.

The initialization sequence:

1. Load candidate configurations from external files
1. Calculate kinematic regressors for all configurations
1. Compute determinant root of the full information matrix

Prerequisites

- Robot model and parameters must be loaded
- Configuration file must specify valid sample data paths

Side Effects

- Sets self.q_measured with candidate joint configurations
- Sets self.R_rearr with rearranged kinematic regressor
- Sets self.\_subX_dict and self.\_subX_list with info matrices
- Sets self.detroot_whole with full matrix determinant root

Raises:

| Type             | Description                                        |
| ---------------- | -------------------------------------------------- |
| `ValueError`     | If sample configuration file is invalid or missing |
| `AssertionError` | If regressor calculation fails                     |

See Also

load_candidate_configurations: Configuration data loading calculate_regressor: Kinematic regressor computation calculate_detroot_whole: Information matrix analysis

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def initialize(self):
    """Initialize the optimization process by preparing all required data.

    This method orchestrates the initialization sequence required before
    optimization can begin. It ensures all mathematical components are
    properly computed and cached for efficient optimization.

    The initialization sequence:
    1. Load candidate configurations from external files
    2. Calculate kinematic regressors for all configurations
    3. Compute determinant root of the full information matrix

    Prerequisites:
        - Robot model and parameters must be loaded
        - Configuration file must specify valid sample data paths

    Side Effects:
        - Sets self.q_measured with candidate joint configurations
        - Sets self.R_rearr with rearranged kinematic regressor
        - Sets self._subX_dict and self._subX_list with info matrices
        - Sets self.detroot_whole with full matrix determinant root

    Raises:
        ValueError: If sample configuration file is invalid or missing
        AssertionError: If regressor calculation fails

    See Also:
        load_candidate_configurations: Configuration data loading
        calculate_regressor: Kinematic regressor computation
        calculate_detroot_whole: Information matrix analysis
    """
    self.load_candidate_configurations()
    self.calculate_regressor()
    self.calculate_detroot_whole()
```

### `solve(save_file=False, selection_method='threshold', **selection_kwargs)`

Solve the optimal configuration selection problem.

This is the main entry point that orchestrates the complete optimal configuration generation workflow. It automatically handles initialization if not already performed, solves the SOCP optimization, and provides comprehensive results analysis.

The method implements the complete D-optimal design workflow:

1. Initialize data and regressors (if needed)
1. Solve SOCP optimization for optimal weights
1. Select configurations with significant weights
1. Optionally save results to files
1. Generate visualization plots

Parameters:

| Name                 | Type   | Description                                                                                                    | Default       |
| -------------------- | ------ | -------------------------------------------------------------------------------------------------------------- | ------------- |
| `save_file`          | `bool` | Whether to save optimal configurations to YAML file in results directory. Default False.                       | `False`       |
| `selection_method`   | `str`  | "threshold" (default) or "iroc" -- see calculate_optimal_configurations for what each one does.                | `'threshold'` |
| `**selection_kwargs` |        | Forwarded to calculate_optimal_configurations (e.g. iroc_rel_tol, iroc_patience when selection_method="iroc"). | `{}`          |

Side Effects

- Updates self.optimal_configurations with selected configs
- Updates self.optimal_weights with optimization weights
- Creates visualization plots
- May create output files if save_file=True
- Prints progress and results to console

Raises:

| Type             | Description                                  |
| ---------------- | -------------------------------------------- |
| `AssertionError` | If minimum configuration requirement not met |
| `ValueError`     | If optimization problem is infeasible        |
| `IOError`        | If file saving fails (logged as warning)     |

Example

> > > opt_calib = TiagoOptimalCalibration(robot) opt_calib.solve(save_file=True) 12 configs are chosen: [0, 5, 12, 18, ...] Optimal configurations written to file successfully

See Also

initialize: Data preparation workflow calculate_optimal_configurations: Core optimization solver plot: Results visualization save_results: File output management

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def solve(self, save_file=False, selection_method="threshold", **selection_kwargs):
    """Solve the optimal configuration selection problem.

    This is the main entry point that orchestrates the complete optimal
    configuration generation workflow. It automatically handles
    initialization if not already performed, solves the SOCP optimization,
    and provides comprehensive results analysis.

    The method implements the complete D-optimal design workflow:
    1. Initialize data and regressors (if needed)
    2. Solve SOCP optimization for optimal weights
    3. Select configurations with significant weights
    4. Optionally save results to files
    5. Generate visualization plots

    Args:
        save_file (bool): Whether to save optimal configurations to YAML
                        file in results directory. Default False.
        selection_method (str): "threshold" (default) or "iroc" -- see
                        calculate_optimal_configurations for what each
                        one does.
        **selection_kwargs: Forwarded to calculate_optimal_configurations
                        (e.g. iroc_rel_tol, iroc_patience when
                        selection_method="iroc").

    Side Effects:
        - Updates self.optimal_configurations with selected configs
        - Updates self.optimal_weights with optimization weights
        - Creates visualization plots
        - May create output files if save_file=True
        - Prints progress and results to console

    Raises:
        AssertionError: If minimum configuration requirement not met
        ValueError: If optimization problem is infeasible
        IOError: If file saving fails (logged as warning)

    Example:
        >>> opt_calib = TiagoOptimalCalibration(robot)
        >>> opt_calib.solve(save_file=True)
        12 configs are chosen: [0, 5, 12, 18, ...]
        Optimal configurations written to file successfully

    See Also:
        initialize: Data preparation workflow
        calculate_optimal_configurations: Core optimization solver
        plot: Results visualization
        save_results: File output management
    """
    if not hasattr(self, "R_rearr"):
        self.initialize()
    self.calculate_optimal_configurations(
        selection_method=selection_method, **selection_kwargs
    )
    if save_file:
        try:
            self.save_results()
            logger.info("Optimal configurations written to file successfully")
        except Exception as e:
            logger.warning(f"Could not write to file: {e}")
    self.plot()
```

### `load_param(config_file, setting_type='calibration')`

Load calibration parameters from YAML configuration file.

Only the unified configuration format is supported; a legacy flat config raises an error naming `figaroh.utils.config_migration`.

Parameters:

| Name           | Type  | Description                          | Default         |
| -------------- | ----- | ------------------------------------ | --------------- |
| `config_file`  | `str` | Path to a unified configuration file | *required*      |
| `setting_type` | `str` | Configuration section to load        | `'calibration'` |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def load_param(self, config_file: str, setting_type: str = "calibration"):
    """Load calibration parameters from YAML configuration file.

    Only the unified configuration format is supported; a legacy flat
    config raises an error naming ``figaroh.utils.config_migration``.

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

        # The legacy flat format was removed in 1.0
        require_unified_config(config_file)
        parser = UnifiedConfigParser(config_file)
        unified_config = parser.parse()
        unified_calib_config = create_task_config(
            self.robot, unified_config, setting_type
        )
        # Convert unified format to the calib_config dict
        self.calib_config = unified_to_legacy_config(
            self.robot, unified_calib_config
        )

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

### `load_candidate_configurations()`

Load candidate joint configurations from external data files.

Reads robot joint configurations from CSV or YAML files that serve as the candidate pool for optimization. The method supports multiple file formats and automatically updates the sample count parameter.

Supported formats:

- CSV: Standard measurement data format with joint configurations
- YAML: Structured format with named joints and configurations

The YAML format expects:

```
calibration_joint_names: [joint1, joint2, ...]
calibration_joint_configurations: [[q1_1, q1_2, ...], [q2_1, ...]]
```

Side Effects

- Sets self.q_measured with loaded joint configurations
- Updates self.calib_config["NbSample"] with actual sample count
- May load self.\_configs for YAML format data

Raises:

| Type                | Description                                                                              |
| ------------------- | ---------------------------------------------------------------------------------------- |
| `ValueError`        | If sample_configs_file not specified in configuration or if file format is not supported |
| `FileNotFoundError` | If specified data file does not exist                                                    |

Example

> > > #### Assuming config specifies "data/candidate_configs.yaml"
> > >
> > > opt_calib.load_candidate_configurations() print(opt_calib.q_measured.shape) # (1000, 7) for TIAGo

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

````
def load_candidate_configurations(self):
    """Load candidate joint configurations from external data files.

    Reads robot joint configurations from CSV or YAML files that serve
    as the candidate pool for optimization. The method supports multiple
    file formats and automatically updates the sample count parameter.

    Supported formats:
    - CSV: Standard measurement data format with joint configurations
    - YAML: Structured format with named joints and configurations

    The YAML format expects:
    ```yaml
    calibration_joint_names: [joint1, joint2, ...]
    calibration_joint_configurations: [[q1_1, q1_2, ...], [q2_1, ...]]
    ```

    Side Effects:
        - Sets self.q_measured with loaded joint configurations
        - Updates self.calib_config["NbSample"] with actual sample count
        - May load self._configs for YAML format data

    Raises:
        ValueError: If sample_configs_file not specified in configuration
                   or if file format is not supported
        FileNotFoundError: If specified data file does not exist

    Example:
        >>> # Assuming config specifies "data/candidate_configs.yaml"
        >>> opt_calib.load_candidate_configurations()
        >>> print(opt_calib.q_measured.shape)  # (1000, 7) for TIAGo
    """
    from figaroh.calibration.calibration_tools import get_idxq_from_jname

    if self._sampleConfigs_file is None:
        raise ValueError("sample_configs_file not specified in " "configuration")

    if "csv" in self._sampleConfigs_file:
        _, self.q_measured = load_data(
            self._data_path, self.model, self.calib_config, []
        )
    elif "yaml" in self._sampleConfigs_file:
        with open(self._sampleConfigs_file, "r") as file:
            self._configs = yaml.load(file, Loader=yaml.SafeLoader)

        q_jointNames = self._configs["calibration_joint_names"]
        q_jointConfigs = np.array(
            self._configs["calibration_joint_configurations"]
        ).T

        df = pd.DataFrame.from_dict(dict(zip(q_jointNames, q_jointConfigs)))

        q = np.zeros([len(df), self.robot.q0.shape[0]])
        for i in range(len(df)):
            for j, name in enumerate(q_jointNames):
                jointidx = get_idxq_from_jname(self.model, name)
                q[i, jointidx] = df[name][i]
        self.q_measured = q

        # update number of samples
        self.calib_config["NbSample"] = self.q_measured.shape[0]
    else:
        raise ValueError("Data file format not supported. Use CSV or YAML format.")
````

### `calculate_regressor()`

Calculate kinematic regressors and information matrices.

Computes the kinematic regressor matrices that relate kinematic parameter variations to end-effector pose changes. This is the mathematical foundation for the optimization problem.

The method performs several key computations:

1. Calculate base kinematic regressors for all configurations
1. Rearrange regressor matrix by sample order for efficiency
1. Compute individual information matrices for each configuration
1. Store results for optimization access

Mathematical Background

For each configuration i, the regressor Rᵢ satisfies: δx = Rᵢ δθ where δx is pose variation and δθ is parameter variation.

The information matrix is: Xᵢ = RᵢᵀRᵢ

Side Effects

- Sets self.R_rearr with rearranged kinematic regressor
- Sets self.\_subX_list with list of information matrices
- Sets self.\_subX_dict with indexed information matrices
- Prints parameter names for verification

Returns:

| Name   | Type | Description                    |
| ------ | ---- | ------------------------------ |
| `bool` |      | True if calculation successful |

Prerequisites

- Joint configurations must be loaded (self.q_measured)
- Robot model and parameters must be initialized

See Also

calculate_base_kinematics_regressor: Core regressor computation rearrange_rb: Matrix rearrangement for optimization sub_info_matrix: Information matrix decomposition

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_regressor(self):
    """Calculate kinematic regressors and information matrices.

    Computes the kinematic regressor matrices that relate kinematic
    parameter variations to end-effector pose changes. This is the
    mathematical foundation for the optimization problem.

    The method performs several key computations:
    1. Calculate base kinematic regressors for all configurations
    2. Rearrange regressor matrix by sample order for efficiency
    3. Compute individual information matrices for each configuration
    4. Store results for optimization access

    Mathematical Background:
        For each configuration i, the regressor Rᵢ satisfies:
        δx = Rᵢ δθ
        where δx is pose variation and δθ is parameter variation.

        The information matrix is: Xᵢ = RᵢᵀRᵢ

    Side Effects:
        - Sets self.R_rearr with rearranged kinematic regressor
        - Sets self._subX_list with list of information matrices
        - Sets self._subX_dict with indexed information matrices
        - Prints parameter names for verification

    Returns:
        bool: True if calculation successful

    Prerequisites:
        - Joint configurations must be loaded (self.q_measured)
        - Robot model and parameters must be initialized

    See Also:
        calculate_base_kinematics_regressor: Core regressor computation
        rearrange_rb: Matrix rearrangement for optimization
        sub_info_matrix: Information matrix decomposition
    """
    (
        Rrand_b,
        R_b,
        R_e,
        paramsrand_base,
        paramsrand_e,
    ) = calculate_base_kinematics_regressor(
        self.q_measured, self.model, self.data, self.calib_config
    )

    # Rearrange the kinematic regressor by sample numbered order
    self.R_rearr = self.rearrange_rb(R_b, self.calib_config)
    subX_list, subX_dict = self.sub_info_matrix(self.R_rearr, self.calib_config)
    self._subX_dict = subX_dict
    self._subX_list = subX_list
    return True
```

### `calculate_detroot_whole()`

Calculate determinant root of complete information matrix.

Computes the determinant root of the full Fisher Information Matrix formed by all candidate configurations. This serves as the theoretical upper bound for the D-optimality criterion and is used for performance comparison.

Mathematical Background

M_full = R^T R (full regressor) detroot_whole = det(M_full)^(1/n) / sqrt(n)

This represents the geometric mean of eigenvalues, normalized by matrix dimension for scale independence.

Side Effects

- Sets self.detroot_whole with computed determinant root
- Prints the computed value for verification

Prerequisites

- Kinematic regressor must be calculated (self.R_rearr)
- Requires picos library for determinant computation

Raises:

| Type             | Description                                  |
| ---------------- | -------------------------------------------- |
| `AssertionError` | If regressor calculation not performed first |
| `ImportError`    | If picos library not available               |

See Also

calculate_regressor: Prerequisites for this computation plot: Uses this value for performance comparison

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_detroot_whole(self):
    """Calculate determinant root of complete information matrix.

    Computes the determinant root of the full Fisher Information Matrix
    formed by all candidate configurations. This serves as the theoretical
    upper bound for the D-optimality criterion and is used for
    performance comparison.

    Mathematical Background:
        M_full = R^T R  (full regressor)
        detroot_whole = det(M_full)^(1/n) / sqrt(n)

        This represents the geometric mean of eigenvalues, normalized
        by matrix dimension for scale independence.

    Side Effects:
        - Sets self.detroot_whole with computed determinant root
        - Prints the computed value for verification

    Prerequisites:
        - Kinematic regressor must be calculated (self.R_rearr)
        - Requires picos library for determinant computation

    Raises:
        AssertionError: If regressor calculation not performed first
        ImportError: If picos library not available

    See Also:
        calculate_regressor: Prerequisites for this computation
        plot: Uses this value for performance comparison
    """
    import picos as pc

    assert self.calculate_regressor(), "Calculate regressor first."
    M_whole = np.matmul(self.R_rearr.T, self.R_rearr)
    self.detroot_whole = pc.DetRootN(M_whole) / np.sqrt(M_whole.shape[0])
    logger.info(f"detrootn of whole matrix: {self.detroot_whole}")
```

### `rearrange_rb(R_b, calib_config)`

rearrange the kinematic regressor by sample numbered order

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def rearrange_rb(self, R_b, calib_config):
    """rearrange the kinematic regressor by sample numbered order"""
    Rb_rearr = np.empty_like(R_b)
    for i in range(calib_config["calibration_index"]):
        for j in range(calib_config["NbSample"]):
            Rb_rearr[j * calib_config["calibration_index"] + i, :] = R_b[
                i * calib_config["NbSample"] + j
            ]
    return Rb_rearr
```

### `sub_info_matrix(R, calib_config)`

Decompose regressor into individual configuration info matrices.

Creates separate information matrices for each configuration by extracting the corresponding rows from the full regressor matrix. This decomposition enables individual configuration evaluation in the optimization process.

Parameters:

| Name           | Type      | Description                                                         | Default    |
| -------------- | --------- | ------------------------------------------------------------------- | ---------- |
| `R`            | `ndarray` | Full rearranged kinematic regressor matrix                          | *required* |
| `calib_config` | `dict`    | Calibration parameters including sample count and calibration index | *required* |

Returns:

| Name    | Type | Description                                                                                                                            |
| ------- | ---- | -------------------------------------------------------------------------------------------------------------------------------------- |
| `tuple` |      | (subX_list, subX_dict) where: - subX_list: List of information matrices (RᵢᵀRᵢ) - subX_dict: Dictionary mapping config index to matrix |

Mathematical Details

For configuration i: Rᵢ = R[i\*idx:(i+1)\*idx, :] (extract rows) Xᵢ = RᵢᵀRᵢ (information matrix)

Example

> > > R_full = np.random.rand(6000, 42) # 1000 configs, 6 DOF subX_list, subX_dict = self.sub_info_matrix(R_full, calib_config) print(len(subX_list)) # 1000 print(subX_dict[0].shape) # (42, 42)

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def sub_info_matrix(self, R, calib_config):
    """Decompose regressor into individual configuration info matrices.

    Creates separate information matrices for each configuration by
    extracting the corresponding rows from the full regressor matrix.
    This decomposition enables individual configuration evaluation
    in the optimization process.

    Args:
        R (ndarray): Full rearranged kinematic regressor matrix
        calib_config (dict): Calibration parameters including sample count
                     and calibration index

    Returns:
        tuple: (subX_list, subX_dict) where:
            - subX_list: List of information matrices (RᵢᵀRᵢ)
            - subX_dict: Dictionary mapping config index to matrix

    Mathematical Details:
        For configuration i:
        Rᵢ = R[i*idx:(i+1)*idx, :]  (extract rows)
        Xᵢ = RᵢᵀRᵢ  (information matrix)

    Example:
        >>> R_full = np.random.rand(6000, 42)  # 1000 configs, 6 DOF
        >>> subX_list, subX_dict = self.sub_info_matrix(R_full, calib_config)
        >>> print(len(subX_list))  # 1000
        >>> print(subX_dict[0].shape)  # (42, 42)
    """
    subX_list = []
    idex = calib_config["calibration_index"]
    for it in range(calib_config["NbSample"]):
        sub_R = R[it * idex : (it * idex + idex), :]
        subX = np.matmul(sub_R.T, sub_R)
        subX_list.append(subX)
    subX_dict = dict(
        zip(
            np.arange(
                calib_config["NbSample"],
            ),
            subX_list,
        )
    )
    return subX_list, subX_dict
```

### `calculate_optimal_configurations(selection_method='threshold', iroc_rel_tol=0.001, iroc_patience=3)`

Solve SOCP optimization to find optimal configuration subset.

This is the core optimization method that solves the D-optimal experimental design problem using Second-Order Cone Programming. The method finds weights for each candidate configuration that maximize the determinant of the Fisher Information Matrix.

Optimization Problem

maximize det(Σᵢ wᵢ Xᵢ)^(1/n) subject to: Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

Where Xᵢ are information matrices and wᵢ are configuration weights.

Selection Process

1. Solve SOCP optimization for optimal (continuous-relaxation) weights
1. Select a discrete subset from the weight ranking, via one of two interchangeable strategies (`selection_method`):
1. `"threshold"` (default, original behavior): keep every configuration whose weight exceeds `eps_opt` (1e-5). Simple, but says nothing about *how many* configurations are actually enough -- it only discards the ones the SOCP relaxation assigned essentially zero weight.
1. `"iroc"`: rank-and-incrementally-test against the normalized D-optimality criterion (the same quantity :meth:`plot` already computes for visualization, here used as a stopping rule instead): grow the selected subset one configuration at a time in weight-rank order and stop at the first point where the criterion's relative gain stays below `iroc_rel_tol` for `iroc_patience` consecutive configurations in a row (a plateau) -- i.e. an automatic, data-driven minimal configuration count, rather than a fixed threshold on individual weights. See :meth:`_select_by_iroc`.
1. Verify minimum configuration requirement is met
1. Store selected configurations and weights

Parameters:

| Name               | Type    | Description                                                                                                      | Default       |
| ------------------ | ------- | ---------------------------------------------------------------------------------------------------------------- | ------------- |
| `selection_method` | `str`   | "threshold" or "iroc" (see above).                                                                               | `'threshold'` |
| `iroc_rel_tol`     | `float` | Relative-gain plateau tolerance, only used when selection_method="iroc".                                         | `0.001`       |
| `iroc_patience`    | `int`   | Number of consecutive sub-tolerance gains required to declare a plateau, only used when selection_method="iroc". | `3`           |

Side Effects

- Sets self.w_list with optimization solution weights
- Sets self.w_dict_sort with sorted weight dictionary
- Sets self.o1_history with the incremental D-optimality criterion trace when selection_method="iroc" (for inspection/plotting)
- Sets self.optimal_configurations with selected configs
- Sets self.optimal_weights with final weight values
- Sets self.nb_chosen with number of selected configurations
- Prints timing information and selection results

Returns:

| Name   | Type | Description                                  |
| ------ | ---- | -------------------------------------------- |
| `bool` |      | True if optimization successful and feasible |

Raises:

| Type             | Description                                                                                |
| ---------------- | ------------------------------------------------------------------------------------------ |
| `AssertionError` | If regressor not calculated or if insufficient configurations selected (infeasible design) |
| `ValueError`     | If selection_method is not "threshold" or "iroc"                                           |

Example

> > > opt_calib.calculate_optimal_configurations() solve time of socp: 2.35 seconds 12 configs are chosen: [0, 5, 12, 18, 23, ...] opt_calib.calculate_optimal_configurations( ... selection_method="iroc" ... ) 9 configs are chosen: [0, 5, 12, 18, 23, 7, 14, 2, 9]

See Also

SOCPOptimizer: The optimization solver implementation calculate_regressor: Required prerequisite computation \_select_by_iroc: The O1-plateau automatic count selection

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def calculate_optimal_configurations(
    self,
    selection_method="threshold",
    iroc_rel_tol=1e-3,
    iroc_patience=3,
):
    """Solve SOCP optimization to find optimal configuration subset.

    This is the core optimization method that solves the D-optimal
    experimental design problem using Second-Order Cone Programming.
    The method finds weights for each candidate configuration that
    maximize the determinant of the Fisher Information Matrix.

    Optimization Problem:
        maximize det(Σᵢ wᵢ Xᵢ)^(1/n)
        subject to: Σᵢ wᵢ ≤ 1, wᵢ ≥ 0

        Where Xᵢ are information matrices and wᵢ are configuration weights.

    Selection Process:
        1. Solve SOCP optimization for optimal (continuous-relaxation)
           weights
        2. Select a discrete subset from the weight ranking, via one of
           two interchangeable strategies (``selection_method``):

           - ``"threshold"`` (default, original behavior): keep every
             configuration whose weight exceeds ``eps_opt`` (1e-5).
             Simple, but says nothing about *how many* configurations
             are actually enough -- it only discards the ones the SOCP
             relaxation assigned essentially zero weight.
           - ``"iroc"``: rank-and-incrementally-test against the
             normalized D-optimality criterion (the same quantity
             :meth:`plot` already computes for visualization, here used
             as a stopping rule instead): grow the selected subset one
             configuration at a time in weight-rank order and stop at
             the first point where the criterion's relative gain stays
             below ``iroc_rel_tol`` for ``iroc_patience`` consecutive
             configurations in a row (a plateau) -- i.e. an automatic,
             data-driven minimal configuration count, rather than a
             fixed threshold on individual weights. See
             :meth:`_select_by_iroc`.
        3. Verify minimum configuration requirement is met
        4. Store selected configurations and weights

    Args:
        selection_method (str): "threshold" or "iroc" (see above).
        iroc_rel_tol (float): Relative-gain plateau tolerance, only
            used when ``selection_method="iroc"``.
        iroc_patience (int): Number of consecutive sub-tolerance gains
            required to declare a plateau, only used when
            ``selection_method="iroc"``.

    Side Effects:
        - Sets self.w_list with optimization solution weights
        - Sets self.w_dict_sort with sorted weight dictionary
        - Sets self.o1_history with the incremental D-optimality
          criterion trace when selection_method="iroc" (for
          inspection/plotting)
        - Sets self.optimal_configurations with selected configs
        - Sets self.optimal_weights with final weight values
        - Sets self.nb_chosen with number of selected configurations
        - Prints timing information and selection results

    Returns:
        bool: True if optimization successful and feasible

    Raises:
        AssertionError: If regressor not calculated or if insufficient
                      configurations selected (infeasible design)
        ValueError: If selection_method is not "threshold" or "iroc"

    Example:
        >>> opt_calib.calculate_optimal_configurations()
        solve time of socp: 2.35 seconds
        12 configs are chosen: [0, 5, 12, 18, 23, ...]
        >>> opt_calib.calculate_optimal_configurations(
        ...     selection_method="iroc"
        ... )
        9 configs are chosen: [0, 5, 12, 18, 23, 7, 14, 2, 9]

    See Also:
        SOCPOptimizer: The optimization solver implementation
        calculate_regressor: Required prerequisite computation
        _select_by_iroc: The O1-plateau automatic count selection
    """
    import time

    assert self.calculate_regressor(), "Calculate regressor first."

    # Picos optimization (A-optimality, C-optimality, D-optimality)
    prev_time = time.time()
    SOCP_algo = SOCPOptimizer(self._subX_dict, self.calib_config)
    self.w_list, self.w_dict_sort = SOCP_algo.solve()
    solve_time = time.time() - prev_time
    logger.info(f"solve time of socp: {solve_time}")

    if selection_method == "threshold":
        chosen_config = self._select_by_threshold()
    elif selection_method == "iroc":
        chosen_config = self._select_by_iroc(iroc_rel_tol, iroc_patience)
    else:
        raise ValueError(
            "selection_method must be 'threshold' or 'iroc', got "
            f"{selection_method!r}."
        )

    assert (
        len(chosen_config) >= self.minNbChosen
    ), "Infeasible design, try to increase NbSample."

    logger.info(f"{len(chosen_config)} configs are chosen: {chosen_config}")
    self.nb_chosen = len(chosen_config)

    # Store optimal configurations and weights
    opt_ids = chosen_config
    opt_configs_values = []
    for opt_id in opt_ids:
        opt_configs_values.append(
            self._configs["calibration_joint_configurations"][opt_id]
        )
    self.optimal_configurations = self._configs.copy()
    self.optimal_configurations["calibration_joint_configurations"] = list(
        opt_configs_values
    )
    self.optimal_weights = self.w_list
    return True
```

### `plot()`

Generate comprehensive visualization of optimization results.

Creates dual-panel plots that provide insight into the optimization quality and configuration selection process. The visualizations help assess the efficiency of the selected configuration subset.

Plot Components:

1. D-optimality criterion vs. number of configurations

- Shows how information matrix determinant improves with additional configurations
- Normalized against theoretical maximum (all configurations)
- Helps identify diminishing returns point

1. Configuration weights in logarithmic scale
1. Displays weight assigned to each candidate configuration
1. Configurations above threshold (eps_opt) are selected
1. Shows selection boundary and weight distribution

Prerequisites

- Optimization must be completed (optimal_configurations available)
- Information matrices must be computed

Side Effects

- Creates matplotlib figure with two subplots
- Displays plots using plt.show()
- May block execution until plots are closed

Returns:

| Name   | Type | Description                 |
| ------ | ---- | --------------------------- |
| `bool` |      | True if plotting successful |

Mathematical Details

D-optimality ratio = detroot_whole / det(selected_subset) This ratio approaches 1.0 as selected subset approaches optimality.

Example

> > > opt_calib.solve()
> > >
> > > #### Plot is automatically generated, or call manually:
> > >
> > > opt_calib.plot()

See Also

calculate_optimal_configurations: Generates data for plotting calculate_detroot_whole: Provides normalization reference

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def plot(self):
    """Generate comprehensive visualization of optimization results.

    Creates dual-panel plots that provide insight into the optimization
    quality and configuration selection process. The visualizations help
    assess the efficiency of the selected configuration subset.

    Plot Components:
    1. D-optimality criterion vs. number of configurations
       - Shows how information matrix determinant improves with
         additional configurations
       - Normalized against theoretical maximum (all configurations)
       - Helps identify diminishing returns point

    2. Configuration weights in logarithmic scale
       - Displays weight assigned to each candidate configuration
       - Configurations above threshold (eps_opt) are selected
       - Shows selection boundary and weight distribution

    Prerequisites:
        - Optimization must be completed (optimal_configurations available)
        - Information matrices must be computed

    Side Effects:
        - Creates matplotlib figure with two subplots
        - Displays plots using plt.show()
        - May block execution until plots are closed

    Returns:
        bool: True if plotting successful

    Mathematical Details:
        D-optimality ratio = detroot_whole / det(selected_subset)
        This ratio approaches 1.0 as selected subset approaches optimality.

    Example:
        >>> opt_calib.solve()
        >>> # Plot is automatically generated, or call manually:
        >>> opt_calib.plot()

    See Also:
        calculate_optimal_configurations: Generates data for plotting
        calculate_detroot_whole: Provides normalization reference
    """
    import picos as pc

    assert (
        hasattr(self, "optimal_configurations")
        and self.optimal_configurations is not None
    ), "Calculate optimal configurations first."

    # Plotting
    det_root_list = []
    n_key_list = []

    # Calculate det_root_list and n_key_list
    for nbc in range(self.minNbChosen, self.calib_config["NbSample"] + 1):
        n_key = list(self.w_dict_sort.keys())[0:nbc]
        n_key_list.append(n_key)
        M_i = pc.sum(self.w_dict_sort[i] * self._subX_list[i] for i in n_key)
        det_root_list.append(pc.DetRootN(M_i) / np.sqrt(nbc))

    # Create subplots
    fig, ax = plt.subplots(2)

    # Plot D-optimality criterion
    ratio = self.detroot_whole / det_root_list[-1]
    plot_range = self.calib_config["NbSample"] - self.minNbChosen
    ax[0].set_ylabel("D-optimality criterion", fontsize=20)
    ax[0].tick_params(axis="y", labelsize=18)
    ax[0].plot(ratio * np.array(det_root_list[:plot_range]))
    ax[0].spines["top"].set_visible(False)
    ax[0].spines["right"].set_visible(False)
    ax[0].grid(True, linestyle="--")

    # Plot quality of estimation
    ax[1].set_ylabel("Weight values (log)", fontsize=20)
    ax[1].set_xlabel("Data sample", fontsize=20)
    ax[1].tick_params(axis="both", labelsize=18)
    ax[1].tick_params(axis="y", labelrotation=30)
    ax[1].scatter(
        np.arange(len(list(self.w_dict_sort.values()))),
        list(self.w_dict_sort.values()),
    )
    ax[1].set_yscale("log")
    ax[1].spines["top"].set_visible(False)
    ax[1].spines["right"].set_visible(False)
    ax[1].grid(True, linestyle="--")
    plt.show()

    return True
```

### `plot_results()`

Plot optimal calibration results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def plot_results(self):
    """Plot optimal calibration results using unified results manager."""
    if (
        not hasattr(self, "optimal_configurations")
        or self.optimal_configurations is None
    ):
        logger.warning(
            "No optimal configuration results to plot. Run solve() first."
        )
        return

    def _basic_plots():
        fig, ax = plt.subplots(1, 2, figsize=(14, 6))

        ax[0].bar(
            list(self.w_dict_sort.keys()),
            list(self.w_dict_sort.values()),
        )
        ax[0].set_xlabel("Configuration indices")
        ax[0].set_ylabel("Weights")
        ax[0].set_title("Chosen configurations")
        ax[0].spines["top"].set_visible(False)
        ax[0].spines["right"].set_visible(False)
        ax[0].grid(True, linestyle="--")

        ax[1].bar(
            list(self.w_dict_sort.keys()),
            list(self.w_dict_sort.values()),
        )
        ax[1].set_yscale("log")
        ax[1].spines["top"].set_visible(False)
        ax[1].spines["right"].set_visible(False)
        ax[1].grid(True, linestyle="--")
        plt.show()

    def _managed_plot():
        robot_name = self.calib_config.get("robot_name", self.model.name)
        results_manager = ResultsManager("optimal_calibration", robot_name)
        weights = (
            np.array(list(self.w_dict_sort.values()))
            if hasattr(self, "w_dict_sort")
            else np.array([])
        )
        results_manager.plot_optimal_calibration_results(
            configurations=self.optimal_configurations,
            weights=weights,
            title="Optimal Calibration Configuration Results",
        )

    plot_with_fallback(_managed_plot, _basic_plots, logger, "optimal_calibration")
```

### `get_optimal_output_dir(default='results')`

Return the directory optimal-configuration results are written to.

Honours the directory component of the configured `tasks.optimal_configuration.output.output_file`. The *filename* is managed by :class:`ResultsManager` (timestamped, one file per format), so only the directory part of that setting is used.

Parameters:

| Name      | Type  | Description                                  | Default     |
| --------- | ----- | -------------------------------------------- | ----------- |
| `default` | `str` | Directory to use when nothing is configured. | `'results'` |

Returns:

| Name  | Type | Description            |
| ----- | ---- | ---------------------- |
| `str` |      | Output directory path. |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def get_optimal_output_dir(self, default="results"):
    """Return the directory optimal-configuration results are written to.

    Honours the directory component of the configured
    ``tasks.optimal_configuration.output.output_file``. The *filename* is
    managed by :class:`ResultsManager` (timestamped, one file per format),
    so only the directory part of that setting is used.

    Args:
        default (str): Directory to use when nothing is configured.

    Returns:
        str: Output directory path.
    """
    configured = getattr(self, "_optimal_output_file", None)
    if not configured:
        return default
    return os.path.dirname(configured) or default
```

### `count_optimal_configurations()`

Return the number of selected configurations.

`optimal_configurations` is a *dict* holding the joint names alongside the selected joint configurations, so `len()` on it counts keys (2), not configurations. Always go through this helper.

Returns:

| Name  | Type | Description                                         |
| ----- | ---- | --------------------------------------------------- |
| `int` |      | Number of selected joint configurations, 0 if none. |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def count_optimal_configurations(self):
    """Return the number of selected configurations.

    ``optimal_configurations`` is a *dict* holding the joint names alongside
    the selected joint configurations, so ``len()`` on it counts keys (2),
    not configurations. Always go through this helper.

    Returns:
        int: Number of selected joint configurations, 0 if none.
    """
    configs = getattr(self, "optimal_configurations", None)
    if not configs:
        return 0
    return len(configs.get("calibration_joint_configurations", []))
```

### `save_results(output_dir=None)`

Save optimal configuration results using unified results manager.

Parameters:

| Name         | Type  | Description                                                                                                                                                                            | Default |
| ------------ | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `output_dir` | `str` | Directory to write results into. When omitted, the directory configured under the unified config's tasks.optimal_configuration.output.output_file is used, and failing that "results". | `None`  |

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def save_results(self, output_dir=None):
    """Save optimal configuration results using unified results manager.

    Args:
        output_dir (str, optional): Directory to write results into. When
            omitted, the directory configured under the unified config's
            ``tasks.optimal_configuration.output.output_file`` is used, and
            failing that ``"results"``.
    """
    if (
        not hasattr(self, "optimal_configurations")
        or self.optimal_configurations is None
    ):
        logger.warning(
            "No optimal configuration results to save. Run solve() first."
        )
        return

    if output_dir is None:
        output_dir = self.get_optimal_output_dir()

    try:
        # Initialize results manager
        robot_name = self.calib_config.get("robot_name", self.model.name)
        results_manager = ResultsManager("optimal_calibration", robot_name)

        # Prepare results dictionary
        results_dict = {
            "optimal_configurations": self.optimal_configurations,
            "selected_weights": (
                self.w_dict_sort if hasattr(self, "w_dict_sort") else {}
            ),
            "minimum_configurations": getattr(self, "minNbChosen", 0),
            "configuration_count": self.count_optimal_configurations(),
            "calibration_config": self.calib_config,
        }

        # D-optimality determinant root of the full information matrix.
        # Named after what it is: this is NOT a condition number.
        if hasattr(self, "detroot_whole"):
            results_dict["d_optimality_detroot"] = float(self.detroot_whole)

        # Save using unified manager
        saved_files = results_manager.save_results(
            results_dict, output_dir, save_formats=["yaml", "csv"]
        )

        return saved_files

    except ImportError:
        # Fallback to existing saving
        import os
        import yaml

        os.makedirs(output_dir, exist_ok=True)

        robot_name = self.calib_config.get("robot_name", self.model.name)
        filename = f"{robot_name}_optimal_configurations.yaml"

        with open(os.path.join(output_dir, filename), "w") as stream:
            try:
                yaml.dump(
                    self.optimal_configurations,
                    stream,
                    sort_keys=False,
                    default_flow_style=True,
                )
            except yaml.YAMLError as exc:
                logger.error(exc)
        logger.info(f"Results saved to {output_dir}/{filename}")

        return {"yaml": os.path.join(output_dir, filename)}
```

## `SOCPOptimizer(subX_dict, calib_config)`

Second-Order Cone Programming optimizer for configuration selection.

Implements the mathematical optimization for D-optimal experimental design using Second-Order Cone Programming (SOCP). This class formulates and solves the convex optimization problem that maximizes the determinant of the Fisher Information Matrix.

Mathematical Formulation

maximize t subject to: t ≤ det(Σᵢ wᵢ Xᵢ)^(1/n) Σᵢ wᵢ ≤ 1 wᵢ ≥ 0

Where:

- t is auxiliary variable for objective
- wᵢ are configuration weights
- Xᵢ are information matrices
- n is matrix dimension

The problem is solved using the CVXOPT solver with picos interface.

Attributes:

| Name           | Type   | Description                                             |
| -------------- | ------ | ------------------------------------------------------- |
| `pool`         | `dict` | Dictionary of information matrices indexed by config ID |
| `calib_config` | `dict` | Calibration parameters including sample count           |
| `problem`      |        | Picos optimization problem instance                     |
| `w`            |        | Decision variable for configuration weights             |
| `t`            |        | Auxiliary variable for determinant objective            |
| `solution`     |        | Optimization solution object                            |

Example

> > > optimizer = SOCPOptimizer(subX_dict, calib_config) weights, sorted_weights = optimizer.solve() print(f"Optimization status: {optimizer.solution.status}")

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def __init__(self, subX_dict, calib_config):
    import picos as pc

    self.pool = subX_dict
    self.calib_config = calib_config
    self.problem = pc.Problem()
    self.w = pc.RealVariable("w", self.calib_config["NbSample"], lower=0)
    self.t = pc.RealVariable("t", 1)
```

## `Detmax(candidate_pool, NbChosen)`

Determinant Maximization optimizer using greedy exchange algorithm.

This class implements a heuristic optimization algorithm for D-optimal experimental design that uses a greedy exchange strategy to find near-optimal configuration subsets. Unlike the SOCP approach, this method provides a combinatorial solution that directly selects discrete configurations.

Algorithm Overview

The DetMax algorithm uses an iterative exchange procedure:

1. Initialize with a random subset of configurations
1. Iteratively add the configuration that maximally improves the determinant criterion
1. Remove the configuration whose absence minimally degrades the determinant criterion
1. Repeat until convergence (no beneficial exchanges)

Mathematical Background

The algorithm maximizes det(Σᵢ∈S Xᵢ)^(1/n) where:

- S is the selected configuration subset
- Xᵢ are information matrices for configurations
- n is the matrix dimension

This is a discrete optimization problem (vs continuous SOCP).

Advantages

- Provides exact discrete solution (no weight thresholding)
- Computationally efficient for small to medium problems
- Intuitive greedy strategy with good convergence properties
- No external optimization solvers required

Limitations

- May converge to local optima (not globally optimal)
- Performance depends on random initialization
- Computational complexity grows with candidate pool size

Attributes:

| Name        | Type   | Description                                             |
| ----------- | ------ | ------------------------------------------------------- |
| `pool`      | `dict` | Dictionary of information matrices indexed by config ID |
| `nd`        | `int`  | Number of configurations to select                      |
| `cur_set`   | `list` | Current configuration subset being evaluated            |
| `fail_set`  | `list` | Configurations that failed selection criteria           |
| `opt_set`   | `list` | Final optimal configuration subset                      |
| `opt_critD` | `list` | Evolution of determinant criterion during optimization  |

Example

> > > ### Create DetMax optimizer
> > >
> > > detmax = Detmax(subX_dict, num_configs=12)
> > >
> > > ### Run optimization
> > >
> > > criterion_history = detmax.main_algo()
> > >
> > > ### Get selected configurations
> > >
> > > selected_configs = detmax.cur_set final_criterion = criterion_history[-1]
> > >
> > > print(f"Selected {len(selected_configs)} configurations") print(f"Final D-optimality: {final_criterion:.4f}")

See Also

SOCPOptimizer: Alternative SOCP-based optimization approach BaseOptimalCalibration: Main calibration framework

Initialize DetMax optimizer with candidate pool and target size.

Sets up the determinant maximization optimizer with the candidate configuration pool and specifies the number of configurations to select in the final optimal subset.

Parameters:

| Name             | Type   | Description                                                                                                                                                        | Default    |
| ---------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ---------- |
| `candidate_pool` | `dict` | Dictionary mapping configuration indices to their corresponding information matrices. Keys are configuration IDs, values are symmetric positive definite matrices. | *required* |
| `NbChosen`       | `int`  | Number of configurations to select in the optimal subset. Must be less than total candidates and sufficient for parameter identifiability.                         | *required* |

Raises:

| Type         | Description                             |
| ------------ | --------------------------------------- |
| `ValueError` | If NbChosen exceeds candidate pool size |
| `TypeError`  | If candidate_pool is not a dictionary   |

Side Effects

- Initializes internal tracking lists (cur_set, fail_set, etc.)
- Stores candidate pool and selection target

Example

> > > ### Information matrices dict
> > >
> > > info_matrices = {0: X0, 1: X1, 2: X2, ...} optimizer = Detmax(info_matrices, NbChosen=10)

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def __init__(self, candidate_pool, NbChosen):
    """Initialize DetMax optimizer with candidate pool and target size.

    Sets up the determinant maximization optimizer with the candidate
    configuration pool and specifies the number of configurations to
    select in the final optimal subset.

    Args:
        candidate_pool (dict): Dictionary mapping configuration indices
                             to their corresponding information matrices.
                             Keys are configuration IDs, values are
                             symmetric positive definite matrices.
        NbChosen (int): Number of configurations to select in the optimal
                      subset. Must be less than total candidates and
                      sufficient for parameter identifiability.

    Raises:
        ValueError: If NbChosen exceeds candidate pool size
        TypeError: If candidate_pool is not a dictionary

    Side Effects:
        - Initializes internal tracking lists (cur_set, fail_set, etc.)
        - Stores candidate pool and selection target

    Example:
        >>> # Information matrices dict
        >>> info_matrices = {0: X0, 1: X1, 2: X2, ...}
        >>> optimizer = Detmax(info_matrices, NbChosen=10)
    """
    self.pool = candidate_pool
    self.nd = NbChosen
    self.cur_set = []
    self.fail_set = []
    self.opt_set = []
    self.opt_critD = []
```

### `get_critD(set)`

Calculate D-optimality criterion for configuration subset.

Computes the determinant root of the Fisher Information Matrix formed by summing the information matrices of configurations in the specified subset. This serves as the objective function for the determinant maximization algorithm.

Parameters:

| Name  | Type   | Description                                                                                   | Default    |
| ----- | ------ | --------------------------------------------------------------------------------------------- | ---------- |
| `set` | `list` | List of configuration indices from the candidate pool to include in the criterion calculation | *required* |

Returns:

| Name    | Type | Description                                                                                             |
| ------- | ---- | ------------------------------------------------------------------------------------------------------- |
| `float` |      | D-optimality criterion value (determinant root) Higher values indicate better parameter identifiability |

Raises:

| Type             | Description                                      |
| ---------------- | ------------------------------------------------ |
| `AssertionError` | If any configuration index not in candidate pool |

Mathematical Details

For subset S, computes: det(Σᵢ∈S Xᵢ)^(1/n) where Xᵢ are information matrices and n is matrix dimension

Example

> > > subset = [0, 5, 12, 18] # Configuration indices criterion = optimizer.get_critD(subset) print(f"D-optimality: {criterion:.6f}")

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def get_critD(self, set):
    """Calculate D-optimality criterion for configuration subset.

    Computes the determinant root of the Fisher Information Matrix
    formed by summing the information matrices of configurations
    in the specified subset. This serves as the objective function
    for the determinant maximization algorithm.

    Args:
        set (list): List of configuration indices from the candidate
                   pool to include in the criterion calculation

    Returns:
        float: D-optimality criterion value (determinant root)
              Higher values indicate better parameter identifiability

    Raises:
        AssertionError: If any configuration index not in candidate pool

    Mathematical Details:
        For subset S, computes: det(Σᵢ∈S Xᵢ)^(1/n)
        where Xᵢ are information matrices and n is matrix dimension

    Example:
        >>> subset = [0, 5, 12, 18]  # Configuration indices
        >>> criterion = optimizer.get_critD(subset)
        >>> print(f"D-optimality: {criterion:.6f}")
    """
    import picos as pc

    infor_mat = 0
    for idx in set:
        assert idx in self.pool.keys(), "chosen sample not in candidate pool"
        infor_mat += self.pool[idx]
    return float(pc.DetRootN(infor_mat))
```

### `main_algo()`

Execute the main determinant maximization algorithm.

Implements the greedy exchange algorithm for D-optimal experimental design. The algorithm alternates between adding configurations that maximally improve the determinant and removing configurations whose absence minimally degrades the determinant.

Algorithm Steps:

1. Initialize random subset of target size from candidate pool
1. Exchange Loop: a. ADD PHASE: Find configuration that maximally improves criterion b. REMOVE PHASE: Find configuration whose removal minimally hurts c. Update current subset and criterion value
1. Repeat until convergence (no beneficial exchanges)
1. Return optimization history

Convergence Condition

The algorithm stops when the optimal configuration to add equals the optimal configuration to remove, indicating no further improvement is possible.

Returns:

| Name   | Type | Description                                                                                                  |
| ------ | ---- | ------------------------------------------------------------------------------------------------------------ |
| `list` |      | History of D-optimality criterion values throughout the optimization process. Last value is final criterion. |

Side Effects

- Updates self.cur_set with final optimal configuration subset
- Updates self.opt_critD with complete optimization history
- Uses random initialization (results may vary between runs)

Complexity

O(max_iterations × candidate_pool_size × target_subset_size) where max_iterations depends on problem structure and initialization

Example

> > > optimizer = Detmax(info_matrices, NbChosen=10) history = optimizer.main_algo() print(f"Converged after {len(history)} iterations") print(f"Final subset: {optimizer.cur_set}") print(f"Final criterion: {history\[-1\]:.6f}")

Note

The algorithm may converge to different local optima depending on random initialization. For critical applications, consider running multiple times with different seeds.

Source code in `src/figaroh/optimal/base_optimal_calibration.py`

```
def main_algo(self):
    """Execute the main determinant maximization algorithm.

    Implements the greedy exchange algorithm for D-optimal experimental
    design. The algorithm alternates between adding configurations that
    maximally improve the determinant and removing configurations whose
    absence minimally degrades the determinant.

    Algorithm Steps:
    1. Initialize random subset of target size from candidate pool
    2. Exchange Loop:
       a. ADD PHASE: Find configuration that maximally improves criterion
       b. REMOVE PHASE: Find configuration whose removal minimally hurts
       c. Update current subset and criterion value
    3. Repeat until convergence (no beneficial exchanges)
    4. Return optimization history

    Convergence Condition:
        The algorithm stops when the optimal configuration to add
        equals the optimal configuration to remove, indicating no
        further improvement is possible.

    Returns:
        list: History of D-optimality criterion values throughout
             the optimization process. Last value is final criterion.

    Side Effects:
        - Updates self.cur_set with final optimal configuration subset
        - Updates self.opt_critD with complete optimization history
        - Uses random initialization (results may vary between runs)

    Complexity:
        O(max_iterations × candidate_pool_size × target_subset_size)
        where max_iterations depends on problem structure and
        initialization

    Example:
        >>> optimizer = Detmax(info_matrices, NbChosen=10)
        >>> history = optimizer.main_algo()
        >>> print(f"Converged after {len(history)} iterations")
        >>> print(f"Final subset: {optimizer.cur_set}")
        >>> print(f"Final criterion: {history[-1]:.6f}")

    Note:
        The algorithm may converge to different local optima depending
        on random initialization. For critical applications, consider
        running multiple times with different seeds.
    """
    import random

    # get all indices in the pool
    pool_idx = tuple(self.pool.keys())

    # initialize a random set
    cur_set = random.sample(pool_idx, self.nd)
    updated_pool = list(set(pool_idx) - set(self.cur_set))

    # adding samples from remaining pool: k = 1
    opt_k = updated_pool[0]
    opt_critD = self.get_critD(cur_set)
    rm_j = cur_set[0]

    while opt_k != rm_j:

        # add
        for k in updated_pool:
            cur_set.append(k)
            cur_critD = self.get_critD(cur_set)
            if opt_critD < cur_critD:
                opt_critD = cur_critD
                opt_k = k
            cur_set.remove(k)
        cur_set.append(opt_k)
        opt_critD = self.get_critD(cur_set)

        # remove
        delta_critD = opt_critD
        rm_j = cur_set[0]
        for j in cur_set:
            rm_set = cur_set.copy()
            rm_set.remove(j)
            cur_delta_critD = opt_critD - self.get_critD(rm_set)

            if cur_delta_critD < delta_critD:
                delta_critD = cur_delta_critD
                rm_j = j
        cur_set.remove(rm_j)
        opt_critD = self.get_critD(cur_set)

        self.opt_critD.append(opt_critD)
    return self.opt_critD
```

## base_optimal_trajectory

Base Optimal Trajectory Generation Framework

This module provides base classes for optimal trajectory generation with configuration management, parameter computation, constraint handling, and IPOPT-based optimization. This framework can be extended for different robots.

## `BaseOptimalTrajectory(robot, active_joints, config_file='config/robot_config.yaml')`

Base class for IPOPT-based optimal trajectory generation.

Features:

- Modular design with separated concerns
- Better error handling and logging
- Configuration validation
- Cleaner interfaces

This base class can be extended for specific robots by implementing robot-specific configuration loading and constraint handling.

Initialize the optimal trajectory generator.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def __init__(
    self,
    robot,
    active_joints: List[str],
    config_file: str = "config/robot_config.yaml",
):
    """Initialize the optimal trajectory generator."""
    self.robot = robot
    self.model = self.robot.model
    self.active_joints = active_joints

    # Set up logger (configuration should be done by application, not library)
    self.logger = logging.getLogger(__name__)

    # Load configuration
    self.trajectory_config, self.identif_config = load_param(
        self.robot, config_file
    )

    # # Initialize components
    # self.initialize()

    # Results storage
    self.results = {
        "T_F": [],
        "P_F": [],
        "V_F": [],
        "A_F": [],
        "iteration_data": [],
        "final_regressor_shape": None,
    }
```

### `initialize()`

Initialize trajectory generation components.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def initialize(self):
    """Initialize trajectory generation components."""
    # Create soft limit pool
    n_active_joints = len(self.active_joints)
    self.soft_lim_pool = np.full(
        (3, n_active_joints), self.trajectory_config["soft_lim"]
    )

    # Initialize cubic spline and waypoint generation
    self.CB = CubicSpline(
        self.robot,
        self.trajectory_config["n_wps"],
        self.active_joints,
        self.trajectory_config["soft_lim"],
    )
    self.WP = WaypointsGeneration(
        self.robot,
        self.trajectory_config["n_wps"],
        self.active_joints,
        self.trajectory_config["soft_lim"],
    )

    # Initialize specialized components
    self.base_computer = BaseParameterComputer(
        self.robot, self.identif_config, self.active_joints, self.soft_lim_pool
    )
    self.constraint_manager = TrajectoryConstraintManager(
        self.robot, self.CB, self.trajectory_config, self.identif_config
    )

    # Compute base parameters
    self.idx_e, self.idx_b = self.base_computer.compute_base_indices()

    self.logger.info(
        f"BaseOptimalTrajectory initialized with {len(self.idx_b)} base parameters"
    )
```

### `solve(stack_reps=2)`

Solve the optimal trajectory generation problem.

Parameters:

| Name         | Type  | Description                            | Default |
| ------------ | ----- | -------------------------------------- | ------- |
| `stack_reps` | `int` | Number of trajectory segments to stack | `2`     |

Returns:

| Type             | Description                                        |
| ---------------- | -------------------------------------------------- |
| `Dict[str, Any]` | Dict containing trajectories and optimization info |

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def solve(self, stack_reps: int = 2) -> Dict[str, Any]:
    """
    Solve the optimal trajectory generation problem.

    Args:
        stack_reps: Number of trajectory segments to stack

    Returns:
        Dict containing trajectories and optimization info
    """
    self.logger.info(
        f"Starting optimal trajectory generation with {stack_reps} segments..."
    )

    try:
        # Initialize
        self.WP.gen_rand_pool(self.soft_lim_pool)
        wp_init = np.zeros(len(self.CB.act_idxq))
        vel_wp_init = np.zeros(len(self.CB.act_idxv))
        acc_wp_init = np.zeros(len(self.CB.act_idxv))

        # Random initial position
        for idx in range(len(self.CB.act_idxq)):
            wp_init[idx] = np.random.choice(self.WP.pool_q[:, idx], 1)[0]

        W_stack = None

        for s_rep in range(stack_reps):
            self.logger.info(f"Optimizing segment {s_rep + 1}/{stack_reps}")
            self.logger.info(f"Initial waypoint: {wp_init}")

            success = self._solve_segment(
                s_rep, wp_init, vel_wp_init, acc_wp_init, W_stack
            )

            if not success:
                self.logger.error(f"Failed to solve segment {s_rep + 1}")
                break

            # Update for next segment
            if s_rep < stack_reps - 1:  # Not the last segment
                wp_init, W_stack = self._prepare_next_segment()

        self.logger.info(
            f"Completed! Generated {len(self.results['T_F'])} trajectory segments"
        )
        self.results["final_regressor_shape"] = (
            W_stack.shape if W_stack is not None else None
        )

        return self.results

    except Exception as e:
        self.logger.error(f"Error in solve: {e}")
        raise
```

### `objective_function(X, opt_cb, tps, vel_wps, acc_wps, wp_init, W_stack=None)`

Objective function: condition number of base regressor matrix.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def objective_function(
    self, X, opt_cb, tps, vel_wps, acc_wps, wp_init, W_stack=None
):
    """Objective function: condition number of base regressor matrix."""
    try:
        # Reshape and arrange waypoints
        X = np.array(X)
        wps_X = np.reshape(
            X,
            (self.trajectory_config["n_wps"] - 1, len(self.active_joints)),
        )
        wps = np.vstack((wp_init, wps_X))
        wps = wps.transpose()

        # Generate full trajectory configuration
        t_f, p_f, v_f, a_f = self.CB.get_full_config(
            self.trajectory_config["freq"], tps, wps, vel_wps, acc_wps
        )

        # Store in callback dictionary
        opt_cb.update({"t_f": t_f, "p_f": p_f, "v_f": v_f, "a_f": a_f})

        # Build stacked base regressor and return condition number
        W_b = self._stack_base_regressors(p_f, v_f, a_f, W_stack=W_stack)
        return np.linalg.cond(W_b)

    except Exception as e:
        self.logger.error(f"Error in objective function: {e}")
        return 1e10  # Return large penalty value
```

### `create_ipopt_problem(n_joints, n_wps, Ns, tps, vel_wps, acc_wps, wp_init, vel_wp_init, acc_wp_init, W_stack)`

Create IPOPT problem instance. Should be implemented by subclasses.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
@abstractmethod
def create_ipopt_problem(
    self,
    n_joints,
    n_wps,
    Ns,
    tps,
    vel_wps,
    acc_wps,
    wp_init,
    vel_wp_init,
    acc_wp_init,
    W_stack,
):
    """Create IPOPT problem instance. Should be implemented by subclasses."""
    raise NotImplementedError("Subclasses must implement create_ipopt_problem")
```

### `plot_results()`

Plot optimal trajectory results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def plot_results(self):
    """Plot optimal trajectory results using unified results manager."""
    if not self.results["T_F"]:
        self.logger.warning("No trajectory data to plot")
        return

    def _basic_plots():
        try:
            # Create subplots
            n_joints = len(self.CB.act_Jid)
            fig, axes = plt.subplots(
                n_joints, 3, sharex=True, figsize=(15, 2 * n_joints)
            )
            if n_joints == 1:
                axes = axes.reshape(1, -1)

            fig.suptitle("Optimal Trajectory Results", fontsize=16)

            # Plot each segment
            colors = plt.cm.tab10(np.linspace(0, 1, len(self.results["T_F"])))

            for seg_idx, (T, P, V, A) in enumerate(
                zip(
                    self.results["T_F"],
                    self.results["P_F"],
                    self.results["V_F"],
                    self.results["A_F"],
                )
            ):
                color = colors[seg_idx]
                label = f"Segment {seg_idx + 1}"

                # P spans nq columns, V and A span nv: index each by
                # the active joint's own idx_q / idx_v (#149).
                for joint_idx, (iq, iv) in enumerate(
                    zip(self.CB.act_idxq, self.CB.act_idxv)
                ):
                    axes[joint_idx, 0].plot(T, P[:, iq], color=color, label=label)
                    axes[joint_idx, 1].plot(T, V[:, iv], color=color, label=label)
                    axes[joint_idx, 2].plot(T, A[:, iv], color=color, label=label)

            # Set labels and formatting
            for joint_idx, name in enumerate(self.CB.act_Jname):
                axes[joint_idx, 0].set_ylabel(f"{name}\nPosition (rad)")
                axes[joint_idx, 1].set_ylabel(f"{name}\nVelocity (rad/s)")
                axes[joint_idx, 2].set_ylabel(f"{name}\nAcceleration (rad/s²)")

                if joint_idx == 0:
                    for col in range(3):
                        axes[joint_idx, col].legend()

                for col in range(3):
                    axes[joint_idx, col].grid(True, alpha=0.3)

            axes[-1, 0].set_xlabel("Time (s)")
            axes[-1, 1].set_xlabel("Time (s)")
            axes[-1, 2].set_xlabel("Time (s)")

            plt.tight_layout()
            plt.show()

        except Exception as e:
            self.logger.error(f"Error plotting results: {e}")

    def _managed_plot():
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        results_manager = ResultsManager("optimal_trajectory", robot_name)

        # Calculate overall condition number
        condition_number = getattr(self, "final_condition_number", 0.0)
        if (
            condition_number == 0.0
            and hasattr(self, "results")
            and "condition_numbers" in self.results
        ):
            condition_number = (
                self.results["condition_numbers"][-1]
                if self.results["condition_numbers"]
                else 0.0
            )

        results_manager.plot_optimal_trajectory_results(
            trajectories=self.results,
            condition_number=condition_number,
            joint_names=self.CB.act_Jname,
            title="Optimal Trajectory Generation Results",
            q_indices=self.CB.act_idxq,
            v_indices=self.CB.act_idxv,
        )

    plot_with_fallback(
        _managed_plot, _basic_plots, self.logger, "optimal_trajectory"
    )
```

### `save_results(output_dir='results')`

Save optimal trajectory results using unified results manager.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def save_results(self, output_dir="results"):
    """Save optimal trajectory results using unified results manager."""
    if not self.results["T_F"]:
        self.logger.warning("No trajectory data to save")
        return

    try:
        # Initialize results manager
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        results_manager = ResultsManager("optimal_trajectory", robot_name)

        # Calculate overall condition number
        condition_number = getattr(self, "final_condition_number", 0.0)
        if (
            condition_number == 0.0
            and hasattr(self, "results")
            and "condition_numbers" in self.results
        ):
            condition_number = (
                self.results["condition_numbers"][-1]
                if self.results["condition_numbers"]
                else 0.0
            )

        # Prepare results dictionary
        results_dict = {
            "trajectory_segments": len(self.results["T_F"]),
            "condition_number": float(condition_number),
            "joint_names": [f"Joint {i+1}" for i in range(len(self.CB.act_Jid))],
            "configuration": self.CB.identif_config,
            "time_segments": [t.tolist() for t in self.results["T_F"]],
            "position_segments": [p.tolist() for p in self.results["P_F"]],
            "velocity_segments": [v.tolist() for v in self.results["V_F"]],
            "acceleration_segments": [a.tolist() for a in self.results["A_F"]],
        }

        # Add condition number history if available
        if "condition_numbers" in self.results:
            results_dict["condition_number_history"] = [
                float(c) for c in self.results["condition_numbers"]
            ]

        # Save using unified manager
        saved_files = results_manager.save_results(
            results_dict, output_dir, save_formats=["yaml", "npz"]
        )

        self.logger.info("Trajectory results saved successfully")
        return saved_files

    except ImportError:
        # Fallback to basic saving
        import os
        import yaml

        os.makedirs(output_dir, exist_ok=True)

        # Basic results dictionary
        robot_name = getattr(self, "robot_name", self.robot.model.name)
        filename = f"{robot_name}_optimal_trajectory.yaml"

        condition_number = getattr(self, "final_condition_number", 0.0)
        results_dict = {
            "trajectory_segments": len(self.results["T_F"]),
            "condition_number": float(condition_number),
            "joint_count": len(self.CB.act_Jid),
        }

        with open(os.path.join(output_dir, filename), "w") as f:
            yaml.dump(results_dict, f, default_flow_style=False)

        self.logger.info(f"Basic results saved to {output_dir}/{filename}")
        return {"yaml": os.path.join(output_dir, filename)}
```

## `BaseTrajectoryIPOPTProblem(opt_traj, n_joints, n_wps, Ns, tps, vel_wps, acc_wps, wp_init, vel_wp_init, acc_wp_init, W_stack, problem_name='TrajectoryOptimization')`

Bases: `BaseOptimizationProblem`

Base IPOPT problem formulation for trajectory optimization.

This class provides a base implementation for trajectory optimization that can be extended for specific robots.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def __init__(
    self,
    opt_traj,
    n_joints,
    n_wps,
    Ns,
    tps,
    vel_wps,
    acc_wps,
    wp_init,
    vel_wp_init,
    acc_wp_init,
    W_stack,
    problem_name="TrajectoryOptimization",
):
    super().__init__(problem_name)

    self.opt_traj = opt_traj
    self.n_joints = n_joints
    self.n_wps = n_wps
    self.Ns = Ns
    self.tps = tps
    self.vel_wps = vel_wps
    self.acc_wps = acc_wps
    self.wp_init = wp_init
    self.vel_wp_init = vel_wp_init
    self.acc_wp_init = acc_wp_init
    self.W_stack = W_stack

    # Storage for optimization callback (inherits callback_data from base)
    self.opt_cb = {"t_f": None, "p_f": None, "v_f": None, "a_f": None}
```

### `get_variable_bounds()`

Get variable bounds for optimization.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_variable_bounds(self) -> Tuple[List[float], List[float]]:
    """Get variable bounds for optimization."""
    return self.opt_traj.constraint_manager.get_variable_bounds()
```

### `get_constraint_bounds()`

Get constraint bounds for optimization.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_constraint_bounds(self) -> Tuple[List[float], List[float]]:
    """Get constraint bounds for optimization."""
    return self.opt_traj.constraint_manager.get_constraint_bounds(self.Ns)
```

### `get_initial_guess()`

Get initial guess from waypoints.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def get_initial_guess(self) -> List[float]:
    """Get initial guess from waypoints."""
    # This will be set when solve() is called with waypoints
    if not hasattr(self, "_initial_wps"):
        # Return zeros as fallback
        return [0.0] * (self.n_joints * (self.n_wps - 1))

    X0 = self._initial_wps[:, range(1, self.n_wps)]
    return np.reshape(X0.transpose(), (self.n_joints * (self.n_wps - 1),)).tolist()
```

### `objective(X)`

Objective function: condition number of base regressor matrix.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def objective(self, X: np.ndarray) -> float:
    """Objective function: condition number of base regressor matrix."""
    return self.opt_traj.objective_function(
        X,
        self.opt_cb,
        self.tps,
        self.vel_wps,
        self.acc_wps,
        self.wp_init,
        self.W_stack,
    )
```

### `constraints(X)`

Constraint function for IPOPT.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def constraints(self, X: np.ndarray) -> np.ndarray:
    """Constraint function for IPOPT."""
    return self.opt_traj.constraint_manager.evaluate_constraints(
        self.Ns, X, self.opt_cb, self.tps, self.vel_wps, self.acc_wps, self.wp_init
    )
```

### `gradient(X)`

Gradient of the objective by forward finite differences.

The default numdifftools gradient uses Richardson extrapolation over many step sizes, which costs tens of seconds per call here because every objective evaluation rebuilds the full regressor.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def gradient(self, X: np.ndarray) -> np.ndarray:
    """
    Gradient of the objective by forward finite differences.

    The default numdifftools gradient uses Richardson extrapolation over
    many step sizes, which costs tens of seconds per call here because
    every objective evaluation rebuilds the full regressor.
    """
    X = np.asarray(X, dtype=float)
    f0 = self.objective(X)
    eps = 1e-6
    grad = np.zeros_like(X)
    for i in range(len(X)):
        X_plus = X.copy()
        X_plus[i] += eps
        grad[i] = (self.objective(X_plus) - f0) / eps
    return grad
```

### `jacobian(X)`

Jacobian of constraints - Custom implementation for better performance.

For trajectory optimization, we can use sparse finite differences instead of full automatic differentiation which is too slow.

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def jacobian(self, X: np.ndarray) -> np.ndarray:
    """
    Jacobian of constraints - Custom implementation for better performance.

    For trajectory optimization, we can use sparse finite differences
    instead of full automatic differentiation which is too slow.
    """
    try:
        # Get current constraint values
        c0 = self.constraints(X)
        n_constraints = len(c0)
        n_vars = len(X)

        # Use finite differences with smaller step size for efficiency
        eps = 1e-6
        jac = np.zeros((n_constraints, n_vars))

        # Compute Jacobian column by column (forward differences)
        for i in range(n_vars):
            X_plus = X.copy()
            X_plus[i] += eps
            c_plus = self.constraints(X_plus)
            jac[:, i] = (c_plus - c0) / eps

        self.logger.debug(f"Constraint jacobian shape: {jac.shape}")
        return jac

    except Exception as e:
        self.logger.warning(f"Error computing jacobian: {e}")
        # Return sparse identity matrix as fallback
        n_constraints = len(self.constraints(X))
        n_vars = len(X)
        # Create a sparse jacobian approximation
        jac = np.zeros((n_constraints, n_vars))
        min_dim = min(n_constraints, n_vars)
        jac[:min_dim, :min_dim] = np.eye(min_dim)
        return jac
```

### `solve_with_waypoints(wps)`

Solve the optimization problem with given initial waypoints.

Parameters:

| Name  | Type | Description       | Default    |
| ----- | ---- | ----------------- | ---------- |
| `wps` |      | Initial waypoints | *required* |

Returns:

| Type                          | Description                      |
| ----------------------------- | -------------------------------- |
| `Tuple[bool, Dict[str, Any]]` | Tuple of (success, results_dict) |

Source code in `src/figaroh/optimal/base_optimal_trajectory.py`

```
def solve_with_waypoints(self, wps) -> Tuple[bool, Dict[str, Any]]:
    """
    Solve the optimization problem with given initial waypoints.

    Args:
        wps: Initial waypoints

    Returns:
        Tuple of (success, results_dict)
    """
    try:
        # Store initial waypoints for get_initial_guess
        self._initial_wps = wps

        # Create solver with trajectory optimization config
        config = IPOPTConfig.for_trajectory_optimization()
        # Adjust settings for this complex problem
        config.tolerance = 1e-3
        config.acceptable_tolerance = 1e-2
        config.max_iterations = self.opt_traj.trajectory_config.get(
            "max_iterations", 200
        )
        config.print_level = 3  # Reduce output
        config.custom_options = {
            b"mu_strategy": b"adaptive",
        }
        solver = RobotIPOPTSolver(self, config)

        # Solve the problem
        success, results = solver.solve()
        converged = success

        # The condition-number objective is non-smooth, so IPOPT often
        # stalls on dual infeasibility after reaching a feasible plateau.
        # Keep such an iterate, but only if it satisfies every constraint.
        if not success and results.get("status") == -1:
            success = self._is_feasible(results["x_opt"])
            if success:
                self.logger.warning(
                    "IPOPT reached the iteration limit (%d) without "
                    "converging; keeping the feasible final iterate "
                    "(objective %.4g)",
                    config.max_iterations,
                    results["obj_val"],
                )

        # Collision is constrained at check points only; accept the
        # segment only if the whole spline is clear (#143).
        if success and not self._is_collision_free(results["x_opt"]):
            self.logger.error(
                "Optimised segment collides between collision check points"
            )
            return False, results

        if success:
            # Extract final waypoint for next segment
            X_opt = results["x_opt"]
            wps_X = np.reshape(np.array(X_opt), (self.n_wps - 1, self.n_joints))
            final_waypoint = wps_X[-1, :]
            # The last callback may have been a finite-difference probe;
            # rebuild the stored trajectory at the returned solution.
            self.objective(np.asarray(X_opt))

            # Update results with trajectory-specific data
            results.update(
                {
                    "t_f": self.opt_cb["t_f"],
                    "p_f": self.opt_cb["p_f"],
                    "v_f": self.opt_cb["v_f"],
                    "a_f": self.opt_cb["a_f"],
                    "iter_data": {
                        "iterations": self.iteration_data["iterations"],
                        "obj_values": self.iteration_data["obj_values"],
                        "solve_time": results["solve_time"],
                        "status": results["status"],
                        "converged": converged,
                        "final_waypoint": final_waypoint,
                    },
                }
            )

            return True, results
        else:
            self.logger.error("Optimization failed")
            return False, results

    except Exception as e:
        self.logger.error(f"Error in IPOPT solve: {e}")
        return False, {"error": str(e)}
```

# Public API and stability

This page is the contract for 1.0. It states which names you can build on, which are experimental, which are internal, and how a public name is retired. The `__all__` lists in the source match it; a unit test (`tests/unit/test_public_api.py`) fails if they drift apart.

## What "public" means

A name is public when it appears in the `__all__` of its package or of a module this page lists. Anything else is not covered, even when it is importable and even when [the API reference](https://thanhndv212.github.io/figaroh-plus/api/index.md) renders it:

- a name that starts with an underscore;
- a module that starts with an underscore;
- a module or name that is not listed here, such as `figaroh.calibration.config` or `figaroh.identification.selection`.

Public names follow [semantic versioning](https://semver.org/): a breaking change to a supported name needs a new major version. Experimental names may change in any minor release.

## Top-level packages

`import figaroh` imports every package below eagerly, so `figaroh.tools.robot` and friends work after a bare import. The status column is the status of the package as a whole; the sections below give the detail.

| Package                  | Status                                           |
| ------------------------ | ------------------------------------------------ |
| `figaroh.tools`          | Supported (`robotvisualization` is experimental) |
| `figaroh.calibration`    | Supported                                        |
| `figaroh.identification` | Supported                                        |
| `figaroh.measurements`   | Experimental                                     |
| `figaroh.utils`          | Supported                                        |
| `figaroh.visualisation`  | Experimental                                     |
| `figaroh.optimal`        | Supported                                        |
| `figaroh.data`           | Supported                                        |
| `figaroh.backends`       | Supported (Pinocchio); MuJoCo is experimental    |
| `figaroh.integration`    | Experimental                                     |

## Supported entry points

Each line names a package or module and the complete list of its public names. For a package, a name that is itself a module (such as `calibration_tools`) means that module is public too and is listed on its own line.

- `figaroh.calibration`: `BaseCalibration`, `calibration_tools`, `data_loader`, `parameter`
- `figaroh.calibration.calibration_tools`: `unified_to_legacy_config`, `get_sup_joints`, `get_joint_offset`, `get_fullparam_offset`, `add_base_name`, `add_pee_name`, `add_eemarker_frame`, `read_config_data`, `load_data`, `get_idxq_from_jname`, `cartesian_to_SE3`, `xyzquat_to_SE3`, `get_rel_transform`, `get_rel_kinreg`, `get_rel_jac`, `initialize_variables`, `calc_updated_fkm`, `update_joint_placement`, `apply_joint_offset`, `random_joint_configuration`, `estimate_frames_closed_form`, `measurement_jacobian`, `select_identifiable_parameters`, `drop_calibration_parameters`, `calculate_kinematics_model`, `calculate_identifiable_kinematics_model`, `calculate_base_kinematics_regressor`
- `figaroh.calibration.parameter`: `get_joint_offset`, `get_fullparam_offset`, `add_base_name`, `add_pee_name`, `add_eemarker_frame`, `FULL_PARAMTPL`, `JOINT_OFFSETTPL`, `ELAS_TPL`, `EE_TPL`, `BASE_TPL`
- `figaroh.calibration.data_loader`: `read_config_data`, `load_data`, `get_idxq_from_jname`
- `figaroh.identification`: `identification_tools`, `parameter`, `physical_fit`, `BaseIdentification`, `BaseResult`, `ReconstructionResult`, `prior_vector_from_dict`, `reconstruct_from_base`, `reconstruct_full_parameters`, `reconstruct_theta_r`, `run_reconstruction`, `run_option_a_reconstruction`, `ProjectionReport`, `RobotProjectionReport`, `check_p10_feasibility`, `is_feasible_link`, `p10_by_joint_from_param_dict`, `param_dict_with_p10_by_joint`, `project_link`, `project_p10_lmi`, `project_robot_p10_lmi`, `pseudo_inertia_matrix_from_p10`, `CADConstraints`, `add_mass_bounds`, `add_com_bounds`, `add_symmetry_constraints`, `bounds_from_urdf`, `build_cad_constraints_from_config`
- `figaroh.identification.identification_tools`: `base_param_from_standard`, `relative_stdev`, `index_in_base_params`, `weigthed_least_squares`, `calculate_first_second_order_differentiation`, `low_pass_filter_data`
- `figaroh.identification.parameter`: `reorder_inertial_parameters`, `add_standard_additional_parameters`, `add_custom_parameters`, `get_standard_parameters`, `get_parameter_info`
- `figaroh.identification.physical_fit`: `FixedExtras`, `PhysicalPolicy`, `ComparatorProblem`, `SolveRecord`, `build_problem`, `solve_exact_reconstruction`, `solve_direct_effort_fit`, `solve_per_link_projection`, `diagnose_exact`
- `figaroh.optimal`: `BaseOptimalCalibration`, `BaseOptimalTrajectory`, `BaseTrajectoryIPOPTProblem`
- `figaroh.data`: `DataSource`, `Session`, `file_sha256`, `Protocol`, `ProtocolSession`, `TrajectoryData`, `PoseObservations`, `JOINT_TORQUE`, `JOINT_FORCE`, `MOTOR_CURRENT`, `MOTOR_TORQUE`, `LOAD_FRACTION`, `DRIVE_KINDS`
- `figaroh.backends`: `DynamicsBackend`, `get_backend`, `list_backends`, `get_backend_info`
- `figaroh.backends.pinocchio`: `PinocchioBackend`
- `figaroh.tools`: `robot`, `randomdata`, `regressor`, `qrdecomposition`, `robotvisualization`, `robotcollisions`, `robotipopt`, `urdf_exporter`, `geometric_calibration_export`, `export_validation`, `report`, `identification_report`, `compare_report`, `run_archive`, `stages`, `solver`
- `figaroh.tools.robot`: `Robot`, `create_robot`, `load_robot`, `get_available_loaders`, `list_available_robots`
- `figaroh.tools.regressor`: `RegressorConfig`, `RegressorBuilder`, `build_regressor_basic`, `eliminate_non_dynaffect`, `get_index_eliminate`, `build_regressor_reduced`, `build_total_regressor_current`, `build_total_regressor_wrench`
- `figaroh.tools.qrdecomposition`: `QRDecomposer`, `QRResult`, `QR_pivoting`, `double_QR`, `get_baseParams`, `get_baseIndex`, `build_baseRegressor`, `cond_num`, `redistribute_min_norm`, `propagate_covariance_min_norm`
- `figaroh.tools.solver`: `LinearSolver`, `solve_linear_system`
- `figaroh.tools.urdf_exporter`: `export_urdf`, `frame_settings_doc`
- `figaroh.tools.geometric_calibration_export`: `build_geometric_calibration`, `export_geometric_calibration_yaml`
- `figaroh.tools.export_validation`: `PoseDelta`, `FkConsistencyResult`, `PoseError`, `URDFComparison`
- `figaroh.tools.run_archive`: `compute_run_dir`, `archive_run`
- `figaroh.tools.stages`: `StageResult`, `record_stage`, `stages_as_dicts`
- `figaroh.tools.report`: `generate_calibration_report`
- `figaroh.tools.identification_report`: `generate_identification_report`
- `figaroh.tools.compare_report`: `generate_compare_page`
- `figaroh.tools.randomdata`: `generate_waypoints`, `generate_waypoints_fext`, `get_torque_rand`
- `figaroh.tools.robotcollisions`: `add_non_adjacent_pairs`, `CollisionManager`, `CollisionWrapper`
- `figaroh.tools.robotipopt`: `IPOPTConfig`, `BaseOptimizationProblem`, `RobotIPOPTSolver`, `TrajectoryOptimizationProblem`, `create_trajectory_solver`
- `figaroh.utils`: `config_parser`, `error_handling`, `ResultsManager`, `FigarohExampleError`, `CalibrationError`, `IdentificationError`, `handle_calibration_errors`, `handle_identification_errors`, `validate_input_data`, `setup_example_logging`
- `figaroh.utils.config_parser`: `UnifiedConfigParser`, `create_task_config`, `parse_configuration`, `get_param_from_yaml`, `is_unified_config`, `ConfigurationError`
- `figaroh.utils.error_handling`: `FigarohExampleError`, `RobotInitializationError`, `ConfigurationError`, `DataProcessingError`, `CalibrationError`, `IdentificationError`, `ValidationError`, `validate_robot_config`, `validate_trajectory_data`, `validate_numeric_range`, `validate_robot_initialization`, `validate_input_data`, `handle_calibration_errors`, `handle_identification_errors`, `safe_execute`, `setup_example_logging`, `ErrorContext`

Notes on judgement calls:

- `figaroh.calibration.calibration_tools` is public because the examples build their calibration pipelines on it. The legacy flat-format `get_param_from_yaml` it used to re-export was removed in 1.0 (see below); `figaroh.utils.config_parser.get_param_from_yaml` is the parser.
- `figaroh.identification.physical_fit` is the module behind `select_stage: physical_fit`. Its functions are public; its private helpers are not.
- The `figaroh.tools` modules `randomdata`, `robotcollisions` and `robotipopt` are public helpers kept for the optimal-design classes.

## Experimental areas

These are importable and tested, but they may change or move in a minor release, and a call can raise `NotImplementedError`.

- **MuJoCo backend and `from_mjcf`.** `MuJoCoBackend` needs the optional `mujoco` package. `RobotIdentificationSystem.from_mjcf` raises `NotImplementedError`: MJCF loading is not supported yet. Genesis and Isaac Sim backends are not shipped; `get_backend` only names them.
- **`figaroh.integration`.** The one-line `RobotIdentificationSystem` workflow.
- **`figaroh.visualisation` and `figaroh.tools.robotvisualization`.** Meshcat and plotting helpers.
- **`figaroh.measurements`.** A measurement container that no shipped workflow uses.
- **Log-Cholesky fits (LC1).** The log-Cholesky physical fit is not part of the library: the feasibility spike was a no-go for 0.7.0 (D5, issue 155). If it ships, it ships as an opt-in, experimental method.

Names in these modules, as they stand today:

- `figaroh.integration`: `RobotIdentificationSystem`, `IdentificationResult`
- `figaroh.visualisation`: `colors`, `MeshcatVisualizer`
- `figaroh.visualisation.colors`: `rgb2int`, `material`, `red`, `blue`, `green`, `yellow`, `magenta`, `cyan`, `white`, `black`, `grey`, `colormap`
- `figaroh.backends.mujoco`: `MuJoCoBackend`
- `figaroh.tools.robotvisualization`: `VisualizationConfig`, `RobotVisualizer`, `place`, `display_COM`, `display_axes`, `rotation_matrix_from_vectors`, `display_force`, `display_bounding_boxes`, `display_joints`
- `figaroh.measurements`: `Measurement`

## Internal modules

These support the public API. Do not import them from outside figaroh; they may change or disappear in any release.

- Any module or name that starts with an underscore, for example `figaroh.identification._physical_comparator` and `figaroh.tools._report_common`.
- `figaroh.tools.load_robot` (use `figaroh.tools.robot`, which re-exports `load_robot`), `figaroh.tools.provenance`.
- `figaroh.calibration.config`, `figaroh.calibration.estimation` and `figaroh.identification.config`, `figaroh.identification.selection`, `figaroh.identification.physical_consistency`, `figaroh.identification.cad_constraints`, `figaroh.identification.reconstruction` and the other modules not listed above. Their main names are re-exported by the packages that list them.
- `figaroh.optimal.config`, `figaroh.optimal.base_parameter`, `figaroh.optimal.contraints`.
- `figaroh.utils.cubic_spline`, `figaroh.utils.config_migration` (run it as `python -m figaroh.utils.config_migration`; its functions are internal).
- `figaroh.data.protocol`, `figaroh.data.source`, `figaroh.data.observations` and `figaroh.data.trajectory`: import their names from `figaroh.data`.

## Deprecated

These names still work but emit a `DeprecationWarning`. Each is deprecated in 1.0 and scheduled for removal in 2.0.

- `figaroh.tools.qrdecomposition.QR_pivoting` and `double_QR`: use `QRDecomposer.decompose_with_pivoting` and `QRDecomposer.double_decomposition`;
- `figaroh.tools.robotcollisions.CollisionWrapper` (warns on construction): use `CollisionManager`;
- the module-level wrappers in `figaroh.tools.robotvisualization` (`place`, `display_COM`, `display_axes`, `rotation_matrix_from_vectors`, `display_force`, `display_bounding_boxes`, `display_joints`): use the `RobotVisualizer` methods;
- the convenience wrappers `plot_calibration_results`, `plot_identification_results` and `save_results` in `figaroh.utils.results_manager`: use the `ResultsManager` methods of the same names;
- the flat `physical_consistency_enabled` identification config key: use `physical_consistency.enabled`.

## Removed in 1.0

These warned for at least one minor release and were removed in 1.0 (issue 185). Each has a migration note in the [CHANGELOG](https://thanhndv212.github.io/figaroh-plus/further_reading/changelog/index.md).

- the legacy flat configuration format (deprecated since 0.4.6): loading it raises an error naming `python -m figaroh.utils.config_migration`, which still reads legacy files to convert them. This removes `figaroh.calibration.config.get_param_from_yaml`, `figaroh.identification.config.get_param_from_yaml`, their `get_param_from_yaml_legacy` wrappers and the `get_param_from_yaml` re-exports of `calibration_tools` and `identification_tools`;
- `get_param_from_yaml_with_warning` in the calibration and identification config modules (since 0.2.0);
- `figaroh.utils.config_parser.get_calibration_param_from_yaml` and `get_identification_param_from_yaml` (since 0.2.0): use `get_param_from_yaml` with `task_type`;
- a non-zero `regularization_coefficient` / `coeff_regularize` (since 0.6.0): now a `ConfigurationError`; zero or absent is still accepted.

## Deprecation policy

- A public name is deprecated by emitting a `DeprecationWarning` that names the replacement. It keeps working for at least one minor release before removal.
- After 1.0, a public name is removed only in a major release.
- Every deprecation and removal has a migration note in the [CHANGELOG](https://thanhndv212.github.io/figaroh-plus/further_reading/changelog/index.md).
- Experimental names and internal modules are outside this policy.

## Python versions

`requires-python` is `>=3.10`, following Pinocchio's own support window. CI tests Python 3.10 (lowest), 3.12 (main) and 3.14 (highest).

# Tools

## Reporting & Verification

The [reporting & verification suite](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md)'s HTML report generators and the static two-run compare page.

### report

HTML diagnostic report generation for calibration results.

Renders the same statistics as `BaseCalibration.print_quality_report()` (convergence, per-DOF residuals, parameter uncertainty, correlation, validation) as a self-contained, shareable HTML page instead of terminal text — with a visual encoding of which parameters are well identified and which are not, and an auto-generated "insights" list flagging things worth a second look.

## `generate_calibration_report(calibrator, output_path=None, title=None)`

Render a self-contained HTML diagnostic report for a calibration run.

Reuses the metrics already computed by `BaseCalibration.print_quality_report()` — no new computation is performed here, only presentation.

Parameters:

| Name          | Type            | Description                                                                                                    | Default    |
| ------------- | --------------- | -------------------------------------------------------------------------------------------------------------- | ---------- |
| `calibrator`  |                 | A BaseCalibration instance after solve() has been called (i.e. evaluation_metrics/results_data are populated). | *required* |
| `output_path` | `Optional[str]` | If given, the HTML is also written to this path.                                                               | `None`     |
| `title`       | `Optional[str]` | Optional report title. Defaults to the robot's class name.                                                     | `None`     |

Returns:

| Type  | Description                             |
| ----- | --------------------------------------- |
| `str` | The rendered HTML document as a string. |

Raises:

| Type             | Description                            |
| ---------------- | -------------------------------------- |
| `AttributeError` | If calibrator has not been solved yet. |

Source code in `src/figaroh/tools/report.py`

```
def generate_calibration_report(
    calibrator, output_path: Optional[str] = None, title: Optional[str] = None
) -> str:
    """Render a self-contained HTML diagnostic report for a calibration run.

    Reuses the metrics already computed by
    ``BaseCalibration.print_quality_report()`` — no new computation is
    performed here, only presentation.

    Args:
        calibrator: A ``BaseCalibration`` instance after ``solve()`` has
            been called (i.e. ``evaluation_metrics``/``results_data`` are
            populated).
        output_path: If given, the HTML is also written to this path.
        title: Optional report title. Defaults to the robot's class name.

    Returns:
        The rendered HTML document as a string.

    Raises:
        AttributeError: If ``calibrator`` has not been solved yet.
    """
    eval_ = calibrator.evaluation_metrics
    calib_config = calibrator.calib_config
    n_samples = calib_config.get("NbSample", 0)
    param_names = calib_config.get("param_name", [])

    validation = None
    results_data = getattr(calibrator, "results_data", None) or {}
    if "validation_metrics" in results_data:
        validation = results_data["validation_metrics"]

    insights = _build_insights(eval_, n_samples, param_names, validation)

    redistributed = None
    try:
        redistributed = calibrator.redistribute_parameters()
    except Exception as e:
        logger.debug("redistribute_parameters unavailable for report: %s", e)

    provenance = getattr(calibrator, "_run_provenance", None)
    report_title = title or (
        f"{_run_title(provenance, type(calibrator).__name__)} "
        "— Calibration Quality Report"
    )
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M")

    from figaroh.tools.provenance import collect_splits
    from figaroh.tools.stages import stages_as_dicts

    stage_records = stages_as_dicts(calibrator)
    splits = collect_splits(calibrator)

    doc = f"""<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8">
<title>{_esc(report_title)}</title>
<style>{_STYLE}</style>
<script>{_SERIES_CHART_SCRIPT}</script>
</head>
<body>
<div class="page">
  <h1>{_esc(report_title)}</h1>
  <p class="subtitle">Generated {_esc(timestamp)}</p>

  <section>
    <h2>Provenance</h2>
    <div class="card">{_provenance_section(provenance)}</div>
  </section>

  <section>
    <h2>Summary</h2>
    <div class="card">{_summary_section(eval_, n_samples)}</div>
  </section>

  <section>
    <h2>Stages and data</h2>
    <div class="card">{_stages_section(stage_records, splits)}</div>
  </section>

  <section>
    <h2>Insights</h2>
    {_insights_section(insights)}
  </section>

  <section>
    <h2>Per-DOF residuals</h2>
    <div class="card">{_per_dof_section(eval_.get("per_dof_stats", {}))}</div>
  </section>

  <section>
    <h2>Validation</h2>
    <div class="card">{_validation_section(validation)}</div>
  </section>

  <section>
    <h2>Before / after</h2>
    <div class="card">{_series_panel_section(
        validation, "calibration", "series-panel"
    )}</div>
  </section>

  <section>
    <h2>Parameter uncertainty</h2>
    <div class="card">{_calibration_uncertainty_section(eval_, param_names)}</div>
  </section>

  <section>
    <h2>Redistributed standard parameters</h2>
    <div class="card">{_redistributed_section(redistributed)}</div>
  </section>

  <section>
    <h2>Parameter correlation</h2>
    <div class="card">{_correlation_section(
        eval_.get("correlated_pairs", [])
    )}</div>
  </section>

  <footer>Generated by figaroh.tools.report</footer>
</div>
</body>
</html>
"""

    if output_path is not None:
        with open(output_path, "w", encoding="utf-8") as f:
            f.write(doc)

    return doc
```

### identification_report

HTML diagnostic report generation for dynamic identification results.

Renders the same statistics as `BaseIdentification.print_quality_report()` (condition number, torque residuals, base-parameter uncertainty, held-out validation, optional physical-consistency / reconstruction status) as a self-contained, shareable HTML page — the identification analogue of `tools/report.py`'s calibration report.

Identification and calibration produce structurally different diagnostics (a one-shot linear QR solve vs. an iterative nonlinear fit with outlier removal), so this is a separate adapter rather than a forced re-use of `generate_calibration_report` — see `docs/decisions/roadmap-mujoco-sysid-inspired-features.md` (Feature 1b) for the comparison that motivated this split. Only the domain-independent HTML/CSS layer is shared, via `tools/_report_common.py`.

## `generate_identification_report(identifier, output_path=None, title=None)`

Render a self-contained HTML diagnostic report for an identification run.

Reuses the metrics already computed by `BaseIdentification.print_quality_report()` — no new computation is performed here, only presentation.

Parameters:

| Name          | Type            | Description                                                                             | Default    |
| ------------- | --------------- | --------------------------------------------------------------------------------------- | ---------- |
| `identifier`  |                 | A BaseIdentification instance after solve() has been called (i.e. result is populated). | *required* |
| `output_path` | `Optional[str]` | If given, the HTML is also written to this path.                                        | `None`     |
| `title`       | `Optional[str]` | Optional report title. Defaults to the robot's class name.                              | `None`     |

Returns:

| Type  | Description                             |
| ----- | --------------------------------------- |
| `str` | The rendered HTML document as a string. |

Raises:

| Type             | Description                            |
| ---------------- | -------------------------------------- |
| `AttributeError` | If identifier has not been solved yet. |

Source code in `src/figaroh/tools/identification_report.py`

```
def generate_identification_report(
    identifier, output_path: Optional[str] = None, title: Optional[str] = None
) -> str:
    """Render a self-contained HTML diagnostic report for an
    identification run.

    Reuses the metrics already computed by
    ``BaseIdentification.print_quality_report()`` — no new computation
    is performed here, only presentation.

    Args:
        identifier: A ``BaseIdentification`` instance after ``solve()``
            has been called (i.e. ``result`` is populated).
        output_path: If given, the HTML is also written to this path.
        title: Optional report title. Defaults to the robot's class name.

    Returns:
        The rendered HTML document as a string.

    Raises:
        AttributeError: If ``identifier`` has not been solved yet.
    """
    result = getattr(identifier, "result", None)
    if result is None:
        raise AttributeError("No identification results available. Run solve() first.")

    base_names = result.get("base parameters names", [])
    std_relative_raw = getattr(identifier, "std_relative", None)
    std_relative: List[float] = (
        list(std_relative_raw) if std_relative_raw is not None else []
    )
    phi_base = getattr(identifier, "phi_base", None)
    correlation = getattr(identifier, "correlation", float("nan"))
    validation = result.get("validation_metrics")

    std_dev_abs: List[float] = []
    if std_relative is not None and phi_base is not None:
        for i in range(len(std_relative)):
            sp = std_relative[i]
            val = phi_base[i] if i < len(phi_base) else float("nan")
            std_dev_abs.append(
                abs(sp / 100.0 * val) if not math.isnan(sp) else float("nan")
            )

    per_joint = None
    if hasattr(identifier, "_compute_per_joint_stats"):
        per_joint = identifier._compute_per_joint_stats()

    insights = _build_insights(result, std_relative, base_names, validation)

    provenance = getattr(identifier, "_run_provenance", None)
    report_title = title or (
        f"{_run_title(provenance, type(identifier).__name__)} "
        "— Identification Quality Report"
    )
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M")

    from figaroh.tools.provenance import collect_splits
    from figaroh.tools.stages import stages_as_dicts

    stage_records = stages_as_dicts(identifier)
    splits = collect_splits(identifier)

    doc = f"""<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8">
<title>{_esc(report_title)}</title>
<style>{_STYLE}</style>
<script>{_SERIES_CHART_SCRIPT}</script>
</head>
<body>
<div class="page">
  <h1>{_esc(report_title)}</h1>
  <p class="subtitle">Generated {_esc(timestamp)}</p>

  <section>
    <h2>Provenance</h2>
    <div class="card">{_provenance_section(provenance)}</div>
  </section>

  <section>
    <h2>Summary</h2>
    <div class="card">{_summary_section(result, correlation)}</div>
  </section>

  <section>
    <h2>Stages and data</h2>
    <div class="card">{_stages_section(stage_records, splits)}</div>
  </section>

  <section>
    <h2>Insights</h2>
    {_insights_section(insights)}
  </section>

  <section>
    <h2>Per-joint torque residuals</h2>
    <div class="card">{_per_joint_section(per_joint, result.get("selected"))}</div>
  </section>

  <section>
    <h2>Validation</h2>
    <div class="card">{_validation_section(validation)}</div>
  </section>

  <section>
    <h2>Before / after</h2>
    <div class="card">{_series_panel_section(
        validation, "identification", "series-panel"
    )}</div>
  </section>

  <section>
    <h2>Base-parameter uncertainty</h2>
    <div class="card">{_param_uncertainty_section(
        base_names,
        std_dev_abs,
        std_relative or [],
        list(phi_base) if phi_base is not None else None,
    )}</div>
  </section>

  <section>
    <h2>Physical consistency / reconstruction</h2>
    <div class="card">{_consistency_section(result)}</div>
  </section>

  <footer>Generated by figaroh.tools.identification_report</footer>
</div>
</body>
</html>
"""

    if output_path is not None:
        with open(output_path, "w", encoding="utf-8") as f:
            f.write(doc)

    return doc
```

### compare_report

Static, offline, two-run comparison page (Feature 6, Phase C).

Unlike `tools/report.py` / `tools/identification_report.py`, this module does not render a specific run's data at generation time. It emits a self-contained HTML *shell* that a user opens directly in a browser and then loads two `*_verification.json` files into (drag-and- drop or a file picker) — the two exports produced by `BaseCalibration.export_verification_report()` / `BaseIdentification.export_verification_report()` (Step 2, extended in Step 3 with the `series`/`compat` fields this page reads). Everything happens client-side in JavaScript: no backend, no network request, no change to either base class (see the roadmap's Step 5 "Files (modified): none required").

Per D3 (roadmap), a compatibility check (same domain, same DOF/joint names, same `decimate` setting for identification, comparable sample counts) runs before anything is rendered; on mismatch the page blocks the comparison with a clear message and offers an explicit "compare anyway" override rather than silently overlaying incompatible runs.

## `generate_compare_page(output_path=None, title=None)`

Render the static, self-contained two-run comparison page.

Unlike the calibration/identification report generators, this takes no run object — it emits a template that loads two `export_verification_report()` JSON files client-side (drag-and- drop or a file picker) and, after a mandatory compatibility check (D3), renders a per-metric diff table and an overlaid before/after series chart. No network request, no backend: the returned document is fully self-contained and can be opened directly from disk.

Parameters:

| Name          | Type            | Description                                                  | Default |
| ------------- | --------------- | ------------------------------------------------------------ | ------- |
| `output_path` | `Optional[str]` | If given, the HTML is also written to this path.             | `None`  |
| `title`       | `Optional[str]` | Optional page title. Defaults to a generic comparison title. | `None`  |

Returns:

| Type  | Description                             |
| ----- | --------------------------------------- |
| `str` | The rendered HTML document as a string. |

Source code in `src/figaroh/tools/compare_report.py`

```
def generate_compare_page(
    output_path: Optional[str] = None, title: Optional[str] = None
) -> str:
    """Render the static, self-contained two-run comparison page.

    Unlike the calibration/identification report generators, this takes
    no run object — it emits a template that loads two
    ``export_verification_report()`` JSON files client-side (drag-and-
    drop or a file picker) and, after a mandatory compatibility check
    (D3), renders a per-metric diff table and an overlaid before/after
    series chart. No network request, no backend: the returned document
    is fully self-contained and can be opened directly from disk.

    Args:
        output_path: If given, the HTML is also written to this path.
        title: Optional page title. Defaults to a generic comparison title.

    Returns:
        The rendered HTML document as a string.
    """
    report_title = title or "FIGAROH — Two-Run Comparison"
    timestamp = datetime.now().strftime("%Y-%m-%d %H:%M")

    doc = f"""<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8">
<title>{report_title}</title>
<style>{_STYLE}</style>
<style>{_COMPARE_STYLE}</style>
<script>{_COMPARE_CHART_SCRIPT}</script>
</head>
<body>
<div class="page">
  <h1>{report_title}</h1>
  <p class="subtitle">Generated {timestamp} &middot; static, offline —
    load two verification exports below. Nothing leaves this page.</p>

  <section>
    <h2>Load runs</h2>
    <div class="card compare-loader">
      <div class="drop-zone" id="zone-a" data-slot="a">
        <p class="drop-label">Run A</p>
        <input type="file" id="file-a" accept="application/json,.json">
        <p class="drop-filename" id="filename-a">No file loaded</p>
      </div>
      <div class="drop-zone" id="zone-b" data-slot="b">
        <p class="drop-label">Run B</p>
        <input type="file" id="file-b" accept="application/json,.json">
        <p class="drop-filename" id="filename-b">No file loaded</p>
      </div>
    </div>
  </section>

  <section id="compat-section" style="display:none;">
    <h2>Compatibility</h2>
    <div class="card">
      <div id="compat-status"></div>
      <div id="force-compare-row">
        <label>
          <input type="checkbox" id="force-compare">
          Compare anyway (results may be misleading)
        </label>
      </div>
    </div>
  </section>

  <section id="results-section" style="display:none;">
    <div class="forced-banner" id="forced-banner">
      These runs failed the compatibility check above — the comparison
      below was forced and may be misleading.
    </div>

    <h2>Metric comparison</h2>
    <div class="card">
      <table class="data">
        <thead>
          <tr>
            <th>Metric</th><th>Run A</th><th>Run B</th>
            <th>&Delta;</th><th>% change</th>
          </tr>
        </thead>
        <tbody id="diff-table-body"></tbody>
      </table>
    </div>

    <h2>Series overlay</h2>
    <div class="card" id="compare-series-card">
      <div class="series-controls">
        <label><input type="checkbox" id="compare-toggle-a" checked>
          Run A</label>
        <label><input type="checkbox" id="compare-toggle-b" checked>
          Run B</label>
        <label for="compare-select">Show:</label>
        <select id="compare-select"></select>
        <button type="button" id="compare-reset">Reset zoom</button>
      </div>
      <svg id="compare-svg" class="series-svg"></svg>
      <div class="series-legend" id="compare-legend"></div>
      <p class="series-hint">Scroll to zoom, hover to inspect.</p>
      <div id="compare-tooltip" class="series-tooltip"></div>
    </div>
  </section>

  <footer>Generated by figaroh.tools.compare_report — static, offline,
    no backend.</footer>
</div>
<script>{_COMPARE_DRIVER_SCRIPT}</script>
</body>
</html>
"""

    if output_path is not None:
        with open(output_path, "w", encoding="utf-8") as f:
            f.write(doc)

    return doc
```

### stages

`StageResult` records: which step (data, fit, validation, physical, export) ran, failed, fell back or did not run (#55).

Which step produced a result, and whether it ran, failed or fell back.

See docs/decisions/data-result-contract.md (#55). Identification and calibration record one :class:`StageResult` per step they run on `obj.stages`; results dictionaries carry them under `"stages"` and the run archive writes them to `stages.json`. A stage that did not run has no record, or an explicit `not_run` when that is worth stating.

## `record_stage(obj, stage, status, reason='', metrics=None, artifacts=None)`

Record `stage` on `obj.stages`, replacing an earlier record of it.

`metrics` maps a name to `(value, unit)` or to a bare value (unit `""`).

Source code in `src/figaroh/tools/stages.py`

```
def record_stage(
    obj,
    stage: str,
    status: str,
    reason: str = "",
    metrics: Optional[Dict[str, Any]] = None,
    artifacts: Optional[List[str]] = None,
) -> StageResult:
    """Record ``stage`` on ``obj.stages``, replacing an earlier record of it.

    ``metrics`` maps a name to ``(value, unit)`` or to a bare value (unit
    ``""``).
    """
    clean = {}
    for name, value in (metrics or {}).items():
        if isinstance(value, tuple):
            value, unit = value
        else:
            unit = ""
        clean[name] = {"value": value, "unit": unit}
    result = StageResult(stage, status, reason, clean, list(artifacts or []))
    stages = [s for s in getattr(obj, "stages", None) or [] if s.stage != stage]
    stages.append(result)
    stages.sort(key=lambda s: STAGES.index(s.stage))
    obj.stages = stages
    return result
```

## `drop_stage(obj, stage)`

Remove the record of `stage` from `obj.stages` (a no-op if none).

Source code in `src/figaroh/tools/stages.py`

```
def drop_stage(obj, stage: str) -> None:
    """Remove the record of ``stage`` from ``obj.stages`` (a no-op if none)."""
    obj.stages = [s for s in getattr(obj, "stages", None) or [] if s.stage != stage]
```

## `stages_as_dicts(obj)`

`obj.stages` as plain dicts (for results dictionaries and JSON).

Source code in `src/figaroh/tools/stages.py`

```
def stages_as_dicts(obj) -> List[Dict[str, Any]]:
    """``obj.stages`` as plain dicts (for results dictionaries and JSON)."""
    return [asdict(s) for s in getattr(obj, "stages", None) or []]
```

## `with_schema(obj, verdict)`

A verdict dict with `schema_version` and the run's stage records.

The records go under `stage_records`: the verdict's own `stages` is the per-stage status map and must not be replaced (#63).

Source code in `src/figaroh/tools/stages.py`

```
def with_schema(obj, verdict: Dict[str, Any]) -> Dict[str, Any]:
    """A verdict dict with ``schema_version`` and the run's stage records.

    The records go under ``stage_records``: the verdict's own ``stages`` is
    the per-stage status map and must not be replaced (#63).
    """
    return {
        "schema_version": SCHEMA_VERSION,
        **verdict,
        "stage_records": stages_as_dicts(obj),
    }
```

## `apply_to_verdict(verdict, obj, selected_stage='fit')`

Separate per-stage verdicts from the run's records (#63).

`verdict.stages` keeps its scoped entries (`numerical_execution`, `prediction`, `solver`) and gains `data`, `fit`, `validation`, `physical` and `export` from the records: `pass`, `fail`, `fallback` (validation on training data: not held-out evidence) or `not_run`; a stage with no record keeps `not_evaluated`. `data_provenance` mirrors `data`.

Source code in `src/figaroh/tools/stages.py`

```
def apply_to_verdict(verdict, obj, selected_stage: str = "fit") -> None:
    """Separate per-stage verdicts from the run's records (#63).

    ``verdict.stages`` keeps its scoped entries (``numerical_execution``,
    ``prediction``, ``solver``) and gains ``data``, ``fit``,
    ``validation``, ``physical`` and ``export`` from the records: ``pass``,
    ``fail``, ``fallback`` (validation on training data: not held-out
    evidence) or ``not_run``; a stage with no record keeps
    ``not_evaluated``. ``data_provenance`` mirrors ``data``.
    """
    from figaroh.tools.provenance import collect_splits

    records = {s.stage: s for s in getattr(obj, "stages", None) or []}
    for stage in STAGES:
        if stage in records:
            verdict.stages[stage] = _VERDICT_STATUS[records[stage].status]
        else:
            verdict.stages.setdefault(stage, "not_evaluated")
    verdict.stages["data_provenance"] = verdict.stages["data"]
    verdict.stage_records = stages_as_dicts(obj)
    fit = records.get("fit")
    verdict.selected_stage = (
        selected_stage if fit is not None and fit.status == "ok" else "none"
    )
    verdict.splits = collect_splits(obj)
```

## `stages_line(obj)`

One-line terminal summary, e.g. `data ok · fit ok · validation fallback (training data, not held-out)`.

Source code in `src/figaroh/tools/stages.py`

```
def stages_line(obj) -> str:
    """One-line terminal summary, e.g. ``data ok · fit ok · validation
    fallback (training data, not held-out)``."""
    parts = []
    for s in getattr(obj, "stages", None) or []:
        text = f"{s.stage} {s.status}"
        if s.stage == "validation" and s.status == "fallback":
            text += " (training data, not held-out)"
        elif s.status == "failed" and s.reason:
            text += f" ({s.reason})"
        parts.append(text)
    return " · ".join(parts) if parts else "no stage records"
```

### \_report_common

Shared HTML/CSS primitives and the `VerificationVerdict`/`ThresholdCheck` dataclasses used by both report generators.

Shared HTML/CSS primitives for the calibration and identification diagnostic reports (`tools/report.py` and `tools/identification_report.py`).

Calibration and identification produce structurally different diagnostics (iterative nonlinear fit with outlier removal vs. one-shot linear QR solve), so each gets its own report module — but both render as a self-contained HTML page with the same look, the same escaping rules, and the same confidence-tier vocabulary. This module holds only that shared, domain-independent layer.

## `ThresholdCheck(name, value, threshold, comparison, passed, status='pass', required=True, reason='')`

One metric checked against one threshold.

## `VerificationVerdict(passed, checks, metrics, insights=list(), metadata=dict(), series=dict(), compat=dict(), status='not_evaluated', scope='thresholds', stages=dict(), policy=dict(), stage_records=list(), selected_stage='fit', splits=dict())`

Machine-checkable pass/fail against a set of quality thresholds.

Produced by :func:`evaluate_thresholds`; `insights`/`metadata`/ `series`/`compat` are filled in by the caller (`BaseCalibration.verify()` / `BaseIdentification.verify()`) after construction, since those are domain-specific / provenance data rather than threshold arithmetic. `metadata` holds the full nested provenance record from :func:`figaroh.tools.provenance.collect_run_provenance` (run id, asset identity, nominal model, config, software, data, timestamps).

## `evaluate_thresholds(metrics, thresholds)`

Evaluate explicit limits; missing required evidence cannot pass.

Specs accept `required=False` for advisory checks. Missing metrics are recorded as not evaluated; computed nonfinite values fail. Empty policies have no acceptance evidence and return not evaluated, with passed=False.

Source code in `src/figaroh/tools/_report_common.py`

```
def evaluate_thresholds(metrics, thresholds):
    """Evaluate explicit limits; missing required evidence cannot pass.

    Specs accept ``required=False`` for advisory checks. Missing metrics are
    recorded as not evaluated; computed nonfinite values fail. Empty policies
    have no acceptance evidence and return not evaluated, with passed=False.
    """
    checks = []
    for name, spec in thresholds.items():
        threshold = float(spec["threshold"])
        comparison = spec["comparison"]
        if comparison not in ("min", "max"):
            raise ValueError(f"Unknown comparison {comparison!r} for {name!r}")
        if not math.isfinite(threshold):
            raise ValueError(f"Threshold for {name!r} must be finite")
        value = metrics.get(name)
        status, reason = "not_evaluated", "Required metric is unavailable"
        if value is not None:
            value = float(value)
            if not math.isfinite(value):
                status, reason = "fail", "Computed metric is nonfinite"
            else:
                passed = (
                    value <= threshold if comparison == "max" else value >= threshold
                )
                status, reason = ("pass" if passed else "fail"), ""
        checks.append(
            ThresholdCheck(
                name,
                value,
                threshold,
                comparison,
                status == "pass",
                status,
                bool(spec.get("required", True)),
                reason,
            )
        )
    status = _check_status(checks)
    return VerificationVerdict(
        status == "pass", checks, dict(metrics), status=status, policy=dict(thresholds)
    )
```

## `scoped_verification(metrics, thresholds, scope, arrays, independent, prediction_keys, solver_success=None, facts=None)`

Build a scoped verdict without claiming unrequested model stages.

Execution verifies finite, nonempty numerical outputs. Prediction also requires independent measurements and explicit error limits for every requested output. Neither scope certifies physical feasibility or export.

`facts` maps a name to `None` (not evaluated), a bool, or a `(bool, reason)` tuple whose reason is the message of a failing fact.

Source code in `src/figaroh/tools/_report_common.py`

```
def scoped_verification(
    metrics,
    thresholds,
    scope,
    arrays,
    independent,
    prediction_keys,
    solver_success=None,
    facts=None,
):
    """Build a scoped verdict without claiming unrequested model stages.

    Execution verifies finite, nonempty numerical outputs. Prediction also
    requires independent measurements and explicit error limits for every
    requested output. Neither scope certifies physical feasibility or export.

    ``facts`` maps a name to ``None`` (not evaluated), a bool, or a
    ``(bool, reason)`` tuple whose reason is the message of a failing fact.
    """
    import numpy as np

    if scope not in ("execution", "prediction"):
        raise ValueError("Verification scope must be 'execution' or 'prediction'")
    verdict = evaluate_thresholds(metrics, thresholds)
    guard = []
    for name, array in arrays.items():
        status, reason = "not_evaluated", "Numerical evidence is unavailable"
        if array is not None:
            values = np.asarray(array, dtype=float)
            valid = values.size > 0 and np.all(np.isfinite(values))
            status, reason = (
                ("pass", "")
                if valid
                else ("fail", "Empty or nonfinite numerical output")
            )
        guard.append(
            ThresholdCheck(
                name,
                None if status == "not_evaluated" else float(status == "pass"),
                1.0,
                "min",
                status == "pass",
                status,
                True,
                reason,
            )
        )
    for name, value in (facts or {}).items():
        # a fact is None (unknown), a bool, or ``(bool, reason)`` whose
        # reason is the message when it fails
        reason = "Numerical dimensions are missing or inconsistent"
        if isinstance(value, tuple):
            value, reason = value
        status = "not_evaluated" if value is None else ("pass" if value else "fail")
        guard.append(
            ThresholdCheck(
                name,
                None if status == "not_evaluated" else float(status == "pass"),
                1.0,
                "min",
                status == "pass",
                status,
                True,
                reason if status != "pass" else "",
            )
        )
    if solver_success is not None:
        status = "pass" if solver_success else "fail"
        guard.append(
            ThresholdCheck(
                "solver_success",
                float(solver_success),
                1.0,
                "min",
                bool(solver_success),
                status,
            )
        )
    numerical_status = _check_status(guard)
    verdict.checks = guard + verdict.checks
    prediction_status = "not_evaluated"
    if scope == "prediction":
        for name, satisfied, reason in [
            (
                "independent_validation",
                independent,
                "No independently loaded validation data",
            ),
            (
                "prediction_error_policy",
                bool(prediction_keys)
                and all(
                    k in thresholds
                    and thresholds[k].get("required", True)
                    and thresholds[k]["comparison"] == "max"
                    and float(thresholds[k]["threshold"]) >= 0
                    for k in prediction_keys
                ),
                "Specify nonnegative per-output maximum error limits before acceptance",
            ),
        ]:
            status = "pass" if satisfied else "not_evaluated"
            verdict.checks.append(
                ThresholdCheck(
                    name,
                    1.0 if satisfied else None,
                    1.0,
                    "min",
                    satisfied,
                    status,
                    True,
                    "" if satisfied else reason,
                )
            )
        prediction_status = _check_status(verdict.checks)
    verdict.status = _check_status(verdict.checks)
    verdict.passed = verdict.status == "pass"
    verdict.scope = scope
    verdict.stages = {
        "numerical_execution": numerical_status,
        "prediction": prediction_status,
        "data_provenance": "not_evaluated",
        "solver": (
            ("pass" if solver_success else "fail")
            if solver_success is not None
            else "not_evaluated"
        ),
        "physical": "not_evaluated",
        "export": "not_evaluated",
    }
    return verdict
```

## `verification_json_data(data)`

Keep invalid evidence readable in strict JSON; status retains failure.

Source code in `src/figaroh/tools/_report_common.py`

```
def verification_json_data(data):
    """Keep invalid evidence readable in strict JSON; status retains failure."""
    import numpy as np

    if isinstance(data, dict):
        return {k: verification_json_data(v) for k, v in data.items()}
    if isinstance(data, (list, tuple)):
        return [verification_json_data(v) for v in data]
    if isinstance(data, np.ndarray):
        return verification_json_data(data.tolist())
    if isinstance(data, np.generic):
        return verification_json_data(data.item())
    if isinstance(data, float) and not math.isfinite(data):
        return None
    return data
```

## `build_provenance_metadata(config_file_path, robot_name)`

Git commit, config file hash, timestamp, robot name — for a :class:`VerificationVerdict`'s `metadata` field.

Source code in `src/figaroh/tools/_report_common.py`

```
def build_provenance_metadata(
    config_file_path: Optional[str], robot_name: str
) -> Dict[str, str]:
    """Git commit, config file hash, timestamp, robot name — for a
    :class:`VerificationVerdict`'s ``metadata`` field."""
    return {
        "git_commit": _git_commit_hash(),
        "config_sha256": _config_file_sha256(config_file_path),
        "timestamp": datetime.now(timezone.utc).isoformat(),
        "robot_name": str(robot_name),
    }
```

### provenance

Run provenance metadata — git commit, config hash, timestamps, robot/asset identity — attached to every calibration/identification run.

Run provenance capture — the single source of truth for "what produced this fit": the nominal reference model, the exact config used, software versions, input data files, and (if configured) the physical asset identity. Consumed identically by the terminal report, the HTML report, the JSON verification verdict, and the run archive, so the four can never disagree about what produced a given result.

Reuses :func:`figaroh.tools._report_common._git_commit_hash` and :func:`figaroh.tools._report_common._config_file_sha256` rather than duplicating hashing logic.

## `collect_splits(obj)`

Which data trained and which validated this run (#63).

From the data-contract objects when the run used them (`trajectory` / `observations` and their validation counterparts: files with sha256, session, adapter); `"legacy input"` otherwise, whose files are listed under provenance `data`. `validation_source` comes from the run's `validation` stage: `held_out`, or `training_fallback` when no held-out data was available.

Source code in `src/figaroh/tools/provenance.py`

```
def collect_splits(obj: Any) -> Dict[str, Any]:
    """Which data trained and which validated this run (#63).

    From the data-contract objects when the run used them
    (``trajectory`` / ``observations`` and their validation counterparts:
    files with sha256, session, adapter); ``"legacy input"`` otherwise,
    whose files are listed under provenance ``data``. ``validation_source``
    comes from the run's ``validation`` stage: ``held_out``, or
    ``training_fallback`` when no held-out data was available.
    """

    def describe(data):
        if data is None:
            return None
        source = data.source
        return {
            "files": dict(source.files),
            "session": source.session.id if source.session else None,
            "adapter": source.adapter,
            "samples": int(data.n_samples),
            "masked_samples": int((~data.mask).sum()),
        }

    training = getattr(obj, "trajectory", None) or getattr(obj, "observations", None)
    validation = getattr(obj, "_val_trajectory", None) or getattr(
        obj, "_val_observations", None
    )
    status = {s.stage: s.status for s in getattr(obj, "stages", None) or []}
    source = {"ok": "held_out", "fallback": "training_fallback"}.get(
        status.get("validation"), "none"
    )
    return {
        "training": describe(training) or "legacy input (see provenance data)",
        "validation": describe(validation),
        "validation_source": source,
    }
```

## `collect_run_provenance(obj, task)`

Build the full provenance record for one V&V run.

Parameters:

| Name   | Type  | Description                                                                                                                                                                                                                                                                                                             | Default    |
| ------ | ----- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `obj`  | `Any` | A BaseIdentification or BaseCalibration instance (or duck-typed equivalent) after its config has been loaded — i.e. safe to call any time after load_param(), though it is normally captured once in \_store_results/ \_store_optimization_results right after solve() finishes computing, so run_finished is accurate. | *required* |
| `task` | `str` | "identification" or "calibration" — selects which curated config-key allowlist to surface under config.values.                                                                                                                                                                                                          | *required* |

Returns:

| Type             | Description                                                     |
| ---------------- | --------------------------------------------------------------- |
| `Dict[str, Any]` | A JSON-serializable nested dict (see module docstring for the   |
| `Dict[str, Any]` | four consumers). Never raises: every sub-lookup is best-effort. |

Source code in `src/figaroh/tools/provenance.py`

```
def collect_run_provenance(obj: Any, task: str) -> Dict[str, Any]:
    """Build the full provenance record for one V&V run.

    Args:
        obj: A ``BaseIdentification`` or ``BaseCalibration`` instance
            (or duck-typed equivalent) after its config has been loaded
            — i.e. safe to call any time after ``load_param()``, though
            it is normally captured once in ``_store_results``/
            ``_store_optimization_results`` right after ``solve()``
            finishes computing, so ``run_finished`` is accurate.
        task: ``"identification"`` or ``"calibration"`` — selects which
            curated config-key allowlist to surface under
            ``config.values``.

    Returns:
        A JSON-serializable nested dict (see module docstring for the
        four consumers). Never raises: every sub-lookup is best-effort.
    """
    config = (
        getattr(obj, "identif_config", None) or getattr(obj, "calib_config", None) or {}
    )
    config_keys = (
        _IDENTIFICATION_CONFIG_KEYS
        if task == "identification"
        else _CALIBRATION_CONFIG_KEYS
    )

    asset = _asset_identity(config)
    now = datetime.now(timezone.utc).isoformat()
    git_commit = _git_commit_hash()
    run_started = getattr(obj, "_run_started_at", None) or now
    run_finished = getattr(obj, "_run_finished_at", None) or now

    run_id = "_".join(
        [
            asset["asset_id"].replace(" ", "-"),
            task,
            datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ"),
            git_commit[:8] if git_commit != "unknown" else "nogit",
        ]
    )

    config_path = getattr(obj, "_config_file_path", None)

    return {
        "run_id": run_id,
        "task": task,
        "asset": asset,
        "model": _model_identity(obj),
        "config": {
            "path": config_path or "unavailable",
            "sha256": _config_file_sha256(config_path),
            "values": _config_values(config, config_keys),
        },
        "software": {
            **_software_versions(),
            # working directory's repository (an example run: examples)
            "git_commit": git_commit,
            "git_dirty": _git_dirty(),
            # the figaroh package's own checkout, the other half of the pair
            "figaroh_revision": _package_revision(),
            "hostname": socket.gethostname(),
        },
        "data": _data_files_provenance(config),
        "timestamps": {
            "run_started": run_started,
            "run_finished": run_finished,
            "report_generated": now,
        },
    }
```

### run_archive

Writes each run to a timestamped `results/runs/<robot>/<task>/<timestamp>/` directory (config snapshot, provenance JSON, report) instead of overwriting a single `results/` path.

Longitudinal run archive — a fleet's V&V history.

`export_html_report()`/`export_verification_report()` write to fixed filenames that get overwritten by the next run, so a robot calibrated monthly only ever has its most recent report on disk. :func:`archive_run` instead writes each run to its own immutable, timestamped directory under `results/runs/{asset_id}/{task}/{timestamp}_{git_short}/` and appends a one-line summary to `results/runs/index.jsonl` — a zero-dependency, git-diffable, `jq`-able index of every run ever performed, per asset.

Call after :meth:`~figaroh.identification.base_identification.BaseIdentification.solve` (and, typically, after `export_html_report()`/`export_verification_report()` so their output gets copied in alongside the raw provenance/parameters).

## `compute_run_dir(obj, root='results/runs')`

Compute this run's dedicated directory path from provenance.

Deterministic for a given `obj._run_provenance` — safe to call more than once (e.g. once for the HTML report's output_path, again for the verdict's) and get back the identical path, since the timestamp is read from the already-fixed provenance record, never re-derived from wall clock.

Parameters:

| Name   | Type  | Description                                                                                                           | Default          |
| ------ | ----- | --------------------------------------------------------------------------------------------------------------------- | ---------------- |
| `obj`  | `Any` | A BaseIdentification/BaseCalibration instance after :meth:solve has been called (i.e. \_run_provenance is populated). | *required*       |
| `root` | `str` | Archive root directory (created if missing).                                                                          | `'results/runs'` |

Returns:

| Name   | Type   | Description                                     |
| ------ | ------ | ----------------------------------------------- |
| `Path` | `Path` | The directory for this run, created if missing. |

Raises:

| Type             | Description                   |
| ---------------- | ----------------------------- |
| `AttributeError` | If called before :meth:solve. |

Source code in `src/figaroh/tools/run_archive.py`

```
def compute_run_dir(obj: Any, root: str = "results/runs") -> Path:
    """Compute this run's dedicated directory path from provenance.

    Deterministic for a given ``obj._run_provenance`` — safe to call more
    than once (e.g. once for the HTML report's output_path, again for the
    verdict's) and get back the identical path, since the timestamp is
    read from the already-fixed provenance record, never re-derived from
    wall clock.

    Args:
        obj: A ``BaseIdentification``/``BaseCalibration`` instance after
            :meth:`solve` has been called (i.e. ``_run_provenance`` is
            populated).
        root: Archive root directory (created if missing).

    Returns:
        Path: The directory for this run, created if missing.

    Raises:
        AttributeError: If called before :meth:`solve`.
    """
    provenance = getattr(obj, "_run_provenance", None)
    if provenance is None:
        raise AttributeError("No run provenance available. Run solve() first.")

    task = provenance.get("task", "unknown")
    asset_id = provenance.get("asset", {}).get("asset_id", "unknown")

    run_finished = provenance.get("timestamps", {}).get("run_finished")
    try:
        dt = datetime.fromisoformat(run_finished)
    except (TypeError, ValueError):
        dt = datetime.now(timezone.utc)
    ts_compact = dt.strftime("%Y%m%dT%H%M%SZ")
    git_short = provenance.get("software", {}).get("git_commit", "nogit")[:8]

    root_path = Path(root)
    run_dir = (
        root_path
        / _safe_path_component(asset_id)
        / _safe_path_component(task)
        / f"{ts_compact}_{_safe_path_component(git_short)}"
    )
    run_dir.mkdir(parents=True, exist_ok=True)
    return run_dir
```

## `archive_run(obj, run_dir)`

Finalize archiving into an already-computed run_dir.

Writes provenance.json / config.snapshot.yaml / parameters.csv, and appends the index.jsonl entry (reading run_dir/verdict.json for pass/metrics if the caller already wrote one there).

Parameters:

| Name      | Type   | Description                                                                                                                                                                                              | Default    |
| --------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `obj`     | `Any`  | A BaseIdentification/BaseCalibration instance after :meth:solve has been called (i.e. \_run_provenance is populated).                                                                                    | *required* |
| `run_dir` | `Path` | The directory for this run (typically obtained via :func:compute_run_dir). Expected to already exist; the caller should have written report.html and/or verdict.json there before calling this function. | *required* |

Returns:

| Name  | Type  | Description                                                             |
| ----- | ----- | ----------------------------------------------------------------------- |
| `str` | `str` | The path to this run's archive directory (echoed back for convenience). |

Raises:

| Type             | Description                   |
| ---------------- | ----------------------------- |
| `AttributeError` | If called before :meth:solve. |

Source code in `src/figaroh/tools/run_archive.py`

```
def archive_run(obj: Any, run_dir: Path) -> str:
    """Finalize archiving into an already-computed run_dir.

    Writes provenance.json / config.snapshot.yaml / parameters.csv, and
    appends the index.jsonl entry (reading run_dir/verdict.json for
    pass/metrics if the caller already wrote one there).

    Args:
        obj: A ``BaseIdentification``/``BaseCalibration`` instance after
            :meth:`solve` has been called (i.e. ``_run_provenance`` is
            populated).
        run_dir: The directory for this run (typically obtained via
            :func:`compute_run_dir`). Expected to already exist; the
            caller should have written ``report.html`` and/or
            ``verdict.json`` there before calling this function.

    Returns:
        str: The path to this run's archive directory (echoed back for
            convenience).

    Raises:
        AttributeError: If called before :meth:`solve`.
    """
    provenance = getattr(obj, "_run_provenance", None)
    if provenance is None:
        raise AttributeError("No run provenance available. Run solve() first.")

    with open(run_dir / "provenance.json", "w") as f:
        json.dump(provenance, f, indent=2, default=str)

    config = getattr(obj, "identif_config", None) or getattr(obj, "calib_config", None)
    if config:
        with open(run_dir / "config.snapshot.yaml", "w") as f:
            yaml.dump(_yaml_safe(config), f, default_flow_style=False, sort_keys=True)

    stages = getattr(obj, "stages", None)
    if stages:
        from figaroh.tools.stages import SCHEMA_VERSION, stages_as_dicts

        with open(run_dir / "stages.json", "w") as f:
            json.dump(
                {"schema_version": SCHEMA_VERSION, "stages": stages_as_dicts(obj)},
                f,
                indent=2,
                default=str,
            )

    param_names, param_values = _extract_parameters(obj)
    if param_names:
        with open(run_dir / "parameters.csv", "w", newline="") as f:
            writer = csv.writer(f)
            writer.writerow(["parameter", "value"])
            writer.writerows(zip(param_names, param_values))

    result = getattr(obj, "result", None)
    if isinstance(result, dict) and isinstance(result.get("selected"), dict):
        # the base-parameter fit stays available beside the selected estimate
        with open(run_dir / "fit_parameters.csv", "w", newline="") as f:
            writer = csv.writer(f)
            writer.writerow(["parameter", "value"])
            writer.writerows(
                zip(
                    result.get("base parameters names", []),
                    result.get("base parameters values", []),
                )
            )

    # Determine the root for index (run_dir is .../root/asset/task/timestamp/)
    root_path = run_dir.parent.parent.parent
    _append_index(root_path, provenance, run_dir)
    logger.info(f"Run archived to {run_dir}")
    return str(run_dir)
```

## Linear Solver

Multivariate linear solver for robot parameter identification.

This module provides advanced linear regression solvers optimized for overdetermined systems (thin, tall matrices) with support for:

- Multiple regularization methods (Ridge, Lasso, Elastic Net, Tikhonov)
- Linear equality and inequality constraints
- Robust regression methods
- Physical parameter bounds
- Iterative refinement for improved accuracy

## `LinearSolver(method='lstsq', regularization=None, alpha=0.0, l1_ratio=0.5, weights=None, constraints=None, bounds=None, max_iter=1000, tol=1e-06, verbose=False)`

Advanced linear solver for overdetermined systems: Ax = b

Optimized for robot identification where A is typically:

- Dense (not sparse)
- Large (many samples)
- Thin (more rows than columns, overdetermined)

Attributes:

| Name             | Type    | Description                    |
| ---------------- | ------- | ------------------------------ |
| `method`         | `str`   | Solving method to use          |
| `regularization` | `str`   | Type of regularization         |
| `alpha`          | `float` | Regularization strength        |
| `constraints`    | `dict`  | Linear constraints on solution |
| `bounds`         | `tuple` | Box constraints on variables   |
| `solver_info`    | `dict`  | Information about the solution |

Initialize linear solver.

Parameters:

| Name             | Type      | Description                                                                                                                                                                                                               | Default   |
| ---------------- | --------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------- |
| `method`         | `str`     | Solving method (see METHODS)                                                                                                                                                                                              | `'lstsq'` |
| `regularization` | `str`     | 'l1', 'l2', 'elastic_net', or None                                                                                                                                                                                        | `None`    |
| `alpha`          | `float`   | Regularization strength (>=0)                                                                                                                                                                                             | `0.0`     |
| `l1_ratio`       | `float`   | Elastic net mixing (0=Ridge, 1=Lasso)                                                                                                                                                                                     | `0.5`     |
| `weights`        | `ndarray` | Sample weights for weighted least squares                                                                                                                                                                                 | `None`    |
| `constraints`    | `dict`    | Linear constraints: - 'A_eq': Equality constraint matrix (A_eq @ x = b_eq) - 'b_eq': Equality constraint vector - 'A_ineq': Inequality constraint matrix (A_ineq @ x \<= b_ineq) - 'b_ineq': Inequality constraint vector | `None`    |
| `bounds`         | `tuple`   | Box constraints (lower, upper) for each variable                                                                                                                                                                          | `None`    |
| `max_iter`       | `int`     | Maximum iterations for iterative methods                                                                                                                                                                                  | `1000`    |
| `tol`            | `float`   | Convergence tolerance                                                                                                                                                                                                     | `1e-06`   |
| `verbose`        | `bool`    | Print solver information                                                                                                                                                                                                  | `False`   |

Source code in `src/figaroh/tools/solver.py`

```
def __init__(
    self,
    method="lstsq",
    regularization=None,
    alpha=0.0,
    l1_ratio=0.5,
    weights=None,
    constraints=None,
    bounds=None,
    max_iter=1000,
    tol=1e-6,
    verbose=False,
):
    """
    Initialize linear solver.

    Args:
        method (str): Solving method (see METHODS)
        regularization (str): 'l1', 'l2', 'elastic_net', or None
        alpha (float): Regularization strength (>=0)
        l1_ratio (float): Elastic net mixing (0=Ridge, 1=Lasso)
        weights (ndarray): Sample weights for weighted least squares
        constraints (dict): Linear constraints:
            - 'A_eq': Equality constraint matrix (A_eq @ x = b_eq)
            - 'b_eq': Equality constraint vector
            - 'A_ineq': Inequality constraint matrix (A_ineq @ x <= b_ineq)
            - 'b_ineq': Inequality constraint vector
        bounds (tuple): Box constraints (lower, upper) for each variable
        max_iter (int): Maximum iterations for iterative methods
        tol (float): Convergence tolerance
        verbose (bool): Print solver information
    """
    if method not in self.METHODS:
        raise ValueError(f"Method must be one of {self.METHODS}")

    self.method = method
    self.regularization = regularization
    self.alpha = alpha
    self.l1_ratio = l1_ratio
    self.weights = weights
    self.constraints = constraints or {}
    self.bounds = bounds
    self.max_iter = max_iter
    self.tol = tol
    self.verbose = verbose
    self.solver_info = {}
```

### `solve(A, b)`

Solve the linear system Ax = b.

Parameters:

| Name | Type      | Description                           | Default    |
| ---- | --------- | ------------------------------------- | ---------- |
| `A`  | `ndarray` | Design matrix (n_samples, n_features) | *required* |
| `b`  | `ndarray` | Target vector (n_samples,)            | *required* |

Returns:

| Name      | Type | Description                     |
| --------- | ---- | ------------------------------- |
| `ndarray` |      | Solution vector x (n_features,) |

Raises:

| Type          | Description                |
| ------------- | -------------------------- |
| `ValueError`  | If inputs are incompatible |
| `LinAlgError` | If solution fails          |

Source code in `src/figaroh/tools/solver.py`

```
def solve(self, A, b):
    """
    Solve the linear system Ax = b.

    Args:
        A (ndarray): Design matrix (n_samples, n_features)
        b (ndarray): Target vector (n_samples,)

    Returns:
        ndarray: Solution vector x (n_features,)

    Raises:
        ValueError: If inputs are incompatible
        np.linalg.LinAlgError: If solution fails
    """
    # Validate inputs
    A, b = self._validate_inputs(A, b)

    # Select and execute solving method
    if self.method == "lstsq":
        x = self._solve_lstsq(A, b)
    elif self.method == "qr":
        x = self._solve_qr(A, b)
    elif self.method == "svd":
        x = self._solve_svd(A, b)
    elif self.method == "ridge":
        x = self._solve_ridge(A, b)
    elif self.method == "lasso":
        x = self._solve_lasso(A, b)
    elif self.method == "elastic_net":
        x = self._solve_elastic_net(A, b)
    elif self.method == "tikhonov":
        x = self._solve_tikhonov(A, b)
    elif self.method == "constrained":
        x = self._solve_constrained(A, b)
    elif self.method == "robust":
        x = self._solve_robust(A, b)
    elif self.method == "weighted":
        x = self._solve_weighted(A, b)
    else:
        raise ValueError(f"Method {self.method} not implemented")

    # Compute solution quality metrics
    self._compute_solution_quality(A, b, x)

    if self.verbose:
        self._print_solution_info()

    return x
```

## `solve_linear_system(A, b, method='lstsq', **kwargs)`

Convenience function to solve linear system Ax = b.

Parameters:

| Name       | Type      | Description                           | Default    |
| ---------- | --------- | ------------------------------------- | ---------- |
| `A`        | `ndarray` | Design matrix (n_samples, n_features) | *required* |
| `b`        | `ndarray` | Target vector (n_samples,)            | *required* |
| `method`   | `str`     | Solving method                        | `'lstsq'`  |
| `**kwargs` |           | Additional arguments for LinearSolver | `{}`       |

Returns:

| Name    | Type | Description                    |
| ------- | ---- | ------------------------------ |
| `tuple` |      | (solution x, solver_info dict) |

Example

> > > A = np.random.randn(1000, 50) # 1000 samples, 50 features b = np.random.randn(1000) x, info = solve_linear_system(A, b, method='ridge', alpha=0.1)

Source code in `src/figaroh/tools/solver.py`

```
def solve_linear_system(A, b, method="lstsq", **kwargs):
    """
    Convenience function to solve linear system Ax = b.

    Args:
        A (ndarray): Design matrix (n_samples, n_features)
        b (ndarray): Target vector (n_samples,)
        method (str): Solving method
        **kwargs: Additional arguments for LinearSolver

    Returns:
        tuple: (solution x, solver_info dict)

    Example:
        >>> A = np.random.randn(1000, 50)  # 1000 samples, 50 features
        >>> b = np.random.randn(1000)
        >>> x, info = solve_linear_system(A, b, method='ridge', alpha=0.1)
    """
    solver = LinearSolver(method=method, **kwargs)
    x = solver.solve(A, b)
    return x, solver.solver_info
```

The linear solver provides comprehensive solving methods for robot parameter identification:

- **Basic methods**: lstsq, QR decomposition, SVD
- **Regularized methods**: Ridge (L2), Lasso (L1), Elastic Net, Tikhonov
- **Advanced methods**: Constrained optimization, robust regression, weighted least squares
- **Constraint support**: Box constraints, linear equality/inequality constraints
- **Quality metrics**: RMSE, R², condition number, residual analysis

## Robot Management

## `Robot(robot_urdf, package_dirs, isFext=False, freeflyer_ori=None, freeflyer_limits=None)`

Bases: `RobotWrapper`

Enhanced Robot class with advanced free-flyer support and visualization utilities.

This class extends RobotWrapper with:

- Sophisticated free-flyer configuration and limits
- Integrated visualization support
- Convenient API for common robot operations
- Enhanced error handling and validation

Initialize enhanced robot model from URDF.

Parameters:

| Name               | Type                            | Description                                                | Default    |
| ------------------ | ------------------------------- | ---------------------------------------------------------- | ---------- |
| `robot_urdf`       | `str`                           | Path to URDF file                                          | *required* |
| `package_dirs`     | `str`                           | Package directories for mesh files                         | *required* |
| `isFext`           | `bool`                          | Whether to add floating base joint                         | `False`    |
| `freeflyer_ori`    | `Optional[ndarray]`             | Optional 3x3 rotation matrix for floating base orientation | `None`     |
| `freeflyer_limits` | `Optional[Tuple[float, float]]` | Optional custom limits for free-flyer (default: (-1, 1))   | `None`     |

Source code in `src/figaroh/tools/robot.py`

```
def __init__(
    self,
    robot_urdf: str,
    package_dirs: str,
    isFext: bool = False,
    freeflyer_ori: Optional[np.ndarray] = None,
    freeflyer_limits: Optional[Tuple[float, float]] = None,
):
    """Initialize enhanced robot model from URDF.

    Args:
        robot_urdf: Path to URDF file
        package_dirs: Package directories for mesh files
        isFext: Whether to add floating base joint
        freeflyer_ori: Optional 3x3 rotation matrix for floating base orientation
        freeflyer_limits: Optional custom limits for free-flyer (default: (-1, 1))
    """
    self.isFext = isFext
    self.robot_urdf = robot_urdf
    self._freeflyer_limits = freeflyer_limits or self.DEFAULT_FREEFLYER_LIMITS

    # Initialize robot model
    root_joint = pin.JointModelFreeFlyer() if isFext else None
    self.initFromURDF(robot_urdf, package_dirs=package_dirs, root_joint=root_joint)

    # Configure free-flyer with advanced options
    if isFext:
        self._configure_freeflyer(freeflyer_ori)

    # Set aliases for backward compatibility
    self.geom_model = self.collision_model

    # Lazy backend for backend-aware computations
    self._backend = None
```

### `num_joints`

Number of actuated joints (excluding universe and free-flyer).

### `actuated_joint_names`

Names of actuated joints only.

### `freeflyer_limits`

Current free-flyer position limits.

### `backend`

Lazily-created DynamicsBackend wrapping this robot's model.

Returns a PinocchioBackend that shares the same pin.Model and pin.Data as this Robot instance — no URDF re-loading, no model duplication. Used by RegressorBuilder and other tools to route dynamics computation through the backend abstraction.

Returns:

| Type | Description               |
| ---- | ------------------------- |
|      | PinocchioBackend instance |

### `update_freeflyer_limits(limits)`

Update free-flyer limits dynamically.

Source code in `src/figaroh/tools/robot.py`

```
def update_freeflyer_limits(self, limits: Tuple[float, float]) -> None:
    """Update free-flyer limits dynamically."""
    if not self.isFext:
        raise ValueError("Robot does not have a free-flyer joint")

    self._freeflyer_limits = limits
    self._set_freeflyer_limits()
```

### `display_q0(visualizer_type=None, q=None)`

Enhanced visualization with configuration options.

Source code in `src/figaroh/tools/robot.py`

```
def display_q0(
    self, visualizer_type: Optional[str] = None, q: Optional[np.ndarray] = None
) -> None:
    """Enhanced visualization with configuration options."""
    visualizer_class = self._get_visualizer_class(visualizer_type)

    if visualizer_class:
        self.setVisualizer(visualizer_class())
        self.initViewer()
        self.loadViewerModel(self.robot_urdf)

        # Display specified configuration or initial configuration
        config = q if q is not None else self.q0
        self.display(config)
```

### `get_configuration_bounds()`

Get configuration space bounds with proper handling of quaternions.

Source code in `src/figaroh/tools/robot.py`

```
def get_configuration_bounds(self) -> Tuple[np.ndarray, np.ndarray]:
    """Get configuration space bounds with proper handling of quaternions."""
    lb, ub = (
        self.model.lowerPositionLimit.copy(),
        self.model.upperPositionLimit.copy(),
    )

    if self.isFext:
        # Ensure quaternion bounds are properly set
        lb[3:7] = -1.0
        ub[3:7] = 1.0

    return lb, ub
```

### `validate_configuration(q)`

Validate if configuration is within bounds and constraints.

Source code in `src/figaroh/tools/robot.py`

```
def validate_configuration(self, q: np.ndarray) -> bool:
    """Validate if configuration is within bounds and constraints."""
    if len(q) != self.model.nq:
        return False

    lb, ub = self.get_configuration_bounds()

    # Check bounds
    if not np.all((q >= lb) & (q <= ub)):
        return False

    # Check quaternion normalization for free-flyer
    if self.isFext:
        quat = q[3:7]
        if not np.isclose(np.linalg.norm(quat), 1.0, atol=1e-6):
            return False

    return True
```

### `__repr__()`

Enhanced string representation.

Source code in `src/figaroh/tools/robot.py`

```
def __repr__(self) -> str:
    """Enhanced string representation."""
    return (
        f"Robot(urdf='{self.robot_urdf}', "
        f"actuated_joints={self.num_joints}, "
        f"total_dofs={self.model.nv}, "
        f"free_flyer={self.isFext})"
    )
```

## `create_robot(robot_urdf, package_dirs=None, loader='figaroh', enhanced_features=True, **kwargs)`

Factory function to create robots with optional enhanced features.

Parameters:

| Name                | Type            | Description                                         | Default     |
| ------------------- | --------------- | --------------------------------------------------- | ----------- |
| `robot_urdf`        | `str`           | Robot identifier or URDF path                       | *required*  |
| `package_dirs`      | `Optional[str]` | Package directories                                 | `None`      |
| `loader`            | `str`           | Loader type from load_robot.py                      | `'figaroh'` |
| `enhanced_features` | `bool`          | If True, returns Robot class with enhanced features | `True`      |
| `**kwargs`          |                 | Additional arguments for loaders                    | `{}`        |

Returns:

| Type                         | Description                                                |
| ---------------------------- | ---------------------------------------------------------- |
| `Union[Robot, RobotWrapper]` | Robot instance (if enhanced_features=True) or RobotWrapper |

Source code in `src/figaroh/tools/robot.py`

```
def create_robot(
    robot_urdf: str,
    package_dirs: Optional[str] = None,
    loader: str = "figaroh",
    enhanced_features: bool = True,
    **kwargs,
) -> Union[Robot, RobotWrapper]:
    """Factory function to create robots with optional enhanced features.

    Args:
        robot_urdf: Robot identifier or URDF path
        package_dirs: Package directories
        loader: Loader type from load_robot.py
        enhanced_features: If True, returns Robot class with enhanced features
        **kwargs: Additional arguments for loaders

    Returns:
        Robot instance (if enhanced_features=True) or RobotWrapper
    """
    if enhanced_features and loader == "figaroh":
        # Use enhanced Robot class
        from .load_robot import _prepare_package_dirs, _validate_urdf_exists

        isFext = kwargs.get("isFext", False)
        robot_pkg = kwargs.get("robot_pkg")

        package_dirs = _prepare_package_dirs(robot_urdf, package_dirs, robot_pkg)
        _validate_urdf_exists(robot_urdf)

        return Robot(
            robot_urdf,
            package_dirs=package_dirs,
            isFext=isFext,
            freeflyer_ori=kwargs.get("freeflyer_ori"),
            freeflyer_limits=kwargs.get("freeflyer_limits"),
        )
    else:
        # Use load_robot.py for other loaders
        from .load_robot import load_robot

        return load_robot(robot_urdf, package_dirs, loader=loader, **kwargs)
```

## `load_robot(robot_urdf, package_dirs=None, isFext=False, load_by_urdf=True, robot_pkg=None, loader='figaroh', **kwargs)`

Load robot model from various sources with multiple loader options.

Parameters:

| Name           | Type            | Description                                              | Default     |
| -------------- | --------------- | -------------------------------------------------------- | ----------- |
| `robot_urdf`   | `str`           | Path to URDF file or robot name for robot_description    | *required*  |
| `package_dirs` | `Optional[str]` | Package directories for mesh files                       | `None`      |
| `isFext`       | `bool`          | Whether to add floating base joint                       | `False`     |
| `load_by_urdf` | `bool`          | Whether to load from URDF file (vs ROS param server)     | `True`      |
| `robot_pkg`    | `Optional[str]` | Name of robot package for path resolution                | `None`      |
| `loader`       | `str`           | Loader type - "figaroh", "robot_description", "yourdfpy" | `'figaroh'` |
| `**kwargs`     |                 | Additional arguments passed to the specific loader       | `{}`        |

Returns:

| Type                              | Description                                                    |
| --------------------------------- | -------------------------------------------------------------- |
| `Union[Robot, RobotWrapper, Any]` | Robot object based on loader type:                             |
| `Union[Robot, RobotWrapper, Any]` | "figaroh": Robot class instance (default, backward compatible) |
| `Union[Robot, RobotWrapper, Any]` | "robot_description": RobotWrapper from pinocchio               |
| `Union[Robot, RobotWrapper, Any]` | "yourdfpy": URDF object from yourdfpy (suitable for viser)     |

Raises:

| Type                | Description                              |
| ------------------- | ---------------------------------------- |
| `FileNotFoundError` | If URDF file not found                   |
| `ImportError`       | If required packages not available       |
| `ValueError`        | If the specified loader is not supported |

For loading by URDF, robot_urdf and package_dirs can be different.

1/ If package_dirs is not provided directly, robot_pkg is used to look up the package directory. 2/ If no mesh files, package_dirs and robot_pkg are not used. 3/ If load_by_urdf is False, the robot is loaded from the ROS parameter server. 4/ For robot_description loader, robot_urdf should be the robot name. 5/ For yourdfpy loader, returns URDF object suitable for viser visualization.

Source code in `src/figaroh/tools/load_robot.py`

```
def load_robot(
    robot_urdf: str,
    package_dirs: Optional[str] = None,
    isFext: bool = False,
    load_by_urdf: bool = True,
    robot_pkg: Optional[str] = None,
    loader: str = "figaroh",
    **kwargs,
) -> Union[Robot, RobotWrapper, Any]:
    """Load robot model from various sources with multiple loader options.

    Args:
        robot_urdf: Path to URDF file or robot name for robot_description
        package_dirs: Package directories for mesh files
        isFext: Whether to add floating base joint
        load_by_urdf: Whether to load from URDF file (vs ROS param server)
        robot_pkg: Name of robot package for path resolution
        loader: Loader type - "figaroh", "robot_description", "yourdfpy"
        **kwargs: Additional arguments passed to the specific loader

    Returns:
        Robot object based on loader type:
        - "figaroh": Robot class instance (default, backward compatible)
        - "robot_description": RobotWrapper from pinocchio
        - "yourdfpy": URDF object from yourdfpy (suitable for viser)

    Raises:
        FileNotFoundError: If URDF file not found
        ImportError: If required packages not available
        ValueError: If the specified loader is not supported

    Note: For loading by URDF, robot_urdf and package_dirs can be different.
          1/ If package_dirs is not provided directly, robot_pkg is used to
          look up the package directory.
          2/ If no mesh files, package_dirs and robot_pkg are not used.
          3/ If load_by_urdf is False, the robot is loaded from the ROS
          parameter server.
          4/ For robot_description loader, robot_urdf should be the robot name.
          5/ For yourdfpy loader, returns URDF object suitable for viser visualization.
    """
    # Handle different loaders
    if loader == "robot_description":
        return _load_robot_description(robot_urdf, isFext, **kwargs)
    elif loader == "yourdfpy":
        return _load_yourdfpy(robot_urdf, package_dirs, robot_pkg, **kwargs)
    elif loader == "figaroh":
        # Original figaroh implementation (backward compatible)
        return _load_figaroh_original(
            robot_urdf, package_dirs, isFext, load_by_urdf, robot_pkg
        )
    else:
        raise ValueError(
            f"Unsupported loader: {loader}. Supported loaders: figaroh, robot_description, yourdfpy"
        )
```

## `get_available_loaders()`

Get information about available robot loaders.

Returns:

| Name   | Type   | Description                                        |
| ------ | ------ | -------------------------------------------------- |
| `dict` | `dict` | Information about each loader and its availability |

Source code in `src/figaroh/tools/load_robot.py`

```
def get_available_loaders() -> dict:
    """Get information about available robot loaders.

    Returns:
        dict: Information about each loader and its availability
    """
    loaders = {
        "figaroh": {
            "description": "Original figaroh Robot class",
            "available": True,
            "returns": "Robot instance",
            "features": ["URDF loading", "ROS param server", "Free-flyer support"],
        },
        "robot_description": {
            "description": "Load from robot_descriptions package",
            "available": _check_package_available("robot_descriptions"),
            "returns": "RobotWrapper instance",
            "features": ["Pre-defined robot models", "Easy robot switching"],
        },
        "yourdfpy": {
            "description": "Load with yourdfpy (for visualization)",
            "available": _check_package_available("yourdfpy"),
            "returns": "URDF object",
            "features": ["Visualization support", "Mesh loading", "Scene graphs"],
        },
    }

    return loaders
```

## `list_available_robots(loader='robot_description')`

List available robot descriptions for a given loader.

Parameters:

| Name     | Type  | Description                               | Default               |
| -------- | ----- | ----------------------------------------- | --------------------- |
| `loader` | `str` | Loader type to check for available robots | `'robot_description'` |

Returns:

| Name   | Type   | Description           |
| ------ | ------ | --------------------- |
| `list` | `list` | Available robot names |

Source code in `src/figaroh/tools/load_robot.py`

```
def list_available_robots(loader: str = "robot_description") -> list:
    """List available robot descriptions for a given loader.

    Args:
        loader: Loader type to check for available robots

    Returns:
        list: Available robot names
    """
    if loader == "robot_description":
        if not _check_package_available("robot_descriptions"):
            return []

        try:
            import robot_descriptions

            # Get all available robot descriptions with URDF format
            available_robots = []

            # Check if DESCRIPTIONS attribute exists
            if hasattr(robot_descriptions, "DESCRIPTIONS"):
                descriptions = robot_descriptions.DESCRIPTIONS
                for robot_name, robot_info in descriptions.items():
                    # Check if the robot has URDF format available
                    if hasattr(robot_info, "has_urdf") and robot_info.has_urdf:
                        available_robots.append(robot_name)
                    # Fallback: check if robot_info has URDF_PATH attribute
                    elif hasattr(robot_info, "URDF_PATH"):
                        available_robots.append(robot_name)
            else:
                # Fallback to original method if DESCRIPTIONS not available
                for attr_name in dir(robot_descriptions):
                    if attr_name.startswith("_"):
                        continue
                    attr_obj = getattr(robot_descriptions, attr_name)
                    if hasattr(attr_obj, "URDF_PATH"):
                        available_robots.append(attr_name)

            return sorted(available_robots)
        except Exception:
            return []

    return []
```

## Regressor Builder

Regressor matrix computation utilities for robot dynamic identification.

## `RegressorConfig(has_friction=False, has_actuator_inertia=False, has_joint_offset=False, is_joint_torques=True, is_external_wrench=False, force_torque=None, additional_columns=0)`

Configuration for regressor computation.

### `extra_blocks()`

Enabled per-joint extra parameter blocks, in column order.

The order matches `add_standard_additional_parameters`: viscous friction, static friction, actuator inertia, joint offset.

Source code in `src/figaroh/tools/regressor.py`

```
def extra_blocks(self) -> List[str]:
    """Enabled per-joint extra parameter blocks, in column order.

    The order matches ``add_standard_additional_parameters``:
    viscous friction, static friction, actuator inertia, joint offset.
    """
    blocks = []
    if self.has_friction:
        blocks += ["fv", "fs"]
    if self.has_actuator_inertia:
        blocks.append("ia")
    if self.has_joint_offset:
        blocks.append("off")
    return blocks
```

## `RegressorBuilder(robot, config=None)`

Enhanced regressor builder with better organization.

Source code in `src/figaroh/tools/regressor.py`

```
def __init__(self, robot, config: Optional[RegressorConfig] = None):
    self.robot = robot
    self.config = config or RegressorConfig()
    # Use backend if available, fall back to direct model access
    self._has_backend = hasattr(robot, "backend")
    self.nv = robot.backend.nv if self._has_backend else robot.model.nv
    self.nonzero_inertias = self._get_nonzero_inertias()
    # Start of each enabled extra block, in units of blocks after the
    # 10 inertial columns per body; disabled blocks take no columns.
    self._extra_block_index = {
        name: k for k, name in enumerate(self.config.extra_blocks())
    }
    self._n_extra_columns = len(self._extra_block_index)
```

### `build_basic_regressor(q, v, a, identif_config=None)`

Build basic regressor matrix.

Source code in `src/figaroh/tools/regressor.py`

```
def build_basic_regressor(
    self, q: np.ndarray, v: np.ndarray, a: np.ndarray, identif_config=None
) -> np.ndarray:
    """Build basic regressor matrix."""
    # Normalize inputs
    Q, V, A, N = self._normalize_inputs(q, v, a)

    if self.config.is_joint_torques:
        return self._build_joint_torque_regressor(Q, V, A, N, identif_config)
    elif self.config.is_external_wrench:
        return self._build_external_wrench_regressor(Q, V, A, N, identif_config)
    else:
        raise ValueError(
            "Must specify either joint_torques or external_wrench mode"
        )
```

## `build_regressor_basic(robot, q, v, a, identif_config, tau=None)`

Legacy function for backward compatibility.

Source code in `src/figaroh/tools/regressor.py`

```
def build_regressor_basic(robot, q, v, a, identif_config, tau=None):
    """Legacy function for backward compatibility."""
    # Calculate additional columns based on enabled options
    additional_columns = sum(
        [
            2 if identif_config.get("has_friction", False) else 0,  # fv and fs
            1 if identif_config.get("has_actuator_inertia", False) else 0,  # ia
            1 if identif_config.get("has_joint_offset", False) else 0,  # offset
        ]
    )

    config = RegressorConfig(
        has_friction=identif_config.get("has_friction", False),
        has_actuator_inertia=identif_config.get("has_actuator_inertia", False),
        has_joint_offset=identif_config.get("has_joint_offset", False),
        is_joint_torques=identif_config.get("is_joint_torques", True),
        is_external_wrench=identif_config.get("is_external_wrench", False),
        force_torque=identif_config.get("force_torque", None),
        additional_columns=additional_columns,
    )

    builder = RegressorBuilder(robot, config)
    return builder.build_basic_regressor(q, v, a, identif_config)
```

## `eliminate_non_dynaffect(W, params_std, tol_e=1e-06)`

Eliminate columns with small L2 norm.

Source code in `src/figaroh/tools/regressor.py`

```
def eliminate_non_dynaffect(W, params_std, tol_e=1e-6):
    """Eliminate columns with small L2 norm."""
    col_norms = np.diag(W.T @ W)
    param_keys = list(params_std.keys())

    keep_indices = []
    keep_params = []

    for i, norm in enumerate(col_norms):
        if norm >= tol_e and i < len(param_keys):
            keep_indices.append(i)
            keep_params.append(param_keys[i])

    W_reduced = W[:, keep_indices]
    return W_reduced, keep_params
```

## `get_index_eliminate(W, params_std, tol_e=1e-06)`

Get indices of columns to eliminate based on tolerance.

Parameters:

| Name         | Type | Description                    | Default    |
| ------------ | ---- | ------------------------------ | ---------- |
| `W`          |      | Joint torque regressor matrix  | *required* |
| `params_std` |      | Standard parameters dictionary | *required* |
| `tol_e`      |      | Tolerance value                | `1e-06`    |

Returns:

| Name    | Type | Description                                               |
| ------- | ---- | --------------------------------------------------------- |
| `tuple` |      | List of indices to eliminate List of remaining parameters |

Source code in `src/figaroh/tools/regressor.py`

```
def get_index_eliminate(W, params_std, tol_e=1e-6):
    """Get indices of columns to eliminate based on tolerance.

    Args:
        W: Joint torque regressor matrix
        params_std: Standard parameters dictionary
        tol_e: Tolerance value

    Returns:
        tuple:
            - List of indices to eliminate
            - List of remaining parameters
    """
    col_norm = np.diag(np.dot(W.T, W))
    idx_e = []
    params_r = []
    for i in range(col_norm.shape[0]):
        if col_norm[i] < tol_e:
            idx_e.append(i)
        else:
            params_r.append(list(params_std.keys())[i])
    return idx_e, params_r
```

## `build_regressor_reduced(W, idx_e)`

Build reduced regressor matrix.

Parameters:

| Name    | Type | Description                     | Default    |
| ------- | ---- | ------------------------------- | ---------- |
| `W`     |      | Input regressor matrix          | *required* |
| `idx_e` |      | Indices of columns to eliminate | *required* |

Returns:

| Name      | Type | Description              |
| --------- | ---- | ------------------------ |
| `ndarray` |      | Reduced regressor matrix |

Source code in `src/figaroh/tools/regressor.py`

```
def build_regressor_reduced(W, idx_e):
    """Build reduced regressor matrix.

    Args:
        W: Input regressor matrix
        idx_e: Indices of columns to eliminate

    Returns:
        ndarray: Reduced regressor matrix
    """
    W_e = np.delete(W, idx_e, 1)
    return W_e
```

## `build_total_regressor_current(W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, identif_config)`

Build regressor for total least squares with current measurements.

Parameters:

| Name               | Type | Description                        | Default    |
| ------------------ | ---- | ---------------------------------- | ---------- |
| `W_b_u`            |      | Base regressor for unloaded case   | *required* |
| `W_b_l`            |      | Base regressor for loaded case     | *required* |
| `W_l`              |      | Full regressor for loaded case     | *required* |
| `I_u`              |      | Joint currents in unloaded case    | *required* |
| `I_l`              |      | Joint currents in loaded case      | *required* |
| `param_standard_l` |      | Standard parameters in loaded case | *required* |
| `identif_config`   |      | Dictionary of settings             | *required* |

Returns:

| Name    | Type | Description                                                                 |
| ------- | ---- | --------------------------------------------------------------------------- |
| `tuple` |      | Total regressor matrix Normalized identif_configeter vector Residual vector |

Source code in `src/figaroh/tools/regressor.py`

```
def build_total_regressor_current(
    W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, identif_config
):
    """Build regressor for total least squares with current measurements.

    Args:
        W_b_u: Base regressor for unloaded case
        W_b_l: Base regressor for loaded case
        W_l: Full regressor for loaded case
        I_u: Joint currents in unloaded case
        I_l: Joint currents in loaded case
        param_standard_l: Standard parameters in loaded case
        identif_config: Dictionary of settings

    Returns:
        tuple:
            - Total regressor matrix
            - Normalized identif_configeter vector
            - Residual vector
    """
    W_tot = np.concatenate((-W_b_u, -W_b_l), axis=0)

    nb_joints = int(len(I_u) / identif_config["nb_samples"])
    n_samples = identif_config["nb_samples"]

    V_a = np.concatenate(
        [
            I_u[:n_samples].reshape(n_samples, 1),
            np.zeros(((nb_joints - 1) * n_samples, 1)),
        ],
        axis=0,
    )

    V_b = np.concatenate(
        [
            I_l[:n_samples].reshape(n_samples, 1),
            np.zeros(((nb_joints - 1) * n_samples, 1)),
        ],
        axis=0,
    )

    for ii in range(1, nb_joints):
        V_a_ii = np.concatenate(
            [
                np.zeros((n_samples * ii, 1)),
                I_u[n_samples * ii : (ii + 1) * n_samples].reshape(n_samples, 1),
                np.zeros((n_samples * (nb_joints - ii - 1), 1)),
            ],
            axis=0,
        )
        V_b_ii = np.concatenate(
            [
                np.zeros((n_samples * ii, 1)),
                I_l[n_samples * ii : (ii + 1) * n_samples].reshape(n_samples, 1),
                np.zeros((n_samples * (nb_joints - ii - 1), 1)),
            ],
            axis=0,
        )
        V_a = np.concatenate((V_a, V_a_ii), axis=1)
        V_b = np.concatenate((V_b, V_b_ii), axis=1)

    W_current = np.concatenate((V_a, V_b), axis=0)
    W_tot = np.concatenate((W_tot, W_current), axis=1)

    if identif_config["has_friction"]:
        W_l_temp = np.zeros((len(W_l), 12))
        for k in [0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 11]:
            W_l_temp[:, k] = W_l[:, (identif_config["which_body_loaded"]) * 12 + k]
        idx_e_temp, params_r_temp = get_index_eliminate(
            W_l_temp, param_standard_l, 1e-6
        )
        W_e_l = build_regressor_reduced(W_l_temp, idx_e_temp)
        W_upayload = np.concatenate(
            (np.zeros((len(W_l), W_e_l.shape[1])), -W_e_l), axis=0
        )
        W_tot = np.concatenate((W_tot, W_upayload), axis=1)
        W_kpayload = np.concatenate(
            (
                np.zeros((len(W_l), 1)),
                -W_l[:, (identif_config["which_body_loaded"]) * 12 + 9].reshape(
                    len(W_l), 1
                ),
            ),
            axis=0,
        )
        W_tot = np.concatenate((W_tot, W_kpayload), axis=1)

    elif identif_config["has_actuator_inertia"]:
        W_l_temp = np.zeros((len(W_l), 14))
        for k in [0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 11, 12, 13]:
            W_l_temp[:, k] = W_l[:, (identif_config["which_body_loaded"]) * 14 + k]
        idx_e_temp, params_r_temp = get_index_eliminate(
            W_l_temp, param_standard_l, 1e-6
        )
        W_e_l = build_regressor_reduced(W_l_temp, idx_e_temp)
        W_upayload = np.concatenate(
            (np.zeros((len(W_l), W_e_l.shape[1])), -W_e_l), axis=0
        )
        W_tot = np.concatenate((W_tot, W_upayload), axis=1)
        W_kpayload = np.concatenate(
            (
                np.zeros((len(W_l), 1)),
                -W_l[:, (identif_config["which_body_loaded"]) * 14 + 9].reshape(
                    len(W_l), 1
                ),
            ),
            axis=0,
        )
        W_tot = np.concatenate((W_tot, W_kpayload), axis=1)

    else:
        W_l_temp = np.zeros((len(W_l), 9))
        for k in range(9):
            W_l_temp[:, k] = W_l[:, (identif_config["which_body_loaded"]) * 10 + k]
        idx_e_temp, params_r_temp = get_index_eliminate(
            W_l_temp, param_standard_l, 1e-6
        )
        W_e_l = build_regressor_reduced(W_l_temp, idx_e_temp)
        W_upayload = np.concatenate(
            (np.zeros((len(W_l), W_e_l.shape[1])), -W_e_l), axis=0
        )
        W_tot = np.concatenate((W_tot, W_upayload), axis=1)
        W_kpayload = np.concatenate(
            (
                np.zeros((len(W_l), 1)),
                -W_l[:, (identif_config["which_body_loaded"]) * 10 + 9].reshape(
                    len(W_l), 1
                ),
            ),
            axis=0,
        )
        W_tot = np.concatenate((W_tot, W_kpayload), axis=1)

    U, S, Vh = np.linalg.svd(W_tot, full_matrices=False)
    V = np.transpose(Vh).conj()
    V_norm = identif_config["mass_load"] * np.divide(V[:, -1], V[-1, -1])
    residue = np.matmul(W_tot, V_norm)

    return W_tot, V_norm, residue
```

## `build_total_regressor_wrench(W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, identif_config)`

Build regressor for total least squares with external wrench measurements.

Parameters:

| Name               | Type | Description                                                           | Default    |
| ------------------ | ---- | --------------------------------------------------------------------- | ---------- |
| `W_b_u`            |      | Base regressor for unloaded case                                      | *required* |
| `W_b_l`            |      | Base regressor for loaded case                                        | *required* |
| `W_l`              |      | Full regressor for loaded case                                        | *required* |
| `tau_u`            |      | External wrench in unloaded case                                      | *required* |
| `tau_l`            |      | External wrench in loaded case                                        | *required* |
| `param_standard_l` |      | Standard parameters in loaded case                                    | *required* |
| `identif_config`   |      | Dictionary of settings. Must provide which_body_loaded and mass_load. | *required* |

Returns:

| Name    | Type | Description                                                        |
| ------- | ---- | ------------------------------------------------------------------ |
| `tuple` |      | Total regressor matrix Normalized parameter vector Residual vector |

Source code in `src/figaroh/tools/regressor.py`

```
def build_total_regressor_wrench(
    W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, identif_config
):
    """Build regressor for total least squares with external wrench measurements.

    Args:
        W_b_u: Base regressor for unloaded case
        W_b_l: Base regressor for loaded case
        W_l: Full regressor for loaded case
        tau_u: External wrench in unloaded case
        tau_l: External wrench in loaded case
        param_standard_l: Standard parameters in loaded case
        identif_config: Dictionary of settings. Must provide
            ``which_body_loaded`` and ``mass_load``.

    Returns:
        tuple:
            - Total regressor matrix
            - Normalized parameter vector
            - Residual vector
    """
    W_tot = np.concatenate((-W_b_u, -W_b_l), axis=0)

    tau_meast_ul = np.reshape(tau_u, (len(tau_u), 1))
    tau_meast_l = np.reshape(tau_l, (len(tau_l), 1))

    nb_samples_ul = int(len(tau_meast_ul) / 6)
    nb_samples_l = int(len(tau_meast_l) / 6)

    tau_ul = np.concatenate(
        [
            tau_meast_ul[:nb_samples_ul],
            np.zeros((len(tau_meast_ul) - nb_samples_ul, 1)),
        ],
        axis=0,
    )

    tau_l = np.concatenate(
        [tau_meast_l[:nb_samples_l], np.zeros((len(tau_meast_l) - nb_samples_l, 1))],
        axis=0,
    )

    for ii in range(1, 6):
        tau_ul_ii = np.concatenate(
            [
                np.concatenate(
                    [
                        np.zeros((nb_samples_ul * ii, 1)),
                        tau_meast_ul[nb_samples_ul * ii : (ii + 1) * nb_samples_ul],
                    ],
                    axis=0,
                ),
                np.zeros((nb_samples_ul * (5 - ii), 1)),
            ],
            axis=0,
        )

        tau_l_ii = np.concatenate(
            [
                np.concatenate(
                    [
                        np.zeros((nb_samples_l * ii, 1)),
                        tau_meast_l[nb_samples_l * ii : (ii + 1) * nb_samples_l],
                    ],
                    axis=0,
                ),
                np.zeros((nb_samples_l * (5 - ii), 1)),
            ],
            axis=0,
        )

        tau_ul = np.concatenate((tau_ul, tau_ul_ii), axis=1)
        tau_l = np.concatenate((tau_l, tau_l_ii), axis=1)

    W_tau = np.concatenate((tau_ul, tau_l), axis=0)
    W_tot = np.concatenate((W_tot, W_tau), axis=1)

    W_l_temp = np.zeros((len(W_l), 9))
    for k in range(9):
        W_l_temp[:, k] = W_l[:, (identif_config["which_body_loaded"]) * 10 + k]
    W_upayload = np.concatenate(
        (np.zeros((len(W_l), W_l_temp.shape[1])), -W_l_temp), axis=0
    )
    W_tot = np.concatenate((W_tot, W_upayload), axis=1)

    W_kpayload = np.concatenate(
        [
            np.zeros((len(W_l), 1)),
            -W_l[:, identif_config["which_body_loaded"] * 10 + 9].reshape(len(W_l), 1),
        ],
        axis=0,
    )
    W_tot = np.concatenate((W_tot, W_kpayload), axis=1)

    U, S, Vh = np.linalg.svd(W_tot, full_matrices=False)
    V = np.transpose(Vh).conj()
    V_norm = identif_config["mass_load"] * np.divide(V[:, -1], V[-1, -1])
    residue = np.matmul(W_tot, V_norm)

    return W_tot, V_norm, residue
```

## Robot Visualization

Enhanced robot visualization utilities with better performance.

## `VisualizationConfig(com_color=None, axes_color=None, force_color=None, bbox_color=None, scale_factor=1.0)`

Configuration for robot visualization.

## `RobotVisualizer(model, data, viz, config=None)`

Enhanced robot visualizer with better organization.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def __init__(
    self,
    model: pin.Model,
    data: pin.Data,
    viz,
    config: Optional[VisualizationConfig] = None,
):
    self.model = model
    self.data = data
    self.viz = viz
    self.config = config or VisualizationConfig()

    # Cache frequently used values
    self.joint_names = model.names
    self.nv = model.nv
```

### `update_kinematics(q)`

Update robot kinematics efficiently.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def update_kinematics(self, q: np.ndarray) -> None:
    """Update robot kinematics efficiently."""
    pin.forwardKinematics(self.model, self.data, q)
    pin.updateFramePlacements(self.model, self.data)
    pin.centerOfMass(self.model, self.data, q, True)
    pin.computeSubtreeMasses(self.model, self.data)
```

### `display_com(q, frame_indices)`

Display center of mass positions with better naming.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_com(self, q: np.ndarray, frame_indices: List[int]) -> None:
    """Display center of mass positions with better naming."""
    self.update_kinematics(q)

    for i, frame_idx in enumerate(frame_indices[:-1]):  # Exclude last frame
        next_frame_idx = frame_indices[i + 1]

        # Calculate link properties
        link_length = np.linalg.norm(
            self.data.oMf[next_frame_idx].translation
            - self.data.oMf[frame_idx].translation
        )

        # Mass-based radius scaling
        mass_ratio = (
            self.data.mass[i] / self.data.mass[0] if self.data.mass[0] > 0 else 0.1
        )
        radius = link_length * mass_ratio * self.config.scale_factor

        # COM placement
        placement = self.data.oMf[frame_idx].copy()
        parent_id = self.model.frames[frame_idx].parentJoint - 1
        if parent_id >= 0:
            placement.translation = self.data.com[parent_id]

        # Create and place sphere
        sphere_name = f"world/com_sphere_{i}"
        self.viz.viewer.gui.addSphere(sphere_name, radius, self.config.com_color)
        self._apply_placement(sphere_name, placement)
```

### `display_axes(q, axis_length=0.15, axis_radius=0.01)`

Display coordinate axes for joints.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_axes(
    self, q: np.ndarray, axis_length: float = 0.15, axis_radius: float = 0.01
) -> None:
    """Display coordinate axes for joints."""
    self.update_kinematics(q)

    for i, joint_name in enumerate(self.joint_names):
        joint_id = self.model.getJointId(joint_name)
        axis_name = f"world/axis_{joint_name}_{i}"

        # Create axis visualization
        self.viz.viewer.gui.addXYZaxis(
            axis_name,
            self.config.axes_color,
            axis_radius * self.config.scale_factor,
            axis_length * self.config.scale_factor,
        )

        # Apply joint placement
        self._apply_placement(axis_name, self.data.oMi[joint_id])
```

### `display_force(force, placement, scale=0.001, name='force_vector')`

Display force vector with better scaling.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_force(
    self,
    force: pin.Force,
    placement: pin.SE3,
    scale: float = 1e-3,
    name: str = "force_vector",
) -> None:
    """Display force vector with better scaling."""
    # Transform force to world frame
    world_force = force.se3Action(placement)
    force_magnitude = np.linalg.norm(world_force.linear)

    if force_magnitude < 1e-10:  # Skip negligible forces
        return

    # Calculate arrow properties
    arrow_length = force_magnitude * scale * self.config.scale_factor
    arrow_radius = arrow_length * 0.05

    # Align arrow with force direction
    force_direction = world_force.linear / force_magnitude
    rotation = self._rotation_from_vectors([1, 0, 0], force_direction)

    arrow_placement = placement.copy()
    arrow_placement.rotation = rotation

    # Create and place arrow
    arrow_name = f"world/{name}_arrow"
    self.viz.viewer.gui.addArrow(
        arrow_name, arrow_radius, arrow_length, self.config.force_color
    )
    self._apply_placement(arrow_name, arrow_placement)
```

### `display_bounding_boxes(q, com_min, com_max, frame_indices)`

Display COM bounding boxes for optimization visualization.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_bounding_boxes(
    self,
    q: np.ndarray,
    com_min: np.ndarray,
    com_max: np.ndarray,
    frame_indices: List[int],
) -> None:
    """Display COM bounding boxes for optimization visualization."""
    self.update_kinematics(q)

    for i, frame_idx in enumerate(frame_indices):
        # Get COM bounds for this segment
        bounds_start = 3 * i
        min_bounds = com_min[bounds_start : bounds_start + 3]
        max_bounds = com_max[bounds_start : bounds_start + 3]

        # Calculate box dimensions
        box_size = (max_bounds - min_bounds) * self.config.scale_factor

        # Box placement at COM location
        placement = self.data.oMf[frame_idx].copy()
        parent_id = self.model.frames[frame_idx].parentJoint - 1
        if parent_id >= 0:
            placement.translation = self.data.com[parent_id]

        # Create and place box
        box_name = f"world/com_bbox_{i}"
        self.viz.viewer.gui.addBox(
            box_name, box_size[0], box_size[1], box_size[2], self.config.bbox_color
        )
        self._apply_placement(box_name, placement)
```

### `display_joints(q)`

Display joint frames efficiently.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_joints(self, q: np.ndarray) -> None:
    """Display joint frames efficiently."""
    self.update_kinematics(q)

    for i in range(self.nv):
        joint_placement = self.data.oMi[i]
        joint_name = f"world/joint_frame_{i}"

        # Create joint frame visualization
        self.viz.viewer.gui.addXYZaxis(
            joint_name,
            self.config.axes_color,
            0.01 * self.config.scale_factor,
            0.15 * self.config.scale_factor,
        )

        # Apply configuration
        self._apply_placement(joint_name, joint_placement)
```

## `place(viz, name, M)`

Legacy placement function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def place(viz, name: str, M: pin.SE3) -> None:
    """Legacy placement function."""
    _warn_deprecated("place", "viz.viewer.gui.applyConfiguration directly")
    viz.viewer.gui.applyConfiguration(name, pin.SE3ToXYZQUATtuple(M))
```

## `display_COM(model, data, viz, q, IDX)`

Legacy COM display function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_COM(
    model: pin.Model, data: pin.Data, viz, q: np.ndarray, IDX: list
) -> None:
    """Legacy COM display function."""
    _warn_deprecated("display_COM", "RobotVisualizer.display_com")
    visualizer = RobotVisualizer(model, data, viz)
    visualizer.display_com(q, IDX)
```

## `display_axes(model, data, viz, q)`

Legacy axes display function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_axes(model: pin.Model, data: pin.Data, viz, q: np.ndarray) -> None:
    """Legacy axes display function."""
    _warn_deprecated("display_axes", "RobotVisualizer.display_axes")
    visualizer = RobotVisualizer(model, data, viz)
    visualizer.display_axes(q)
```

## `rotation_matrix_from_vectors(vec1, vec2)`

Legacy rotation function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def rotation_matrix_from_vectors(vec1: np.ndarray, vec2: np.ndarray) -> np.ndarray:
    """Legacy rotation function."""
    _warn_deprecated("rotation_matrix_from_vectors", "RobotVisualizer")
    visualizer = RobotVisualizer(None, None, None)
    return visualizer._rotation_from_vectors(vec1, vec2)
```

## `display_force(viz, phi, M_se3)`

Legacy force display function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_force(viz, phi: pin.Force, M_se3: pin.SE3) -> None:
    """Legacy force display function."""
    _warn_deprecated("display_force", "RobotVisualizer.display_force")
    visualizer = RobotVisualizer(None, None, viz)
    visualizer.display_force(phi, M_se3)
```

## `display_bounding_boxes(viz, model, data, q, COM_min, COM_max, IDX)`

Legacy bounding box display function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_bounding_boxes(
    viz,
    model: pin.Model,
    data: pin.Data,
    q: np.ndarray,
    COM_min: np.ndarray,
    COM_max: np.ndarray,
    IDX: list,
) -> None:
    """Legacy bounding box display function."""
    _warn_deprecated("display_bounding_boxes", "RobotVisualizer.display_bounding_boxes")
    visualizer = RobotVisualizer(model, data, viz)
    visualizer.display_bounding_boxes(q, COM_min, COM_max, IDX)
```

## `display_joints(viz, model, data, q)`

Legacy joint display function.

Source code in `src/figaroh/tools/robotvisualization.py`

```
def display_joints(viz, model: pin.Model, data: pin.Data, q: np.ndarray) -> None:
    """Legacy joint display function."""
    _warn_deprecated("display_joints", "RobotVisualizer.display_joints")
    visualizer = RobotVisualizer(model, data, viz)
    visualizer.display_joints(q)
```

## Robot Collisions

Enhanced collision detection and visualization utilities.

## `CollisionManager(robot, geom_model=None, geom_data=None, viz=None)`

Enhanced collision detection with better performance and safety.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def __init__(self, robot, geom_model=None, geom_data=None, viz=None):
    self.robot = robot
    self.viz = viz
    self.model = robot.model
    self.data = robot.model.createData()

    # Initialize geometry models
    self.geom_model = geom_model or getattr(robot, "geom_model", None)
    self.geom_data = geom_data or (
        self.geom_model.createData() if self.geom_model else None
    )

    if self.geom_data:
        self.geom_data.collisionRequests.enable_contact = True

    # Visualization cache
    self._vis_cache = {}
    self._max_patches = 10
```

### `setup_collision_pairs(srdf_model_path=None)`

Setup collision pairs with optional SRDF filtering.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def setup_collision_pairs(self, srdf_model_path: Optional[str] = None) -> None:
    """Setup collision pairs with optional SRDF filtering."""
    if not self.geom_model:
        raise ValueError("Geometry model not available for collision setup")

    # Add all collision pairs
    self.geom_model.addAllCollisionPairs()

    # Remove pairs specified in SRDF
    if srdf_model_path and self._file_exists(srdf_model_path):
        pin.removeCollisionPairs(self.model, self.geom_model, srdf_model_path)
```

### `check_collisions(q, update_geometry=True)`

Check for collisions at given configuration.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def check_collisions(self, q: np.ndarray, update_geometry: bool = True) -> bool:
    """Check for collisions at given configuration."""
    if not self.geom_model or not self.geom_data:
        return False

    if update_geometry:
        pin.updateGeometryPlacements(
            self.model, self.data, self.geom_model, self.geom_data, q
        )

    return pin.computeCollisions(
        self.model, self.data, self.geom_model, self.geom_data, q, False
    )
```

### `get_collision_details()`

Get detailed collision information.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def get_collision_details(self) -> List[Tuple[int, any, any]]:
    """Get detailed collision information."""
    if not self.geom_data:
        return []

    return [
        (idx, self.geom_model.collisionPairs[idx], result)
        for idx, result in enumerate(self.geom_data.collisionResults)
        if result.isCollision()
    ]
```

### `get_collision_distances(collision_details=None)`

Get minimum distances for collision pairs.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def get_collision_distances(
    self, collision_details: Optional[List] = None
) -> np.ndarray:
    """Get minimum distances for collision pairs."""
    if not self.geom_data:
        return np.array([])

    if collision_details is None:
        collision_details = self.get_collision_details()

    if not collision_details:
        return np.array([])

    return np.array(
        [
            self.geom_data.distanceResults[idx].min_distance
            for idx, _, _ in collision_details
        ]
    )
```

### `get_all_distances()`

Get distances for all collision pairs.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def get_all_distances(self) -> np.ndarray:
    """Get distances for all collision pairs."""
    if not self.geom_model or not self.geom_data:
        return np.array([])

    return np.array(
        [
            pin.computeDistance(self.geom_model, self.geom_data, k).min_distance
            for k in range(len(self.geom_model.collisionPairs))
        ]
    )
```

### `print_collision_pairs()`

Print all collision pair information.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def print_collision_pairs(self) -> None:
    """Print all collision pair information."""
    if not self.geom_model or not self.geom_data:
        print("No geometry model available")
        return

    print(f"Total collision pairs: {len(self.geom_model.collisionPairs)}")
    print("-" * 60)

    for k in range(len(self.geom_model.collisionPairs)):
        result = self.geom_data.collisionResults[k]
        pair = self.geom_model.collisionPairs[k]

        name1 = self.geom_model.geometryObjects[pair.first].name
        name2 = self.geom_model.geometryObjects[pair.second].name
        status = "COLLISION" if result.isCollision() else "FREE"

        print(f"Pair {k:3d}: {name1:20s} <-> {name2:20s} [{status}]")
```

### `visualize_collisions(collision_details=None)`

Visualize collision contacts with enhanced display.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def visualize_collisions(self, collision_details: Optional[List] = None) -> None:
    """Visualize collision contacts with enhanced display."""
    if not self.viz:
        return

    if collision_details is None:
        collision_details = self.get_collision_details()

    # Clean up old visualizations
    self._cleanup_old_contacts(len(collision_details))

    # Display new contacts
    for i, (idx, pair, result) in enumerate(collision_details[: self._max_patches]):
        if result.getNbContacts() > 0:
            contact = result.getContact(0)  # First contact point
            self._display_contact(i, contact, pair)
```

## `CollisionWrapper(robot, geom_model=None, geom_data=None, viz=None)`

Bases: `CollisionManager`

Legacy wrapper.

.. deprecated:: 1.0 Use :class:`CollisionManager`. Removal in 2.0.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def __init__(self, robot, geom_model=None, geom_data=None, viz=None):
    warnings.warn(
        "CollisionWrapper is deprecated in 1.0, will be removed in 2.0; "
        "use CollisionManager",
        DeprecationWarning,
        stacklevel=2,
    )
    super().__init__(robot, geom_model, geom_data, viz)

    # Legacy aliases
    self.rmodel = self.model
    self.rdata = self.data
    self.gmodel = self.geom_model
    self.gdata = self.geom_data
```

### `add_collisions()`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def add_collisions(self):
    """Legacy method."""
    if self.geom_model:
        self.geom_model.addAllCollisionPairs()
```

### `remove_collisions(srdf_model_path)`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def remove_collisions(self, srdf_model_path):
    """Legacy method."""
    if srdf_model_path and self.geom_model:
        pin.removeCollisionPairs(self.model, self.geom_model, srdf_model_path)
```

### `computeCollisions(q, geom_data=None)`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def computeCollisions(self, q, geom_data=None):
    """Legacy method."""
    if geom_data:
        self.geom_data = geom_data
    return self.check_collisions(q)
```

### `getCollisionList()`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def getCollisionList(self):
    """Legacy method."""
    return self.get_collision_details()
```

### `getCollisionDistances(collisions=None)`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def getCollisionDistances(self, collisions=None):
    """Legacy method."""
    return self.get_collision_distances(collisions)
```

### `getDistances()`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def getDistances(self):
    """Legacy method."""
    return self.get_all_distances()
```

### `getAllpairs()`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def getAllpairs(self):
    """Legacy method."""
    self.print_collision_pairs()
```

### `check_collision(q)`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def check_collision(self, q):
    """Legacy method."""
    return self.check_collisions(q)
```

### `displayCollisions(collisions=None)`

Legacy method.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def displayCollisions(self, collisions=None):
    """Legacy method."""
    self.visualize_collisions(collisions)
```

## `add_non_adjacent_pairs(model, geom_model)`

Add a collision pair for every two geometries on non-adjacent bodies.

Geometries on the same joint, or on a joint and its parent (adjacent bodies, usually in contact by design), are not paired. Fixed bodies belong to the joint they are merged into (`universe` for the world). Returns the number of pairs added.

Source code in `src/figaroh/tools/robotcollisions.py`

```
def add_non_adjacent_pairs(model, geom_model) -> int:
    """Add a collision pair for every two geometries on non-adjacent bodies.

    Geometries on the same joint, or on a joint and its parent (adjacent
    bodies, usually in contact by design), are not paired. Fixed bodies
    belong to the joint they are merged into (``universe`` for the world).
    Returns the number of pairs added.
    """
    objects = geom_model.geometryObjects
    added = 0
    for i in range(len(objects)):
        for j in range(i + 1, len(objects)):
            a, b = objects[i].parentJoint, objects[j].parentJoint
            if a == b or model.parents[a] == b or model.parents[b] == a:
                continue
            geom_model.addCollisionPair(pin.CollisionPair(i, j))
            added += 1
    return added
```

## QR Decomposition

QR decomposition utilities for robot parameter identification.

This module provides QR-based helpers to:

- identify a set of base parameters (a full-rank subset) from a rank-deficient regressor
- express those base parameters as linear combinations of the original/full parameters

In particular, both QR workflows compute a matrix $M$ such that:

$$ \\phi\_{base} = M,\\theta\_{full} $$

where columns of $M$ correspond to the *remaining* parameter vector ordered as `params_r`.

Note

In FIGAROH, `params_r` typically refers to the list of *standard parameter names remaining after column elimination* (e.g., after removing all-zero regressor columns). It is not necessarily the complete set of standard parameters (which is often available as a dictionary like `params_std`).

Use :meth:`QRDecomposer.expand_mapping_matrix_to_full` if you need an "M" defined over a full, canonical parameter ordering.

The resulting row ordering matches the returned base-parameter expressions.

## `QRResult(rank, base_indices, pivot_order, W_b, beta, M, base_param_expressions, phi_b, phi_b_nom, method, diag_R, cond_R1)`

Structured output of a QR base-parameter decomposition.

All numerical fields are stored at full float64 precision. Rounding for display is done only at the expression-building stage.

Attributes:

| Name                     | Type                  | Description                                                                                                          |
| ------------------------ | --------------------- | -------------------------------------------------------------------------------------------------------------------- |
| `rank`                   | `int`                 | Identified numerical rank of the regressor.                                                                          |
| `base_indices`           | `List[int]`           | Column indices into params_r that form the independent (base) set.                                                   |
| `pivot_order`            | `Optional[List[int]]` | Full pivot permutation used by the pivoting path (from :func:deterministic_column_pivots); None for the double path. |
| `W_b`                    | `ndarray`             | Base regressor matrix, shape (m, rank).                                                                              |
| `beta`                   | `ndarray`             | Dependency coefficient matrix, shape (rank, n - rank). Full precision — not rounded.                                 |
| `M`                      | `ndarray`             | Base mapping matrix satisfying phi_base = M @ theta_r, shape (rank, n).                                              |
| `base_param_expressions` | `List[str]`           | Human-readable expression strings, length rank.                                                                      |
| `phi_b`                  | `Optional[ndarray]`   | Identified base-parameter values, shape (rank,); None when computed without a tau vector.                            |
| `phi_b_nom`              | `Optional[ndarray]`   | Nominal base-parameter values from params_std prior, shape (rank,); None when params_std was not provided.           |
| `method`                 | `str`                 | "pivoting" or "double".                                                                                              |
| `diag_R`                 | `ndarray`             | Absolute diagonal of R at the factorisation point, shape (min(m, n),). Useful for stability diagnostics.             |
| `cond_R1`                | `float`               | Condition number of the R[:rank, :rank] block. Measures how well-conditioned the base part of the factorisation is.  |

## `QRDecomposer(tolerance=TOL_QR, beta_tolerance=TOL_BETA, relative_tolerance=None, deterministic=True)`

Enhanced QR decomposition handler for robot parameter identification.

Parameters:

| Name            | Type   | Description                                                                                                                                                                                                                                             | Default |
| --------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `deterministic` | `bool` | Choose base columns with :func:deterministic_column_pivots (default), so ties between equally large columns are broken by parameter name instead of BLAS rounding noise (#116). False keeps LAPACK's pivoted QR, whose tie order is platform dependent. | `True`  |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def __init__(
    self,
    tolerance: float = TOL_QR,
    beta_tolerance: float = TOL_BETA,
    relative_tolerance: Optional[float] = None,
    deterministic: bool = True,
):
    """
    Args:
        deterministic: Choose base columns with
            :func:`deterministic_column_pivots` (default), so ties
            between equally large columns are broken by parameter name
            instead of BLAS rounding noise (#116). ``False`` keeps
            LAPACK's pivoted QR, whose tie order is platform dependent.
    """
    self.deterministic = deterministic
    self.tolerance = tolerance
    self.beta_tolerance = beta_tolerance
    self.relative_tolerance = relative_tolerance

    # Last computed base-mapping matrix and its labels.
    # M maps the remaining parameter vector (ordered as params_r) to base.
    self.M: Optional[np.ndarray] = None
    self.M_params_r: Optional[List[str]] = None
    self.M_base_params_expr: Optional[List[str]] = None
    self.M_method: Optional[str] = None
    # Column indices into params_r selected as the base (independent) set
    # by the last decomposition. Only populated by double_decomposition().
    self.base_indices: Optional[List[int]] = None
    self.regroup_indices: Optional[List[int]] = None
    # Diagnostics from the last decomposition (available via get_diagnostics)
    self._last_diag_R: Optional[np.ndarray] = None
    self._last_cond_R1: Optional[float] = None
```

### `get_M()`

Return the last computed base mapping matrix `M` (or None).

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_M(self) -> Optional[np.ndarray]:
    """Return the last computed base mapping matrix ``M`` (or None)."""
    return self.M
```

### `get_M_labels()`

Return (base_param_expr, params_r) labels for the last stored M.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_M_labels(self) -> Tuple[Optional[List[str]], Optional[List[str]]]:
    """Return (base_param_expr, params_r) labels for the last stored M."""
    return self.M_base_params_expr, self.M_params_r
```

### `get_base_indices()`

Return the base-column indices (into `params_r`) from the last `double_decomposition()` call, or `None` if not yet run.

These indices select the columns of a *reduced* regressor (built with the same `params_r` ordering) that reproduce base-parameter predictions: `tau ≈ W_reduced[:, base_indices] @ phi_base`. Since the base/dependent column split reflects the robot's kinematic structure rather than a specific trajectory, this selection is valid for any regressor built with the same `params_r` ordering — e.g. to evaluate an identified model on held-out validation data.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_base_indices(self) -> Optional[List[int]]:
    """Return the base-column indices (into ``params_r``) from the last
    ``double_decomposition()`` call, or ``None`` if not yet run.

    These indices select the columns of a *reduced* regressor (built
    with the same ``params_r`` ordering) that reproduce base-parameter
    predictions: ``tau ≈ W_reduced[:, base_indices] @ phi_base``. Since
    the base/dependent column split reflects the robot's kinematic
    structure rather than a specific trajectory, this selection is
    valid for any regressor built with the same ``params_r`` ordering
    — e.g. to evaluate an identified model on held-out validation data.
    """
    return self.base_indices
```

### `get_diagnostics()`

Return stability diagnostics from the last decomposition.

Keys

`rank`: identified rank. `diag_R`: absolute diagonal of R at the factorisation point. `cond_R1`: condition number of the R[:rank, :rank] block. `method`: `"pivoting"` or `"double"`.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_diagnostics(self) -> Dict[str, Any]:
    """Return stability diagnostics from the last decomposition.

    Keys:
        ``rank``: identified rank.
        ``diag_R``: absolute diagonal of R at the factorisation point.
        ``cond_R1``: condition number of the R[:rank, :rank] block.
        ``method``: ``"pivoting"`` or ``"double"``.
    """
    return {
        "rank": self.M.shape[0] if self.M is not None else None,
        "diag_R": self._last_diag_R,
        "cond_R1": self._last_cond_R1,
        "method": self.M_method,
    }
```

### `decompose(W_e, params_r, tau=None, params_std=None, method='double')`

Unified entry point that returns a structured :class:`QRResult`.

This is the preferred API for new code. It delegates to either :meth:`decompose_with_pivoting` (`method="pivoting"`) or :meth:`double_decomposition` (`method="double"`), but always returns a :class:`QRResult` regardless of path.

Parameters:

| Name         | Type                         | Description                                                                                                                                | Default    |
| ------------ | ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------ | ---------- |
| `W_e`        | `ndarray`                    | Full regressor matrix, shape (m, n).                                                                                                       | *required* |
| `params_r`   | `List[str]`                  | Remaining parameter names (columns of W_e), length n.                                                                                      | *required* |
| `tau`        | `Optional[ndarray]`          | Torque/effort vector, shape (m,). Defaults to zeros, which is sufficient for rank/basis computation; required for meaningful phi_b values. | `None`     |
| `params_std` | `Optional[Dict[str, float]]` | Optional prior parameter dict used to compute phi_b_nom (available in the double path only).                                               | `None`     |
| `method`     | `str`                        | "double" (default) or "pivoting".                                                                                                          | `'double'` |

Returns:

| Type       | Description                        |
| ---------- | ---------------------------------- |
| `QRResult` | A fully-populated :class:QRResult. |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def decompose(
    self,
    W_e: np.ndarray,
    params_r: List[str],
    tau: Optional[np.ndarray] = None,
    params_std: Optional[Dict[str, float]] = None,
    method: str = "double",
) -> "QRResult":
    """Unified entry point that returns a structured :class:`QRResult`.

    This is the preferred API for new code.  It delegates to either
    :meth:`decompose_with_pivoting` (``method="pivoting"``) or
    :meth:`double_decomposition` (``method="double"``), but always
    returns a :class:`QRResult` regardless of path.

    Args:
        W_e: Full regressor matrix, shape ``(m, n)``.
        params_r: Remaining parameter names (columns of ``W_e``), length n.
        tau: Torque/effort vector, shape ``(m,)``.  Defaults to zeros,
            which is sufficient for rank/basis computation; required for
            meaningful ``phi_b`` values.
        params_std: Optional prior parameter dict used to compute
            ``phi_b_nom`` (available in the double path only).
        method: ``"double"`` (default) or ``"pivoting"``.

    Returns:
        A fully-populated :class:`QRResult`.
    """
    if tau is None:
        tau = np.zeros(W_e.shape[0])

    method_norm = method.strip().lower()
    if method_norm in {"pivoting", "pivot", "qr_pivoting", "qr-pivoting"}:
        Q, R, P, rank = self._pivoted_qr(W_e, params_r)
        params_sorted = [params_r[P[i]] for i in range(P.shape[0])]
        R1, Q1, R2 = self._extract_base_components(R, Q, rank)
        beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
        phi_b = np.linalg.solve(R1, Q1.T @ tau)
        W_b = Q1 @ R1
        base_param_expressions = self._build_parameter_expressions(
            params_sorted[:rank], params_sorted[rank:], beta
        )
        n_params = len(params_r)
        M = self._build_M_from_pivoting(
            beta=beta, P=P, rank=rank, n_params=n_params
        )
        diag_R = np.abs(np.diag(R))
        cond_R1 = float(np.linalg.cond(R1)) if rank > 0 else 0.0
        base_indices = sorted(P[:rank].tolist())
        return QRResult(
            rank=rank,
            base_indices=base_indices,
            pivot_order=P.tolist(),
            W_b=W_b,
            beta=beta,
            M=M,
            base_param_expressions=base_param_expressions,
            phi_b=phi_b,
            phi_b_nom=None,
            method="pivoting",
            diag_R=diag_R,
            cond_R1=cond_R1,
        )

    if method_norm in {"double", "double_qr", "double-qr"}:
        base_indices, regroup_indices = self._identify_base_parameters(
            W_e, params_r
        )
        W_base, W_regroup, params_base, params_regroup = self._regroup_parameters(
            W_e, params_r, base_indices, regroup_indices
        )
        W_regrouped = np.c_[W_base, W_regroup]
        Q_r, R_r = np.linalg.qr(W_regrouped)
        rank = len(base_indices)
        R1, Q1, R2 = self._extract_base_components(R_r, Q_r, rank)
        beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
        phi_b = np.linalg.solve(R1, Q1.T @ tau)
        W_b = Q1 @ R1
        base_param_expressions = self._build_parameter_expressions(
            params_base, params_regroup, beta
        )
        n_params = len(params_r)
        M = self._build_M_from_partition(
            beta=beta,
            base_indices=base_indices,
            regroup_indices=regroup_indices,
            n_params=n_params,
        )
        diag_R = np.abs(np.diag(R_r))
        cond_R1 = float(np.linalg.cond(R1)) if rank > 0 else 0.0
        phi_b_nom = None
        if params_std is not None:
            phi_b_nom = self.compute_nominal_base_parameters(
                params_base, params_regroup, beta, params_std
            )
        return QRResult(
            rank=rank,
            base_indices=base_indices,
            pivot_order=None,
            W_b=W_b,
            beta=beta,
            M=M,
            base_param_expressions=base_param_expressions,
            phi_b=phi_b,
            phi_b_nom=phi_b_nom,
            method="double",
            diag_R=diag_R,
            cond_R1=cond_R1,
        )

    raise ValueError(f"Unknown method={method!r}. Expected 'double' or 'pivoting'.")
```

### `decompose_with_pivoting(tau, W_e, params_r)`

Perform QR decomposition with column pivoting.

This identifies an effective rank, computes base parameters, and returns a base regressor `W_b` along with a dict mapping base-parameter expressions to values.

Parameters:

| Name       | Type        | Description                                           | Default    |
| ---------- | ----------- | ----------------------------------------------------- | ---------- |
| `tau`      | `ndarray`   | Torque/effort vector, shape (m,).                     | *required* |
| `W_e`      | `ndarray`   | Full regressor matrix, shape (m, n).                  | *required* |
| `params_r` | `List[str]` | Remaining parameter names (columns of W_e), length n. | *required* |

Returns:

| Type                               | Description                                                                                                                             |
| ---------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------- |
| `Tuple[ndarray, Dict[str, float]]` | (W_b, base_parameters) where: - W_b has shape (m, r) with r = rank(W_e) - base_parameters maps expression strings (length r) to values. |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def decompose_with_pivoting(
    self, tau: np.ndarray, W_e: np.ndarray, params_r: List[str]
) -> Tuple[np.ndarray, Dict[str, float]]:
    """Perform QR decomposition with column pivoting.

    This identifies an effective rank, computes base parameters, and
    returns a base regressor ``W_b`` along with a dict mapping
    base-parameter expressions to values.

    Args:
        tau: Torque/effort vector, shape (m,).
        W_e: Full regressor matrix, shape (m, n).
        params_r: Remaining parameter names (columns of W_e), length n.

    Returns:
        (W_b, base_parameters) where:
          - W_b has shape (m, r) with r = rank(W_e)
          - base_parameters maps expression strings (length r) to values.
    """
    Q, R, P, rank = self._pivoted_qr(W_e, params_r)

    # Reorder parameters according to pivoting
    params_sorted = [params_r[P[i]] for i in range(P.shape[0])]

    # Extract base components
    R1, Q1, R2 = self._extract_base_components(R, Q, rank)

    # Compute base parameters — full precision; round only for display (#142)
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
    phi_b = np.linalg.solve(R1, Q1.T @ tau)
    W_b = Q1 @ R1

    # Build parameter expressions (display rounding happens inside)
    base_params = self._build_parameter_expressions(
        params_sorted[:rank], params_sorted[rank:], beta
    )

    # Store mapping matrix M such that: phi_base = M @ theta_r
    n_params = len(params_r)
    M = self._build_M_from_pivoting(
        beta=beta,
        P=P,
        rank=rank,
        n_params=n_params,
    )

    self.M = M
    self.M_params_r = list(params_r)
    self.M_base_params_expr = list(base_params)
    self.M_method = "pivoting"
    self._last_diag_R = np.abs(np.diag(R))
    self._last_cond_R1 = float(np.linalg.cond(R1)) if rank > 0 else 0.0

    return W_b, dict(zip(base_params, phi_b))
```

### `double_decomposition(tau, W_e, params_r, params_std=None)`

Perform the "double QR" workflow for base-parameter identification.

This follows a two-stage procedure

1. Identify an independent/base subset via QR on `W_e`.
1. Regroup columns and run a second QR to compute linear dependencies.

The output base parameters are returned as expression strings built from the original parameter names.

Parameters:

| Name         | Type                         | Description                                                                                                                                                                                                                                                                           | Default    |
| ------------ | ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `tau`        | `ndarray`                    | Torque/effort vector, shape (m,).                                                                                                                                                                                                                                                     | *required* |
| `W_e`        | `ndarray`                    | Full regressor matrix, shape (m, n).                                                                                                                                                                                                                                                  | *required* |
| `params_r`   | `List[str]`                  | Remaining parameter names (columns of W_e), length n.                                                                                                                                                                                                                                 | *required* |
| `params_std` | `Optional[Dict[str, float]]` | Optional mapping from full parameter name -> value. If provided, also returns the standard-parameter values of the base expressions. Returns: If params_std is None: (W_b, base_parameters, params_base_expr, phi_b) else: (W_b, base_parameters, params_base_expr, phi_b, phi_b_nom) | `None`     |
| `Where`      |                              | W_b has shape (m, r) params_base_expr is a list[str] of length r phi_b is a vector of length r - phi_b_nom is length r if params_std is provided                                                                                                                                      | *required* |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def double_decomposition(
    self,
    tau: np.ndarray,
    W_e: np.ndarray,
    params_r: List[str],
    params_std: Optional[Dict[str, float]] = None,
) -> Union[
    Tuple[np.ndarray, Dict, List, np.ndarray],
    Tuple[np.ndarray, Dict, List, np.ndarray, np.ndarray],
]:
    """Perform the "double QR" workflow for base-parameter identification.

    This follows a two-stage procedure:
        1) Identify an independent/base subset via QR on ``W_e``.
        2) Regroup columns and run a second QR to compute linear
           dependencies.

    The output base parameters are returned as expression strings built
    from the original parameter names.

    Args:
        tau: Torque/effort vector, shape (m,).
        W_e: Full regressor matrix, shape (m, n).
        params_r: Remaining parameter names (columns of W_e), length n.
        params_std: Optional mapping from full parameter name -> value. If
            provided, also returns the standard-parameter values of
            the base expressions.

            Returns:
                    If params_std is None:
                        (W_b, base_parameters, params_base_expr, phi_b)
                    else:
                        (W_b, base_parameters, params_base_expr, phi_b,
                         phi_b_nom)

        Where:
          - W_b has shape (m, r)
          - params_base_expr is a list[str] of length r
          - phi_b is a vector of length r
                        - phi_b_nom is length r if params_std is provided
    """

    # First QR to identify base parameters
    base_indices, regroup_indices = self._identify_base_parameters(W_e, params_r)

    # Regroup and second QR
    (
        W_base,
        W_regroup,
        params_base,
        params_regroup,
    ) = self._regroup_parameters(
        W_e,
        params_r,
        base_indices,
        regroup_indices,
    )

    # Second QR decomposition
    W_regrouped = np.c_[W_base, W_regroup]
    Q_r, R_r = np.linalg.qr(W_regrouped)

    rank = len(base_indices)
    R1, Q1, R2 = self._extract_base_components(R_r, Q_r, rank)

    # Compute parameters — full precision; round only for display (#142)
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
    phi_b = np.linalg.solve(R1, Q1.T @ tau)
    W_b = Q1 @ R1

    # Build expressions and compute standard parameters if provided
    params_base_expr = self._build_parameter_expressions(
        params_base, params_regroup, beta
    )
    base_parameters = dict(zip(params_base_expr, phi_b))

    # Store mapping matrix M such that: phi_base = M @ theta_r
    n_params = len(params_r)
    M = self._build_M_from_partition(
        beta=beta,
        base_indices=base_indices,
        regroup_indices=regroup_indices,
        n_params=n_params,
    )

    self.M = M
    self.M_params_r = list(params_r)
    self.M_base_params_expr = list(params_base_expr)
    self.M_method = "double"
    self.base_indices = list(base_indices)
    self.regroup_indices = list(regroup_indices)
    self._last_diag_R = np.abs(np.diag(R_r))
    self._last_cond_R1 = float(np.linalg.cond(R1)) if rank > 0 else 0.0

    if params_std is not None:
        phi_b_nom = self.compute_nominal_base_parameters(
            params_base, params_regroup, beta, params_std
        )

        return W_b, base_parameters, params_base_expr, phi_b, phi_b_nom

    return W_b, base_parameters, params_base_expr, phi_b
```

### `compute_nominal_base_parameters(base_params, regroup_params, beta, params_std)`

Compute numerical values of base parameters from priors.

This computes the numerical value of each base expression using:

```
phi_std[i] = params_std[base_param]
             + sum_j beta[i, j] * params_std[dep_param]
```

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def compute_nominal_base_parameters(
    self,
    base_params: List[str],
    regroup_params: List[str],
    beta: np.ndarray,
    params_std: Dict[str, float],
) -> np.ndarray:
    """Compute numerical values of base parameters from priors.

    This computes the numerical value of each base expression using:

        phi_std[i] = params_std[base_param]
                     + sum_j beta[i, j] * params_std[dep_param]
    """
    phi_std = [params_std[param] for param in base_params]

    for i in range(len(phi_std)):
        for j, regroup_param in enumerate(regroup_params):
            if j < beta.shape[1]:
                phi_std[i] += beta[i, j] * params_std[regroup_param]

    return np.array(phi_std, dtype=float)
```

### `get_base_mapping_matrix_pivoting(W_e, params_r)`

Return the base-parameter mapping matrix M for pivoting-QR.

The matrix `M` satisfies `phi_base = M @ theta_r` where `theta_r` is ordered as `params_r` (the remaining parameters).

Row order matches the returned base-parameter expression strings.

Parameters:

| Name       | Type        | Description                                           | Default    |
| ---------- | ----------- | ----------------------------------------------------- | ---------- |
| `W_e`      | `ndarray`   | Full regressor matrix, shape (m, n).                  | *required* |
| `params_r` | `List[str]` | Remaining parameter names (columns of W_e), length n. | *required* |

Returns:

| Type                        | Description                                                                                                             |
| --------------------------- | ----------------------------------------------------------------------------------------------------------------------- |
| `Tuple[ndarray, List[str]]` | (M, base_params_expr) - M has shape (r, n) where r is the identified rank - base_params_expr is a list[str] of length r |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_base_mapping_matrix_pivoting(
    self, W_e: np.ndarray, params_r: List[str]
) -> Tuple[np.ndarray, List[str]]:
    r"""Return the base-parameter mapping matrix M for pivoting-QR.

    The matrix ``M`` satisfies ``phi_base = M @ theta_r`` where
    ``theta_r`` is ordered as ``params_r`` (the remaining parameters).

    Row order matches the returned base-parameter expression strings.

    Args:
        W_e: Full regressor matrix, shape (m, n).
        params_r: Remaining parameter names (columns of W_e), length n.

    Returns:
        (M, base_params_expr)
          - M has shape (r, n) where r is the identified rank
          - base_params_expr is a list[str] of length r
    """
    _, R, P, rank = self._pivoted_qr(W_e, params_r)

    # Dependency coefficients in the pivoted ordering — full precision
    R1 = R[:rank, :rank]
    R2 = R[:rank, rank:] if rank < R.shape[1] else np.array([]).reshape(rank, 0)
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
    n_params = len(params_r)
    M = self._build_M_from_pivoting(
        beta=beta,
        P=P,
        rank=rank,
        n_params=n_params,
    )

    params_sorted = [params_r[P[i]] for i in range(P.shape[0])]
    base_params_expr = self._build_parameter_expressions(
        params_sorted[:rank], params_sorted[rank:], beta
    )
    return M, base_params_expr
```

### `get_base_mapping_matrix_double(W_e, params_r)`

Return the base-parameter mapping matrix M for the double-QR workflow.

The returned `M` satisfies `phi_base = M @ theta_r` with columns ordered as `params_r` (the remaining parameters).

Parameters:

| Name       | Type        | Description                                           | Default    |
| ---------- | ----------- | ----------------------------------------------------- | ---------- |
| `W_e`      | `ndarray`   | Full regressor matrix, shape (m, n).                  | *required* |
| `params_r` | `List[str]` | Remaining parameter names (columns of W_e), length n. | *required* |

Returns:

| Type                                              | Description                                                                                                                                                                    |
| ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| `Tuple[ndarray, List[str], List[int], List[int]]` | (M, base_params_expr, base_indices, regroup_indices) - M has shape (r, n) - base_params_expr is a list[str] of length r - base_indices and regroup_indices index into params_r |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_base_mapping_matrix_double(
    self, W_e: np.ndarray, params_r: List[str]
) -> Tuple[np.ndarray, List[str], List[int], List[int]]:
    """Return the base-parameter mapping matrix M for the double-QR
    workflow.

    The returned ``M`` satisfies ``phi_base = M @ theta_r`` with columns
    ordered as ``params_r`` (the remaining parameters).

    Args:
        W_e: Full regressor matrix, shape (m, n).
        params_r: Remaining parameter names (columns of W_e), length n.

    Returns:
        (M, base_params_expr, base_indices, regroup_indices)
          - M has shape (r, n)
          - base_params_expr is a list[str] of length r
          - base_indices and regroup_indices index into params_r
    """
    base_indices, regroup_indices = self._identify_base_parameters(W_e, params_r)
    (
        W_base,
        W_regroup,
        params_base,
        params_regroup,
    ) = self._regroup_parameters(
        W_e,
        params_r,
        base_indices,
        regroup_indices,
    )

    W_regrouped = np.c_[W_base, W_regroup]
    _, R_r = np.linalg.qr(W_regrouped)
    rank = len(base_indices)

    R1 = R_r[:rank, :rank]
    R2 = R_r[:rank, rank:] if rank < R_r.shape[1] else np.array([]).reshape(rank, 0)
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))

    n_params = len(params_r)
    M = self._build_M_from_partition(
        beta=beta,
        base_indices=base_indices,
        regroup_indices=regroup_indices,
        n_params=n_params,
    )

    base_params_expr = self._build_parameter_expressions(
        params_base, params_regroup, beta
    )
    return M, base_params_expr, base_indices, regroup_indices
```

### `get_base_mapping_matrix(W_e, params_r, method='double')`

Convenience wrapper to retrieve the base mapping matrix.

Parameters:

| Name       | Type        | Description                                           | Default    |
| ---------- | ----------- | ----------------------------------------------------- | ---------- |
| `W_e`      | `ndarray`   | Full regressor matrix, shape (m, n).                  | *required* |
| `params_r` | `List[str]` | Remaining parameter names (columns of W_e), length n. | *required* |
| `method`   | `str`       | "double" (default) or "pivoting".                     | `'double'` |

Returns:

| Type                        | Description           |
| --------------------------- | --------------------- |
| `Tuple[ndarray, List[str]]` | (M, base_params_expr) |

Raises:

| Type         | Description                  |
| ------------ | ---------------------------- |
| `ValueError` | if method is not recognized. |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_base_mapping_matrix(
    self, W_e: np.ndarray, params_r: List[str], method: str = "double"
) -> Tuple[np.ndarray, List[str]]:
    """Convenience wrapper to retrieve the base mapping matrix.

    Args:
        W_e: Full regressor matrix, shape (m, n).
        params_r: Remaining parameter names (columns of W_e), length n.
        method: "double" (default) or "pivoting".

    Returns:
        (M, base_params_expr)

    Raises:
        ValueError: if method is not recognized.
    """
    method_norm = method.strip().lower()
    if method_norm in {"double", "double_qr", "double-qr"}:
        M, base_params_expr, _, _ = self.get_base_mapping_matrix_double(
            W_e, params_r
        )
        return M, base_params_expr
    if method_norm in {"pivoting", "pivot", "qr_pivoting", "qr-pivoting"}:
        return self.get_base_mapping_matrix_pivoting(W_e, params_r)
    raise ValueError(f"Unknown method={method!r}. Expected 'double' or 'pivoting'.")
```

### `expand_mapping_matrix_to_full(M, params_r, full_param_names)`

Expand an M defined on `params_r` into a full parameter ordering.

This is useful when the QR routines operate on a reduced set of parameters (after removing all-zero columns), but you want a mapping matrix defined over the complete standard-parameter list.

Parameters:

| Name               | Type        | Description                                                   | Default    |
| ------------------ | ----------- | ------------------------------------------------------------- | ---------- |
| `M`                | `ndarray`   | Base mapping matrix for remaining parameters, shape (r, n_r). | *required* |
| `params_r`         | `List[str]` | Remaining parameter names (columns of M), length n_r.         | *required* |
| `full_param_names` | `List[str]` | Full/canonical parameter ordering to expand into.             | *required* |

Returns:

| Type      | Description                                              |
| --------- | -------------------------------------------------------- |
| `ndarray` | M_full of shape (r, n_full) where columns not present in |
| `ndarray` | params_r are set to 0.                                   |

Raises:

| Type         | Description                                                                            |
| ------------ | -------------------------------------------------------------------------------------- |
| `ValueError` | if M has incompatible shape or if a name in params_r is missing from full_param_names. |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
@staticmethod
def expand_mapping_matrix_to_full(
    M: np.ndarray,
    params_r: List[str],
    full_param_names: List[str],
) -> np.ndarray:
    """Expand an M defined on ``params_r`` into a full parameter ordering.

    This is useful when the QR routines operate on a reduced set of
    parameters (after removing all-zero columns), but you want a mapping
    matrix defined over the complete standard-parameter list.

    Args:
        M: Base mapping matrix for remaining parameters, shape (r, n_r).
        params_r: Remaining parameter names (columns of ``M``), length n_r.
        full_param_names: Full/canonical parameter ordering to expand into.

    Returns:
        M_full of shape (r, n_full) where columns not present in
        ``params_r`` are set to 0.

    Raises:
        ValueError: if M has incompatible shape or if a name in params_r is
            missing from full_param_names.
    """
    if M.ndim != 2:
        raise ValueError("M must be a 2D array")
    if M.shape[1] != len(params_r):
        raise ValueError(
            "M has incompatible column count: "
            f"M.shape[1]={M.shape[1]} but len(params_r)={len(params_r)}"
        )

    full_index = {name: idx for idx, name in enumerate(full_param_names)}
    missing = [name for name in params_r if name not in full_index]
    if missing:
        raise ValueError(
            "params_r contains names not present in full_param_names: "
            f"{', '.join(missing)}"
        )

    M_full = np.zeros((M.shape[0], len(full_param_names)), dtype=M.dtype)
    for j_r, name in enumerate(params_r):
        M_full[:, full_index[name]] = M[:, j_r]
    return M_full
```

## `deterministic_column_pivots(W, names=None, *, tol=TOL_QR, relative_tol=None, tie_rtol=TIE_RTOL)`

Pick independent regressor columns the same way on every machine.

LAPACK's pivoted QR (`scipy.linalg.qr(pivoting=True)`) takes the column of largest residual norm at each step. Columns whose norms tie to rounding error (common in rigid-body regressors) are then ordered by BLAS noise, so the selected set, and with it `phi_base`, depends on the platform.

This greedy selection (Gram-Schmidt on the `R` factor, with re-orthogonalisation) takes the largest-residual column too, but treats all columns within `tie_rtol` of the largest as tied and picks the one with the smallest name (falling back to the smallest index when `names` is None). The outcome then depends on the column names, not on their order in `W` or on rounding noise below `tie_rtol`.

Parameters:

| Name           | Type                  | Description                                                                                      | Default    |
| -------------- | --------------------- | ------------------------------------------------------------------------------------------------ | ---------- |
| `W`            | `ndarray`             | Regressor, shape (m, n).                                                                         | *required* |
| `names`        | `Optional[List[str]]` | Column names, length n. Ties are broken by name.                                                 | `None`     |
| `tol`          | `float`               | Absolute rank threshold on the residual norm.                                                    | `TOL_QR`   |
| `relative_tol` | `Optional[float]`     | If set, threshold is max(relative_tol * first_pivot, tol), as in :meth:QRDecomposer.\_find_rank. | `None`     |
| `tie_rtol`     | `float`               | Relative width of the tie band.                                                                  | `TIE_RTOL` |

Returns:

| Type                  | Description                                                          |
| --------------------- | -------------------------------------------------------------------- |
| `ndarray`             | (P, rank): a permutation of range(n) whose first rank                |
| `int`                 | entries are the selected columns in selection order, followed by the |
| `Tuple[ndarray, int]` | remaining columns in ascending name (index) order.                   |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def deterministic_column_pivots(
    W: np.ndarray,
    names: Optional[List[str]] = None,
    *,
    tol: float = TOL_QR,
    relative_tol: Optional[float] = None,
    tie_rtol: float = TIE_RTOL,
) -> Tuple[np.ndarray, int]:
    """Pick independent regressor columns the same way on every machine.

    LAPACK's pivoted QR (``scipy.linalg.qr(pivoting=True)``) takes the column
    of largest residual norm at each step. Columns whose norms tie to rounding
    error (common in rigid-body regressors) are then ordered by BLAS noise,
    so the selected set, and with it ``phi_base``, depends on the platform.

    This greedy selection (Gram-Schmidt on the ``R`` factor, with
    re-orthogonalisation) takes the largest-residual column too, but treats
    all columns within ``tie_rtol`` of the largest as tied and picks the one
    with the smallest name (falling back to the smallest index when ``names``
    is None). The outcome then depends on the column names, not on their
    order in ``W`` or on rounding noise below ``tie_rtol``.

    Args:
        W: Regressor, shape ``(m, n)``.
        names: Column names, length n. Ties are broken by name.
        tol: Absolute rank threshold on the residual norm.
        relative_tol: If set, threshold is ``max(relative_tol * first_pivot,
            tol)``, as in :meth:`QRDecomposer._find_rank`.
        tie_rtol: Relative width of the tie band.

    Returns:
        ``(P, rank)``: a permutation of ``range(n)`` whose first ``rank``
        entries are the selected columns in selection order, followed by the
        remaining columns in ascending name (index) order.
    """
    n = W.shape[1]
    if n == 0:
        return np.empty(0, dtype=int), 0
    keys = list(names) if names is not None else list(range(n))
    rank_order = sorted(range(n), key=lambda j: (keys[j], j))
    prio = np.empty(n, dtype=int)
    prio[rank_order] = np.arange(n)

    A = np.linalg.qr(np.asarray(W, dtype=float), mode="r")
    remaining = np.ones(n, dtype=bool)
    chosen: List[int] = []
    thr = None
    for _ in range(min(A.shape[0], n)):
        norms = np.linalg.norm(A, axis=0)
        norms[~remaining] = -1.0
        top = norms.max()
        if thr is None:
            thr = tol
            if relative_tol is not None:
                thr = max(relative_tol * top, tol)
        if top <= thr:
            break
        tied = np.flatnonzero(norms >= top * (1.0 - tie_rtol))
        p = int(tied[np.argmin(prio[tied])])
        chosen.append(p)
        remaining[p] = False
        q = A[:, p] / norms[p]
        for _ in range(2):
            A -= np.outer(q, q @ A)
    rest = [j for j in rank_order if remaining[j]]
    return np.array(chosen + rest, dtype=int), len(chosen)
```

## `QR_pivoting(tau, W_e, params_r, tol_qr=TOL_QR)`

Legacy QR pivoting function.

.. deprecated:: 1.0 Use :meth:`QRDecomposer.decompose_with_pivoting`. Removal in 2.0.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def QR_pivoting(
    tau: np.ndarray,
    W_e: np.ndarray,
    params_r: list,
    tol_qr: float = TOL_QR,
) -> tuple:
    """Legacy QR pivoting function.

    .. deprecated:: 1.0
        Use :meth:`QRDecomposer.decompose_with_pivoting`. Removal in 2.0.
    """
    warnings.warn(
        "QR_pivoting is deprecated in 1.0, will be removed in 2.0; "
        "use QRDecomposer.decompose_with_pivoting",
        DeprecationWarning,
        stacklevel=2,
    )
    decomposer = QRDecomposer(tolerance=tol_qr)
    return decomposer.decompose_with_pivoting(tau, W_e, params_r)
```

## `double_QR(tau, W_e, params_r, params_std=None, tol_qr=TOL_QR)`

Legacy double QR function.

.. deprecated:: 1.0 Use :meth:`QRDecomposer.double_decomposition`. Removal in 2.0.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def double_QR(
    tau: np.ndarray,
    W_e: np.ndarray,
    params_r: list,
    params_std: dict = None,
    tol_qr: float = TOL_QR,
) -> tuple:
    """Legacy double QR function.

    .. deprecated:: 1.0
        Use :meth:`QRDecomposer.double_decomposition`. Removal in 2.0.
    """
    warnings.warn(
        "double_QR is deprecated in 1.0, will be removed in 2.0; "
        "use QRDecomposer.double_decomposition",
        DeprecationWarning,
        stacklevel=2,
    )
    decomposer = QRDecomposer(tolerance=tol_qr)
    return decomposer.double_decomposition(tau, W_e, params_r, params_std)
```

## `get_baseParams(W_e, params_r, params_std=None, tol_qr=TOL_QR)`

Legacy function for getting base parameters.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_baseParams(
    W_e: np.ndarray,
    params_r: list,
    params_std: dict = None,
    tol_qr: float = TOL_QR,
) -> tuple:
    """Legacy function for getting base parameters."""
    decomposer = QRDecomposer(tolerance=tol_qr)
    dummy_tau = np.zeros(W_e.shape[0])  # Not used in this function
    result = decomposer.double_decomposition(dummy_tau, W_e, params_r, params_std)
    base_indices, _ = decomposer._identify_base_parameters(W_e, params_r)
    return result[0], result[2], base_indices
```

## `get_baseIndex(W_e, params_r, tol_qr=TOL_QR)`

Legacy function for getting base indices.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def get_baseIndex(
    W_e: np.ndarray,
    params_r: list,
    tol_qr: float = TOL_QR,
) -> tuple:
    """Legacy function for getting base indices."""
    decomposer = QRDecomposer(tolerance=tol_qr)
    base_indices, _ = decomposer._identify_base_parameters(W_e, params_r)
    return tuple(base_indices)
```

## `build_baseRegressor(W_e, idx_base)`

Legacy function for building base regressor.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def build_baseRegressor(W_e: np.ndarray, idx_base: tuple) -> np.ndarray:
    """Legacy function for building base regressor."""
    return W_e[:, list(idx_base)]
```

## `cond_num(W_b, norm_type=None)`

Calculate condition number with various norms.

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def cond_num(W_b: np.ndarray, norm_type: str = None) -> float:
    """Calculate condition number with various norms."""
    if norm_type == "fro":
        return np.linalg.cond(W_b, "fro")
    elif norm_type == "max_over_min_sigma":
        return np.linalg.cond(W_b, 2) / np.linalg.cond(W_b, -2)
    else:
        return np.linalg.cond(W_b)
```

## `redistribute_min_norm(M, phi_base)`

Minimum-norm redistribution of base-parameter values onto the full standard-parameter set, via the Moore-Penrose pseudoinverse of `M`.

Given $\\phi\_{base} = M,\\theta_r$, returns $\\hat\\theta_r = M^+,\\phi\_{base}$: the minimum-L2-norm $\\theta_r$ consistent with the identified $\\phi\_{base}$, i.e. each redundant group's fitted value is spread across its members instead of being concentrated on one arbitrary representative (the effective default when only the base subset is reported and everything else is left at its nominal value). Round-trips exactly: `M @ redistribute_min_norm(M, phi_base) == phi_base` (up to floating point), so predictions built from the redistributed vector are unchanged from those built from the base-only fit.

This does not add information: non-identifiable directions stay non-identifiable. It only picks a less arbitrary point among the (infinite) equally-valid ways to split an identified combination back onto its individually non-identifiable members.

Parameters:

| Name       | Type      | Description                                                                    | Default    |
| ---------- | --------- | ------------------------------------------------------------------------------ | ---------- |
| `M`        | `ndarray` | Base-mapping matrix, shape (rank, n_params), such that phi_base = M @ theta_r. | *required* |
| `phi_base` | `ndarray` | Fitted base-parameter values, shape (rank,).                                   | *required* |

Returns:

| Type      | Description                     |
| --------- | ------------------------------- |
| `ndarray` | theta_r_hat, shape (n_params,). |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def redistribute_min_norm(M: np.ndarray, phi_base: np.ndarray) -> np.ndarray:
    r"""Minimum-norm redistribution of base-parameter values onto the full
    standard-parameter set, via the Moore-Penrose pseudoinverse of ``M``.

    Given $\phi_{base} = M\,\theta_r$, returns
    $\hat\theta_r = M^+\,\phi_{base}$: the minimum-L2-norm $\theta_r$
    consistent with the identified $\phi_{base}$, i.e. each redundant
    group's fitted value is spread across its members instead of being
    concentrated on one arbitrary representative (the effective default
    when only the base subset is reported and everything else is left at
    its nominal value). Round-trips exactly:
    ``M @ redistribute_min_norm(M, phi_base) == phi_base`` (up to floating
    point), so predictions built from the redistributed vector are
    unchanged from those built from the base-only fit.

    This does not add information: non-identifiable directions stay
    non-identifiable. It only picks a less arbitrary point among the
    (infinite) equally-valid ways to split an identified combination back
    onto its individually non-identifiable members.

    Args:
        M: Base-mapping matrix, shape ``(rank, n_params)``, such that
            ``phi_base = M @ theta_r``.
        phi_base: Fitted base-parameter values, shape ``(rank,)``.

    Returns:
        ``theta_r_hat``, shape ``(n_params,)``.
    """
    return np.linalg.pinv(M) @ phi_base
```

## `propagate_covariance_min_norm(M, C_base)`

Propagate base-parameter covariance through the minimum-norm map.

Returns $C\_{full} = M^+,C\_{base},(M^+)^T$ — the covariance of the minimum-norm *estimator* (:func:`redistribute_min_norm`), conditional on having committed to that redistribution choice. It is **not** the true uncertainty of each individual standard parameter: along directions `M` cannot see, the data provides literally zero information, and this propagation reports a finite number reflecting the estimator's own sensitivity to noise in `phi_base`, not an unconditional physical uncertainty. Callers surfacing this to users must label it as such.

Parameters:

| Name     | Type      | Description                                                                    | Default    |
| -------- | --------- | ------------------------------------------------------------------------------ | ---------- |
| `M`      | `ndarray` | Base-mapping matrix, shape (rank, n_params), such that phi_base = M @ theta_r. | *required* |
| `C_base` | `ndarray` | Covariance of the fitted base parameters, shape (rank, rank).                  | *required* |

Returns:

| Type      | Description                         |
| --------- | ----------------------------------- |
| `ndarray` | C_full, shape (n_params, n_params). |

Source code in `src/figaroh/tools/qrdecomposition.py`

```
def propagate_covariance_min_norm(M: np.ndarray, C_base: np.ndarray) -> np.ndarray:
    r"""Propagate base-parameter covariance through the minimum-norm map.

    Returns $C_{full} = M^+\,C_{base}\,(M^+)^T$ — the covariance of the
    minimum-norm *estimator* (:func:`redistribute_min_norm`), conditional
    on having committed to that redistribution choice. It is **not** the
    true uncertainty of each individual standard parameter: along
    directions ``M`` cannot see, the data provides literally zero
    information, and this propagation reports a finite number reflecting
    the estimator's own sensitivity to noise in ``phi_base``, not an
    unconditional physical uncertainty. Callers surfacing this to users
    must label it as such.

    Args:
        M: Base-mapping matrix, shape ``(rank, n_params)``, such that
            ``phi_base = M @ theta_r``.
        C_base: Covariance of the fitted base parameters, shape
            ``(rank, rank)``.

    Returns:
        ``C_full``, shape ``(n_params, n_params)``.
    """
    M_pinv = np.linalg.pinv(M)
    return M_pinv @ C_base @ M_pinv.T
```

## Robot IPOPT

General IPOPT-based optimization framework for robotics applications.

This module provides a comprehensive, flexible framework for setting up and solving nonlinear optimization problems using IPOPT (Interior Point OPTimizer), with specialized support for robotics applications including trajectory optimization, parameter identification, and general robot optimization problems.

Key Features

- Unified IPOPT interface with robotics-specific configurations
- Automatic differentiation support for gradients and Jacobians
- Built-in problem validation and result analysis
- Specialized classes for trajectory optimization problems
- Factory functions for rapid problem setup
- Comprehensive logging and iteration tracking

Main Classes

- IPOPTConfig: Configuration management for IPOPT solver settings
- BaseOptimizationProblem: Abstract base class for optimization problems
- RobotIPOPTSolver: High-level solver interface with result analysis
- TrajectoryOptimizationProblem: Specialized class for trajectory problems

Usage Examples

Basic trajectory optimization:

```
from figaroh.tools.robotipopt import IPOPTConfig, RobotIPOPTSolver

# Create custom problem class
class MyProblem(BaseOptimizationProblem):
    def objective(self, x):
        return np.sum(x**2)  # Minimize sum of squares

    def constraints(self, x):
        return np.array([x[0] + x[1] - 1.0])  # x[0] + x[1] = 1

    # ... implement other required methods

# Solve the problem
problem = MyProblem()
config = IPOPTConfig.for_trajectory_optimization()
solver = RobotIPOPTSolver(problem, config)
success, results = solver.solve()
```

Using the factory function:

```
from figaroh.tools.robotipopt import create_trajectory_solver

def my_objective(x):
    return np.sum(x**2)

def my_constraints(x):
    return np.array([x[0] + x[1] - 1.0])

def get_bounds():
    var_bounds = ([-10, -10], [10, 10])
    cons_bounds = ([0], [0])
    return var_bounds, cons_bounds

def initial_guess():
    return [0.5, 0.5]

solver = create_trajectory_solver(
    my_objective, my_constraints, get_bounds, initial_guess
)
success, results = solver.solve()
```

Complete robotics example:

```
import numpy as np
from figaroh.tools.robotipopt import (
    BaseOptimizationProblem, RobotIPOPTSolver, IPOPTConfig
)

class RobotTrajectoryProblem(BaseOptimizationProblem):
    def __init__(self, robot, waypoints, active_joints):
        super().__init__("RobotTrajectory")
        self.robot = robot
        self.waypoints = waypoints
        self.active_joints = active_joints
        self.n_joints = len(active_joints)
        self.n_waypoints = len(waypoints)
        self.n_vars = self.n_joints * self.n_waypoints

    def get_variable_bounds(self):
        # Joint limits for all waypoints
        lb = [-np.pi] * self.n_vars
        ub = [np.pi] * self.n_vars
        return lb, ub

    def get_constraint_bounds(self):
        # Boundary conditions (start/end positions)
        n_constraints = 2 * self.n_joints
        return [0] * n_constraints, [0] * n_constraints

    def get_initial_guess(self):
        # Linear interpolation between start and end
        return np.linspace(
            self.waypoints[0], self.waypoints[-1],
            self.n_waypoints * self.n_joints
        )

    def objective(self, x):
        # Minimize trajectory smoothness (squared accelerations)
        q = x.reshape(self.n_waypoints, self.n_joints)
        acc = np.diff(q, n=2, axis=0)  # Second differences
        return np.sum(acc**2)

    def constraints(self, x):
        # Enforce boundary conditions
        q = x.reshape(self.n_waypoints, self.n_joints)
        constraints = np.concatenate([
            q[0] - self.waypoints[0],   # Start position
            q[-1] - self.waypoints[-1]  # End position
        ])
        return constraints

# Usage
problem = RobotTrajectoryProblem(robot, waypoints, active_joints)
config = IPOPTConfig.for_trajectory_optimization()
solver = RobotIPOPTSolver(problem, config)

if solver.validate_problem_setup():
    success, results = solver.solve()
    if success:
        optimal_trajectory = results['x_opt'].reshape(
            problem.n_waypoints, problem.n_joints
        )
        print("Optimization successful!")
        print(f"Final objective: {results['obj_val']:.6f}")
        print(solver.get_solution_summary())
```

Dependencies

- cyipopt: Python interface to IPOPT solver
- numpy: Numerical computations
- numdifftools: Automatic differentiation (fallback)

References

- IPOPT documentation: https://coin-or.github.io/Ipopt/
- cyipopt: https://github.com/mechmotum/cyipopt

## `IPOPTConfig(tolerance=1e-06, acceptable_tolerance=0.0001, max_iterations=3000, max_cpu_time=1000000.0, print_level=5, output_file=None, hessian_approximation='limited-memory', warm_start=True, check_derivatives=True, linear_solver='mumps', custom_options=dict())`

Configuration parameters for IPOPT solver.

This class provides a convenient way to set IPOPT solver options with sensible defaults for robotics applications. It includes predefined configurations for common robotics optimization scenarios.

The configuration is designed to work well with:

- Trajectory optimization problems (smooth, continuous trajectories)
- Parameter identification (high-precision parameter estimation)
- General robotics optimization (balanced performance/accuracy)

Attributes:

| Name                    | Type             | Description                                                                                                   |
| ----------------------- | ---------------- | ------------------------------------------------------------------------------------------------------------- |
| `tolerance`             | `float`          | Primary convergence tolerance for optimization. Default: 1e-6. Smaller values increase precision but runtime. |
| `acceptable_tolerance`  | `float`          | Fallback tolerance if primary fails. Default: 1e-4. Used when primary tolerance cannot be achieved.           |
| `max_iterations`        | `int`            | Maximum number of optimization iterations. Default: 3000. Increase for complex problems.                      |
| `max_cpu_time`          | `float`          | Maximum CPU time in seconds. Default: 1e6. Prevents infinite loops in difficult problems.                     |
| `print_level`           | `int`            | IPOPT output verbosity (0-12). Default: 5. Higher values provide more detailed output.                        |
| `output_file`           | `Optional[str]`  | File to save IPOPT output. Default: None. Useful for debugging and analysis.                                  |
| `hessian_approximation` | `str`            | Hessian computation method. Default: "limited-memory". Options: "exact", "limited-memory".                    |
| `warm_start`            | `bool`           | Use previous solution as starting point. Default: True. Speeds up subsequent optimizations.                   |
| `check_derivatives`     | `bool`           | Enable derivative checking. Default: True. Helps detect implementation errors.                                |
| `linear_solver`         | `str`            | Linear algebra solver backend. Default: "mumps". Options: "mumps", "ma27", "ma57", "ma77".                    |
| `custom_options`        | `Dict[str, Any]` | Additional IPOPT options. Default: {}. For advanced users to set specialized options.                         |

Examples:

Basic usage:

```
# Use default configuration
config = IPOPTConfig()

# Customize specific parameters
config = IPOPTConfig(
    tolerance=1e-8,
    max_iterations=5000,
    print_level=3
)
```

Predefined configurations:

```
# For trajectory optimization (speed-focused)
config = IPOPTConfig.for_trajectory_optimization()

# For parameter identification (precision-focused)
config = IPOPTConfig.for_parameter_identification()
```

Custom IPOPT options:

```
config = IPOPTConfig(
    custom_options={
        "mu_strategy": "adaptive",
        "nlp_scaling_method": "gradient-based"
    }
)
```

Note

All string options in custom_options should be provided as strings. They will be automatically encoded to bytes for IPOPT compatibility.

### `to_ipopt_options()`

Convert configuration to IPOPT option dictionary.

Transforms the configuration parameters into the format expected by IPOPT, including proper encoding of string options to bytes.

Returns:

| Type             | Description                                                                                   |
| ---------------- | --------------------------------------------------------------------------------------------- |
| `Dict[str, Any]` | Dict\[str, Any\]: Dictionary of IPOPT options ready for use with cyipopt.Problem.add_option() |

Note

All string values are automatically encoded to bytes as required by the IPOPT C interface via cyipopt.

Source code in `src/figaroh/tools/robotipopt.py`

```
def to_ipopt_options(self) -> Dict[str, Any]:
    """
    Convert configuration to IPOPT option dictionary.

    Transforms the configuration parameters into the format expected
    by IPOPT, including proper encoding of string options to bytes.

    Returns:
        Dict[str, Any]: Dictionary of IPOPT options ready for use
                       with cyipopt.Problem.add_option()

    Note:
        All string values are automatically encoded to bytes as required
        by the IPOPT C interface via cyipopt.
    """
    options = {
        # Basic convergence settings
        b"tol": self.tolerance,
        b"acceptable_tol": self.acceptable_tolerance,
        b"max_iter": self.max_iterations,
        b"max_cpu_time": self.max_cpu_time,
        # Output settings
        b"print_level": self.print_level,
        # Algorithm settings
        b"hessian_approximation": self.hessian_approximation.encode(),
        b"linear_solver": self.linear_solver.encode(),
        # Derivative checking
        b"check_derivatives_for_naninf": (
            b"yes" if self.check_derivatives else b"no"
        ),
        # Warm start
        b"warm_start_init_point": b"yes" if self.warm_start else b"no",
        b"acceptable_obj_change_tol": self.acceptable_tolerance,
    }

    # Add output file if specified
    if self.output_file:
        options[b"output_file"] = self.output_file.encode()

    # Add custom options
    for key, value in self.custom_options.items():
        if isinstance(key, str):
            key = key.encode()
        if isinstance(value, str):
            value = value.encode()
        options[key] = value

    return options
```

### `for_trajectory_optimization()`

Create configuration optimized for trajectory optimization problems.

This configuration prioritizes convergence speed over extreme precision, making it suitable for real-time or interactive trajectory planning. Uses adaptive barrier parameter strategy for better convergence.

Returns:

| Name          | Type          | Description                                      |
| ------------- | ------------- | ------------------------------------------------ |
| `IPOPTConfig` | `IPOPTConfig` | Optimized configuration for trajectory problems. |

Source code in `src/figaroh/tools/robotipopt.py`

```
@classmethod
def for_trajectory_optimization(cls) -> "IPOPTConfig":
    """
    Create configuration optimized for trajectory optimization problems.

    This configuration prioritizes convergence speed over extreme
    precision, making it suitable for real-time or interactive
    trajectory planning. Uses adaptive barrier parameter strategy
    for better convergence.

    Returns:
        IPOPTConfig: Optimized configuration for trajectory problems.
    """
    return cls(
        tolerance=1e-4,
        acceptable_tolerance=1e-3,
        max_iterations=1000,
        print_level=4,
        hessian_approximation="limited-memory",
        custom_options={
            b"mu_strategy": b"adaptive",
            b"adaptive_mu_globalization": b"obj-constr-filter",
        },
    )
```

### `for_parameter_identification()`

Create configuration optimized for parameter identification problems.

This configuration prioritizes high precision over speed, making it suitable for accurate parameter estimation in robotics applications. Uses exact Hessian computation and monotone barrier strategy for numerical stability.

Returns:

| Name          | Type          | Description                                           |
| ------------- | ------------- | ----------------------------------------------------- |
| `IPOPTConfig` | `IPOPTConfig` | Optimized configuration for parameter identification. |

Source code in `src/figaroh/tools/robotipopt.py`

```
@classmethod
def for_parameter_identification(cls) -> "IPOPTConfig":
    """
    Create configuration optimized for parameter identification problems.

    This configuration prioritizes high precision over speed, making it
    suitable for accurate parameter estimation in robotics applications.
    Uses exact Hessian computation and monotone barrier strategy for
    numerical stability.

    Returns:
        IPOPTConfig: Optimized configuration for parameter identification.
    """
    return cls(
        tolerance=1e-8,
        acceptable_tolerance=1e-6,
        max_iterations=5000,
        print_level=3,
        hessian_approximation="exact",
        custom_options={
            b"mu_strategy": b"monotone",
            b"fixed_variable_treatment": b"make_parameter",
        },
    )
```

## `BaseOptimizationProblem(name='OptimizationProblem')`

Bases: `ABC`

Abstract base class for IPOPT optimization problems.

This class defines the interface that all IPOPT problems must implement. Subclasses should implement the objective function, constraints, and their derivatives (or use automatic differentiation).

The class provides default implementations for gradient and Jacobian computation using automatic differentiation via numdifftools. For better performance, subclasses can override these methods with analytical derivatives.

Attributes:

| Name             | Type     | Description                                       |
| ---------------- | -------- | ------------------------------------------------- |
| `name`           | `str`    | Human-readable name for the optimization problem. |
| `logger`         | `Logger` | Logger instance for this problem.                 |
| `iteration_data` | `Dict`   | Storage for tracking optimization iterations.     |
| `callback_data`  | `Dict`   | Storage for passing data between methods.         |

Required Methods (must be implemented by subclasses):

- get_variable_bounds(): Return variable bounds as (lower, upper)
- get_constraint_bounds(): Return constraint bounds as (lower, upper)
- get_initial_guess(): Return initial guess for optimization variables
- objective(x): Evaluate objective function at point x
- constraints(x): Evaluate constraint functions at point x

Optional Methods (have default implementations):

- gradient(x): Compute objective gradient (uses auto-diff by default)
- jacobian(x): Compute constraint Jacobian (uses auto-diff by default)
- hessian(x, lagrange, obj_factor): Compute Hessian (IPOPT approx by default)
- intermediate(...): Callback for iteration tracking

Examples:

Basic implementation:

```
class QuadraticProblem(BaseOptimizationProblem):
    def get_variable_bounds(self):
        return ([-10, -10], [10, 10])

    def get_constraint_bounds(self):
        return ([0], [0])  # Equality constraint

    def get_initial_guess(self):
        return [1.0, 1.0]

    def objective(self, x):
        return x[0]**2 + x[1]**2  # Minimize sum of squares

    def constraints(self, x):
        return np.array([x[0] + x[1] - 1.0])  # x[0] + x[1] = 1
```

With custom derivatives:

```
class CustomProblem(BaseOptimizationProblem):
    # ... implement required methods ...

    def gradient(self, x):
        # Custom analytical gradient
        return np.array([2*x[0], 2*x[1]])

    def jacobian(self, x):
        # Custom analytical Jacobian
        return np.array([[1.0, 1.0]])
```

Initialize the optimization problem.

Source code in `src/figaroh/tools/robotipopt.py`

```
def __init__(self, name: str = "OptimizationProblem"):
    """Initialize the optimization problem."""
    self.name = name
    self.logger = logging.getLogger(f"{__name__}.{name}")

    # Storage for iteration tracking
    self.iteration_data = {
        "iterations": [],
        "obj_values": [],
        "constraint_violations": [],
        "solve_times": [],
    }

    # Callback storage for passing data between methods
    self.callback_data: Dict[str, Any] = {}
```

### `get_variable_bounds()`

Get bounds for optimization variables.

Returns:

| Type                              | Description                           |
| --------------------------------- | ------------------------------------- |
| `Tuple[List[float], List[float]]` | Tuple of (lower_bounds, upper_bounds) |

Source code in `src/figaroh/tools/robotipopt.py`

```
@abstractmethod
def get_variable_bounds(self) -> Tuple[List[float], List[float]]:
    """
    Get bounds for optimization variables.

    Returns:
        Tuple of (lower_bounds, upper_bounds)
    """
    pass
```

### `get_constraint_bounds()`

Get bounds for constraints.

Returns:

| Type                              | Description                                                 |
| --------------------------------- | ----------------------------------------------------------- |
| `Tuple[List[float], List[float]]` | Tuple of (constraint_lower_bounds, constraint_upper_bounds) |

Source code in `src/figaroh/tools/robotipopt.py`

```
@abstractmethod
def get_constraint_bounds(self) -> Tuple[List[float], List[float]]:
    """
    Get bounds for constraints.

    Returns:
        Tuple of (constraint_lower_bounds, constraint_upper_bounds)
    """
    pass
```

### `get_initial_guess()`

Get initial guess for optimization variables.

Returns:

| Type          | Description                     |
| ------------- | ------------------------------- |
| `List[float]` | Initial guess as list of floats |

Source code in `src/figaroh/tools/robotipopt.py`

```
@abstractmethod
def get_initial_guess(self) -> List[float]:
    """
    Get initial guess for optimization variables.

    Returns:
        Initial guess as list of floats
    """
    pass
```

### `objective(x)`

Evaluate objective function.

Parameters:

| Name | Type      | Description            | Default    |
| ---- | --------- | ---------------------- | ---------- |
| `x`  | `ndarray` | Optimization variables | *required* |

Returns:

| Type    | Description              |
| ------- | ------------------------ |
| `float` | Objective function value |

Source code in `src/figaroh/tools/robotipopt.py`

```
@abstractmethod
def objective(self, x: np.ndarray) -> float:
    """
    Evaluate objective function.

    Args:
        x: Optimization variables

    Returns:
        Objective function value
    """
    pass
```

### `constraints(x)`

Evaluate constraint functions.

Parameters:

| Name | Type      | Description            | Default    |
| ---- | --------- | ---------------------- | ---------- |
| `x`  | `ndarray` | Optimization variables | *required* |

Returns:

| Type      | Description                |
| --------- | -------------------------- |
| `ndarray` | Constraint function values |

Source code in `src/figaroh/tools/robotipopt.py`

```
@abstractmethod
def constraints(self, x: np.ndarray) -> np.ndarray:
    """
    Evaluate constraint functions.

    Args:
        x: Optimization variables

    Returns:
        Constraint function values
    """
    pass
```

### `gradient(x)`

Compute gradient of objective function.

Default implementation uses automatic differentiation. Override for custom implementations.

Parameters:

| Name | Type      | Description            | Default    |
| ---- | --------- | ---------------------- | ---------- |
| `x`  | `ndarray` | Optimization variables | *required* |

Returns:

| Type      | Description     |
| --------- | --------------- |
| `ndarray` | Gradient vector |

Source code in `src/figaroh/tools/robotipopt.py`

```
def gradient(self, x: np.ndarray) -> np.ndarray:
    """
    Compute gradient of objective function.

    Default implementation uses automatic differentiation.
    Override for custom implementations.

    Args:
        x: Optimization variables

    Returns:
        Gradient vector
    """
    try:
        return nd.Gradient(self.objective)(x)
    except Exception as e:
        self.logger.warning(f"Error computing gradient: {e}")
        return np.zeros_like(x)
```

### `jacobian(x)`

Compute Jacobian of constraint functions.

Default implementation uses automatic differentiation. Override for custom implementations.

Parameters:

| Name | Type      | Description            | Default    |
| ---- | --------- | ---------------------- | ---------- |
| `x`  | `ndarray` | Optimization variables | *required* |

Returns:

| Type      | Description     |
| --------- | --------------- |
| `ndarray` | Jacobian matrix |

Source code in `src/figaroh/tools/robotipopt.py`

```
def jacobian(self, x: np.ndarray) -> np.ndarray:
    """
    Compute Jacobian of constraint functions.

    Default implementation uses automatic differentiation.
    Override for custom implementations.

    Args:
        x: Optimization variables

    Returns:
        Jacobian matrix
    """
    try:
        constraints = self.constraints(x)
        if len(constraints) == 0:
            # No constraints
            return np.zeros((0, len(x)))

        jac = nd.Jacobian(self.constraints)(x)

        # Handle case where constraint returns scalar
        if constraints.ndim == 0 or len(constraints) == 1:
            jac = jac.reshape(1, -1)

        self.logger.debug(f"Constraint jacobian shape: {jac.shape}")
        return jac
    except Exception as e:
        self.logger.warning(f"Error computing jacobian: {e}")
        # Return identity matrix as fallback
        constraints = self.constraints(x)
        n_constraints = len(constraints) if hasattr(constraints, "__len__") else 1
        return np.eye(n_constraints, len(x))
```

### `hessian(x, lagrange, obj_factor)`

Compute Hessian of Lagrangian.

Default implementation returns False to use IPOPT's approximation. Override for custom implementations.

Parameters:

| Name         | Type      | Description              | Default    |
| ------------ | --------- | ------------------------ | ---------- |
| `x`          | `ndarray` | Optimization variables   | *required* |
| `lagrange`   | `ndarray` | Lagrange multipliers     | *required* |
| `obj_factor` | `float`   | Objective scaling factor | *required* |

Returns:

| Type                   | Description                                  |
| ---------------------- | -------------------------------------------- |
| `Union[bool, ndarray]` | Hessian matrix or False to use approximation |

Source code in `src/figaroh/tools/robotipopt.py`

```
def hessian(
    self, x: np.ndarray, lagrange: np.ndarray, obj_factor: float
) -> Union[bool, np.ndarray]:
    """
    Compute Hessian of Lagrangian.

    Default implementation returns False to use IPOPT's approximation.
    Override for custom implementations.

    Args:
        x: Optimization variables
        lagrange: Lagrange multipliers
        obj_factor: Objective scaling factor

    Returns:
        Hessian matrix or False to use approximation
    """
    return False
```

### `intermediate(alg_mod, iter_count, obj_value, inf_pr, inf_du, mu, d_norm, regularization_size, alpha_du, alpha_pr, ls_trials)`

Intermediate callback for iteration tracking.

Parameters:

| Name                  | Type    | Description              | Default    |
| --------------------- | ------- | ------------------------ | ---------- |
| `alg_mod`             | `int`   | Algorithm mode           | *required* |
| `iter_count`          | `int`   | Iteration count          | *required* |
| `obj_value`           | `float` | Current objective value  | *required* |
| `inf_pr`              | `float` | Primal infeasibility     | *required* |
| `inf_du`              | `float` | Dual infeasibility       | *required* |
| `mu`                  | `float` | Barrier parameter        | *required* |
| `d_norm`              | `float` | Step size                | *required* |
| `regularization_size` | `float` | Regularization parameter | *required* |
| `alpha_du`            | `float` | Dual step size           | *required* |
| `alpha_pr`            | `float` | Primal step size         | *required* |
| `ls_trials`           | `int`   | Line search trials       | *required* |

Returns:

| Type   | Description                                  |
| ------ | -------------------------------------------- |
| `bool` | True to continue optimization, False to stop |

Source code in `src/figaroh/tools/robotipopt.py`

```
def intermediate(
    self,
    alg_mod: int,
    iter_count: int,
    obj_value: float,
    inf_pr: float,
    inf_du: float,
    mu: float,
    d_norm: float,
    regularization_size: float,
    alpha_du: float,
    alpha_pr: float,
    ls_trials: int,
) -> bool:
    """
    Intermediate callback for iteration tracking.

    Args:
        alg_mod: Algorithm mode
        iter_count: Iteration count
        obj_value: Current objective value
        inf_pr: Primal infeasibility
        inf_du: Dual infeasibility
        mu: Barrier parameter
        d_norm: Step size
        regularization_size: Regularization parameter
        alpha_du: Dual step size
        alpha_pr: Primal step size
        ls_trials: Line search trials

    Returns:
        True to continue optimization, False to stop
    """
    self.iteration_data["iterations"].append(iter_count)
    self.iteration_data["obj_values"].append(obj_value)
    self.iteration_data["constraint_violations"].append(max(inf_pr, inf_du))

    return True
```

## `RobotIPOPTSolver(problem, config=None)`

General IPOPT solver for robotics optimization problems.

This class provides a high-level interface for solving optimization problems using IPOPT, with built-in support for result analysis, error handling, and problem validation specifically designed for robotics applications.

Features

- Automatic problem setup and configuration
- Built-in problem validation before solving
- Comprehensive result analysis and logging
- Solution history tracking for multiple solves
- Robotics-specific error handling and diagnostics
- Support for iterative solving and warm starts

Attributes:

| Name            | Type                      | Description                           |
| --------------- | ------------------------- | ------------------------------------- |
| `problem`       | `BaseOptimizationProblem` | The optimization problem to solve.    |
| `config`        | `IPOPTConfig`             | IPOPT solver configuration.           |
| `logger`        | `Logger`                  | Logger for solver operations.         |
| `last_solution` | `ndarray`                 | Most recent optimization solution.    |
| `last_info`     | `Dict`                    | Most recent IPOPT solver information. |
| `solve_history` | `List[Dict]`              | History of all solve attempts.        |

Examples:

Basic usage:

```
# Create problem and solver
problem = MyOptimizationProblem()
solver = RobotIPOPTSolver(problem)

# Solve the problem
success, results = solver.solve()

if success:
    print(f"Optimal solution: {results['x_opt']}")
    print(f"Objective value: {results['obj_val']}")
```

With custom configuration:

```
# Create custom configuration
config = IPOPTConfig(
    tolerance=1e-8,
    max_iterations=5000,
    print_level=3
)

# Create solver with custom config
solver = RobotIPOPTSolver(problem, config)

# Validate problem before solving
if solver.validate_problem_setup():
    success, results = solver.solve()
```

Multiple solves with warm start:

```
solver = RobotIPOPTSolver(problem)

# First solve
success1, results1 = solver.solve()

# Modify problem parameters...
problem.update_parameters(new_params)

# Second solve (uses warm start automatically)
success2, results2 = solver.solve()

# Access solve history
print(f"Solved {len(solver.solve_history)} problems")
```

Initialize the IPOPT solver.

Parameters:

| Name      | Type                      | Description                                                   | Default    |
| --------- | ------------------------- | ------------------------------------------------------------- | ---------- |
| `problem` | `BaseOptimizationProblem` | Optimization problem to solve                                 | *required* |
| `config`  | `Optional[IPOPTConfig]`   | IPOPT configuration (default: trajectory optimization config) | `None`     |

Source code in `src/figaroh/tools/robotipopt.py`

```
def __init__(
    self, problem: BaseOptimizationProblem, config: Optional[IPOPTConfig] = None
):
    """
    Initialize the IPOPT solver.

    Args:
        problem: Optimization problem to solve
        config: IPOPT configuration (default: trajectory optimization
               config)
    """
    self.problem = problem
    self.config = config or IPOPTConfig.for_trajectory_optimization()
    self.logger = logging.getLogger(f"{__name__}.RobotIPOPTSolver")

    # Results storage
    self.last_solution = None
    self.last_info = None
    self.solve_history = []
```

### `solve()`

Solve the optimization problem.

Performs the complete optimization process including problem setup, IPOPT configuration, solving, and result analysis. The method handles all IPOPT interactions and provides comprehensive error handling and logging.

Returns:

| Type                          | Description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        |
| ----------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `Tuple[bool, Dict[str, Any]]` | Tuple\[bool, Dict[str, Any]\]: A tuple containing: - success (bool): True if optimization succeeded, False otherwise - results (Dict[str, Any]): Dictionary containing: * 'x_opt': Optimal solution vector * 'obj_val': Final objective function value * 'status': IPOPT exit status code * 'status_msg': Human-readable status message * 'solve_time': Total optimization time in seconds * 'iterations': Number of iterations performed * 'iteration_data': Detailed iteration history * 'callback_data': Custom data from problem callbacks * 'ipopt_info': Complete IPOPT solver information * 'success': Copy of success flag for convenience |

Raises:

| Type        | Description                                                                                                       |
| ----------- | ----------------------------------------------------------------------------------------------------------------- |
| `Exception` | If critical errors occur during problem setup or solving. Non-critical errors are caught and returned in results. |

Note

IPOPT status codes for success: 0 (solved) and 1 (solved to acceptable level). All other codes, including -1 (maximum number of iterations exceeded), indicate failure or early termination; the last iterate is still returned in `x_opt`.

Source code in `src/figaroh/tools/robotipopt.py`

```
def solve(self) -> Tuple[bool, Dict[str, Any]]:
    """
    Solve the optimization problem.

    Performs the complete optimization process including problem setup,
    IPOPT configuration, solving, and result analysis. The method
    handles all IPOPT interactions and provides comprehensive error
    handling and logging.

    Returns:
        Tuple[bool, Dict[str, Any]]: A tuple containing:
            - success (bool): True if optimization succeeded,
              False otherwise
            - results (Dict[str, Any]): Dictionary containing:
                * 'x_opt': Optimal solution vector
                * 'obj_val': Final objective function value
                * 'status': IPOPT exit status code
                * 'status_msg': Human-readable status message
                * 'solve_time': Total optimization time in seconds
                * 'iterations': Number of iterations performed
                * 'iteration_data': Detailed iteration history
                * 'callback_data': Custom data from problem callbacks
                * 'ipopt_info': Complete IPOPT solver information
                * 'success': Copy of success flag for convenience

    Raises:
        Exception: If critical errors occur during problem setup
                  or solving. Non-critical errors are caught and
                  returned in results.

    Note:
        IPOPT status codes for success: 0 (solved) and 1 (solved to
        acceptable level). All other codes, including -1 (maximum number
        of iterations exceeded), indicate failure or early termination;
        the last iterate is still returned in ``x_opt``.
    """
    try:
        # Import cyipopt only when needed
        try:
            import cyipopt
        except ImportError:
            raise ImportError(
                "cyipopt is required for IPOPT optimization. "
                "Install with: pip install cyipopt"
            )

        self.logger.info(f"Setting up IPOPT problem: {self.problem.name}")

        # Get problem dimensions and bounds
        x0 = self.problem.get_initial_guess()
        lb, ub = self.problem.get_variable_bounds()
        cl, cu = self.problem.get_constraint_bounds()

        self.logger.info(
            f"Problem dimensions: {len(x0)} variables, " f"{len(cl)} constraints"
        )

        # Create IPOPT problem
        nlp = cyipopt.Problem(
            n=len(x0),
            m=len(cl),
            problem_obj=self.problem,
            lb=lb,
            ub=ub,
            cl=cl,
            cu=cu,
        )

        # Apply configuration
        for key, value in self.config.to_ipopt_options().items():
            nlp.add_option(key, value)

        # Solve optimization
        self.logger.info("Starting IPOPT optimization...")
        start_time = time.time()
        x_opt, info = nlp.solve(x0)
        solve_time = time.time() - start_time

        # Store results
        self.last_solution = x_opt
        self.last_info = info

        # Analyze results
        # Converged IPOPT exit codes; -1 is Maximum_Iterations_Exceeded
        success = info["status"] in [0, 1]
        if info["status"] == -1:
            self.logger.warning(
                "IPOPT stopped at the iteration limit without converging"
            )

        self.logger.info(f"Optimization completed in {solve_time:.2f} seconds")
        self.logger.info(f"Status: {info['status']} - {info['status_msg']}")
        self.logger.info(f"Final objective: {info['obj_val']:.6e}")

        # Prepare results dictionary
        results = {
            "success": success,
            "x_opt": x_opt,
            "obj_val": info["obj_val"],
            "status": info["status"],
            "status_msg": info["status_msg"],
            "solve_time": solve_time,
            "iterations": len(self.problem.iteration_data["iterations"]),
            "iteration_data": self.problem.iteration_data.copy(),
            "callback_data": self.problem.callback_data.copy(),
            "ipopt_info": info,
        }

        # Store in history
        self.solve_history.append(results)

        return success, results

    except Exception as e:
        self.logger.error(f"Error during optimization: {e}")
        return False, {"error": str(e)}
```

### `get_solution_summary()`

Get a summary of the last solution.

Source code in `src/figaroh/tools/robotipopt.py`

```
def get_solution_summary(self) -> str:
    """Get a summary of the last solution."""
    if self.last_solution is None:
        return "No solution available"

    info = self.last_info
    return (
        f"IPOPT Solution Summary:\n"
        f"  Status: {info['status']} - {info['status_msg']}\n"
        f"  Objective: {info['obj_val']:.6e}\n"
        f"  Iterations: {len(self.problem.iteration_data['iterations'])}\n"
        f"  Variables: {len(self.last_solution)}\n"
        f"  Constraints: {len(self.problem.get_constraint_bounds()[0])}\n"
    )
```

### `validate_problem_setup()`

Validate that the optimization problem is properly set up.

Returns:

| Type   | Description                                    |
| ------ | ---------------------------------------------- |
| `bool` | True if problem appears valid, False otherwise |

Source code in `src/figaroh/tools/robotipopt.py`

```
def validate_problem_setup(self) -> bool:
    """
    Validate that the optimization problem is properly set up.

    Returns:
        True if problem appears valid, False otherwise
    """
    try:
        self.logger.info("Validating problem setup...")

        # Check dimensions
        x0 = self.problem.get_initial_guess()
        lb, ub = self.problem.get_variable_bounds()
        cl, cu = self.problem.get_constraint_bounds()

        if len(lb) != len(ub) or len(lb) != len(x0):
            self.logger.error("Variable bounds dimension mismatch")
            return False

        if len(cl) != len(cu):
            self.logger.error("Constraint bounds dimension mismatch")
            return False

        # Test function evaluations
        obj_val = self.problem.objective(np.array(x0))
        if not np.isfinite(obj_val):
            self.logger.error(
                f"Objective function returns non-finite value: {obj_val}"
            )
            return False

        constraints = self.problem.constraints(np.array(x0))
        if len(constraints) != len(cl):
            self.logger.error("Constraint function dimension mismatch")
            return False

        if not np.all(np.isfinite(constraints)):
            self.logger.error("Constraint function returns non-finite values")
            return False

        self.logger.info("Problem validation passed")
        return True

    except Exception as e:
        self.logger.error(f"Problem validation failed: {e}")
        return False
```

## `TrajectoryOptimizationProblem(name='TrajectoryOptimization')`

Bases: `BaseOptimizationProblem`

Specialized optimization problem for trajectory optimization.

This class provides a framework for trajectory optimization problems with common patterns like waypoint parameterization and constraint handling.

Initialize trajectory optimization problem.

Source code in `src/figaroh/tools/robotipopt.py`

```
def __init__(self, name: str = "TrajectoryOptimization"):
    """Initialize trajectory optimization problem."""
    super().__init__(name)

    # Trajectory-specific data
    self.trajectory_data = {
        "times": None,
        "positions": None,
        "velocities": None,
        "accelerations": None,
    }
```

### `set_objective_function(obj_func)`

Set custom objective function.

Source code in `src/figaroh/tools/robotipopt.py`

```
def set_objective_function(self, obj_func: Callable[[np.ndarray], float]):
    """Set custom objective function."""
    self._custom_objective = obj_func
```

### `set_constraint_function(cons_func)`

Set custom constraint function.

Source code in `src/figaroh/tools/robotipopt.py`

```
def set_constraint_function(self, cons_func: Callable[[np.ndarray], np.ndarray]):
    """Set custom constraint function."""
    self._custom_constraints = cons_func
```

### `objective(x)`

Evaluate objective function.

Source code in `src/figaroh/tools/robotipopt.py`

```
def objective(self, x: np.ndarray) -> float:
    """Evaluate objective function."""
    if hasattr(self, "_custom_objective"):
        return self._custom_objective(x)
    else:
        raise NotImplementedError(
            "Must implement objective function or set custom function"
        )
```

### `constraints(x)`

Evaluate constraint functions.

Source code in `src/figaroh/tools/robotipopt.py`

```
def constraints(self, x: np.ndarray) -> np.ndarray:
    """Evaluate constraint functions."""
    if hasattr(self, "_custom_constraints"):
        return self._custom_constraints(x)
    else:
        raise NotImplementedError(
            "Must implement constraints function or set custom function"
        )
```

## `create_trajectory_solver(objective_func, constraint_func, get_bounds_func, initial_guess_func, name='CustomTrajectory')`

Factory function to create a trajectory optimization solver.

Parameters:

| Name                 | Type                                                        | Description                                                  | Default              |
| -------------------- | ----------------------------------------------------------- | ------------------------------------------------------------ | -------------------- |
| `objective_func`     | `Callable[[ndarray], float]`                                | Function to evaluate objective                               | *required*           |
| `constraint_func`    | `Callable[[ndarray], ndarray]`                              | Function to evaluate constraints                             | *required*           |
| `get_bounds_func`    | `Callable[[], Tuple[Tuple[List, List], Tuple[List, List]]]` | Function that returns ((var_lb, var_ub), (cons_lb, cons_ub)) | *required*           |
| `initial_guess_func` | `Callable[[], List[float]]`                                 | Function that returns initial guess                          | *required*           |
| `name`               | `str`                                                       | Problem name                                                 | `'CustomTrajectory'` |

Returns:

| Type               | Description             |
| ------------------ | ----------------------- |
| `RobotIPOPTSolver` | Configured IPOPT solver |

Source code in `src/figaroh/tools/robotipopt.py`

```
def create_trajectory_solver(
    objective_func: Callable[[np.ndarray], float],
    constraint_func: Callable[[np.ndarray], np.ndarray],
    get_bounds_func: Callable[[], Tuple[Tuple[List, List], Tuple[List, List]]],
    initial_guess_func: Callable[[], List[float]],
    name: str = "CustomTrajectory",
) -> RobotIPOPTSolver:
    """
    Factory function to create a trajectory optimization solver.

    Args:
        objective_func: Function to evaluate objective
        constraint_func: Function to evaluate constraints
        get_bounds_func: Function that returns ((var_lb, var_ub),
                        (cons_lb, cons_ub))
        initial_guess_func: Function that returns initial guess
        name: Problem name

    Returns:
        Configured IPOPT solver
    """

    class CustomProblem(TrajectoryOptimizationProblem):
        def __init__(self):
            super().__init__(name)
            self.set_objective_function(objective_func)
            self.set_constraint_function(constraint_func)
            self._get_bounds = get_bounds_func
            self._get_initial = initial_guess_func

        def get_variable_bounds(self):
            return self._get_bounds()[0]

        def get_constraint_bounds(self):
            return self._get_bounds()[1]

        def get_initial_guess(self):
            return self._get_initial()

    problem = CustomProblem()
    config = IPOPTConfig.for_trajectory_optimization()
    return RobotIPOPTSolver(problem, config)
```

## Random Data Generation

## `generate_waypoints(N, robot, mlow, mhigh)`

Generate random values for joint positions, velocities, accelerations.

Parameters:

| Name    | Type | Description                   | Default    |
| ------- | ---- | ----------------------------- | ---------- |
| `N`     |      | Number of samples             | *required* |
| `robot` |      | Robot model object            | *required* |
| `mlow`  |      | Lower bound for random values | *required* |
| `mhigh` |      | Upper bound for random values | *required* |

Returns:

| Name    | Type | Description                                             |
| ------- | ---- | ------------------------------------------------------- |
| `tuple` |      | (q, v, a) joint position, velocity, acceleration arrays |

Source code in `src/figaroh/tools/randomdata.py`

```
def generate_waypoints(N, robot, mlow, mhigh):
    """Generate random values for joint positions, velocities, accelerations.

    Args:
        N: Number of samples
        robot: Robot model object
        mlow: Lower bound for random values
        mhigh: Upper bound for random values

    Returns:
        tuple: (q, v, a) joint position, velocity, acceleration arrays
    """
    has_backend = hasattr(robot, "backend")
    nq = robot.backend.nq if has_backend else robot.model.nq
    nv = robot.backend.nv if has_backend else robot.model.nv
    q = np.empty((1, nq))
    v = np.empty((1, nv))
    a = np.empty((1, nv))
    for i in range(N):
        q = np.vstack((q, np.random.uniform(low=mlow, high=mhigh, size=(nq,))))
        v = np.vstack((v, np.random.uniform(low=mlow, high=mhigh, size=(nv,))))
        a = np.vstack((a, np.random.uniform(low=mlow, high=mhigh, size=(nv,))))
    return q, v, a
```

## `generate_waypoints_fext(N, robot, mlow, mhigh)`

Generate random values for joints with external forces.

Parameters:

| Name    | Type | Description                   | Default    |
| ------- | ---- | ----------------------------- | ---------- |
| `N`     |      | Number of samples             | *required* |
| `robot` |      | Robot model object            | *required* |
| `mlow`  |      | Lower bound for random values | *required* |
| `mhigh` |      | Upper bound for random values | *required* |

Returns:

| Name    | Type | Description                                             |
| ------- | ---- | ------------------------------------------------------- |
| `tuple` |      | (q, v, a) joint position, velocity, acceleration arrays |

Source code in `src/figaroh/tools/randomdata.py`

```
def generate_waypoints_fext(N, robot, mlow, mhigh):
    """Generate random values for joints with external forces.

    Args:
        N: Number of samples
        robot: Robot model object
        mlow: Lower bound for random values
        mhigh: Upper bound for random values

    Returns:
        tuple: (q, v, a) joint position, velocity, acceleration arrays
    """
    has_backend = hasattr(robot, "backend")
    nq = robot.backend.nq if has_backend else robot.model.nq
    nv = robot.backend.nv if has_backend else robot.model.nv
    q0 = robot.q0[:7]
    v0 = np.zeros(6)
    a0 = np.zeros(6)
    q = np.empty((1, nq))
    v = np.empty((1, nv))
    a = np.empty((1, nv))
    for i in range(N):
        q_ = np.append(q0, np.random.uniform(low=mlow, high=mhigh, size=(nq - 7,)))
        q = np.vstack((q, q_))

        v_ = np.append(v0, np.random.uniform(low=mlow, high=mhigh, size=(nv - 6,)))
        v = np.vstack((v, v_))

        a_ = np.append(a0, np.random.uniform(low=mlow, high=mhigh, size=(nv - 6,)))
        a = np.vstack((a, a_))
    return q, v, a
```

## `get_torque_rand(N, robot, q, v, a, identif_config)`

Calculate random torque values including various dynamic effects.

Parameters:

| Name             | Type | Description                      | Default    |
| ---------------- | ---- | -------------------------------- | ---------- |
| `N`              |      | Number of samples                | *required* |
| `robot`          |      | Robot model object               | *required* |
| `q`              |      | Joint positions array            | *required* |
| `v`              |      | Joint velocities array           | *required* |
| `a`              |      | Joint accelerations array        | *required* |
| `identif_config` |      | Dictionary of dynamic parameters | *required* |

Returns:

| Name    | Type | Description         |
| ------- | ---- | ------------------- |
| `array` |      | Joint torques array |

Source code in `src/figaroh/tools/randomdata.py`

```
def get_torque_rand(N, robot, q, v, a, identif_config):
    """Calculate random torque values including various dynamic effects.

    Args:
        N: Number of samples
        robot: Robot model object
        q: Joint positions array
        v: Joint velocities array
        a: Joint accelerations array
        identif_config: Dictionary of dynamic parameters

    Returns:
        array: Joint torques array
    """
    has_backend = hasattr(robot, "backend")
    nv = robot.backend.nv if has_backend else robot.model.nv
    tau = np.zeros(nv * N)
    for i in range(N):
        if has_backend:
            tau_vec = robot.backend.compute_inverse_dynamics(q[i, :], v[i, :], a[i, :])
        else:
            tau_vec = pin.rnea(robot.model, robot.data, q[i, :], v[i, :], a[i, :])
        for j in range(nv):
            tau[j * N + i] = tau_vec[j]
    if identif_config["has_friction"]:
        for i in range(N):
            for j in range(nv):
                fv_term = v[i, j] * identif_config["fv"][j]
                fs_term = np.sign(v[i, j]) * identif_config["fs"][j]
                tau[j * N + i] += fv_term + fs_term
    if identif_config["has_actuator_inertia"]:
        for i in range(N):
            for j in range(nv):
                tau[j * N + i] += identif_config["Ia"][j] * a[i, j]
    if identif_config["has_joint_offset"]:
        for i in range(N):
            for j in range(nv):
                tau[j * N + i] += identif_config["off"][j]
    if identif_config["has_coupled_wrist"]:
        for i in range(N):
            for j in range(nv):
                if j == nv - 2:
                    tau[j * N + i] += (
                        identif_config["Iam6"] * v[i, nv - 1]
                        + identif_config["fvm6"] * v[i, nv - 1]
                        + identif_config["fsm6"] * np.sign(v[i, nv - 2] + v[i, nv - 1])
                    )
                if j == nv - 1:
                    tau[j * N + i] += (
                        identif_config["Iam6"] * v[i, nv - 2]
                        + identif_config["fvm6"] * v[i, nv - 2]
                        + identif_config["fsm6"] * np.sign(v[i, nv - 2] + v[i, nv - 1])
                    )
    return tau
```

## URDF Export

Applies calibrated joint parameters back onto a URDF file.

URDF exporter for identified/calibrated robot parameters.

Reads a nominal URDF and produces a modified URDF by applying parameter overlays. Parameter names encode the semantics: `d_px_joint2` is additive (geometric offset), `m_link1` is absolute (mass override).

This is used after identification or calibration to materialize results into a tangible URDF file for downstream use (simulation, visualization, model-based control).

**Two parameter categories**

Joint-level parameters (auto-applied to the URDF): These directly modify the URDF's joint origins, link inertias, and dynamics attributes. They come from figaroh's identification or calibration solvers and can be applied automatically:

```
- Joint placement (in the joint frame): ``d_px_{joint}``,
  ``d_py_{joint}``, ``d_pz_{joint}``, ``d_phix_{joint}``,
  ``d_phiy_{joint}``, ``d_phiz_{joint}``; a joint's six values are
  composed as ``origin * SE3(exp3(d_phi), d_p)``
- Joint offset / calibration (additive): ``offsetPX_{joint}``,
  ``offsetPY_{joint}``, ``offsetPZ_{joint}``, ``offsetRX_{joint}``,
  ``offsetRY_{joint}``, ``offsetRZ_{joint}``
- Mass (absolute): ``m_{link}``
- Standard inertial parameters (absolute, all ten per link):
  ``m_``, ``mx_``, ``my_``, ``mz_``, ``Ixx_``, ``Ixy_``, ``Iyy_``,
  ``Ixz_``, ``Iyz_``, ``Izz_`` + ``{link}``; see
  :func:`_apply_standard_inertial` for the conventions
- Viscous/static friction (absolute): ``fv_{joint}``, ``fs_{joint}``
- Armature (absolute): ``Ia_{joint}``
- Joint elasticity (additive): ``k_PX_{joint}``, ..., ``k_RZ_{joint}``,
  stored as a custom ``<dynamics elasticity>`` attribute that URDF
  parsers ignore: it does not change the reloaded kinematics

Inertial targets (``m_`` ... ``Izz_``) name a link, or a moving joint
as identification does (``get_standard_parameters`` keys by Pinocchio
joint); a joint resolves to its child link.

Refused (``ValueError``, nothing written): legacy ``off_{joint}``; names
targeting a joint or link the URDF does not have; a joint carrying both
``offset*`` and ``d_*``; a partial inertial set (first moments or tensor
entries without all ten parameters); a joint target whose child link has
massive links attached by fixed joints (Pinocchio merges them into the
joint's body, so the estimate does not belong to one URDF link); a set
with ``m <= 0``; and, unless ``allow_infeasible=True``, a set that is not
physically consistent.
```

Metrology frame parameters (user-defined, not auto-applied): These define the transformation between the robot (URDF) and the external measurement system (mocap, camera, chessboard, etc.). They depend on the **calibration setup**, not on the URDF itself. `export_urdf()` will **not** auto-apply them; instead it logs a reminder and returns them as metadata for the user to configure::

```
    base_px, base_py, base_pz, base_phix, base_phiy, base_phiz
        Transform from the **metrology frame** (e.g. mocap world,
        Vicon origin) to the robot's ``base_link``.
        Default: identity (no offset from origin).

    pEEx_{frame}, pEEy_{frame}, pEEz_{frame}
    phiEEx_{frame}, phiEEy_{frame}, phiEEz_{frame}
        Transform from the last robot joint (e.g. ``arm_7_joint``,
        ``head_2_link``) to the **measurement frame** mounted on the
        end-effector. What this frame is depends on calibration type:

        - **Mocap calibration**: optical marker cluster frame
          (markers attached to the end-effector).
        - **Eye-hand calibration**: camera optical frame
          (e.g. ``xtion_rgb_optical_frame``) or chessboard frame
          (pattern on the gripper).
        Default: identity (measurement frame coincides with the joint).
```

Typical usage::

```
from figaroh.tools.urdf_exporter import export_urdf, frame_settings_doc

# Joint-level params (auto-applied to URDF)
params = {
    "d_px_joint2": 0.05,
    "m_link1": 2.5,
    "fv_joint1": 0.2,
}
modified_path = export_urdf("robot.urdf", params, verbose=True)

# Metrology frames (user-defined — see frame_settings_doc())
defaults = frame_settings_doc()
# → prints descriptions + default values for base and EE frame params
```

## `is_kinematic_correction(name)`

Whether `name` is a joint correction that changes the reloaded kinematics: a `full_params` placement (`d_*`) or a joint offset (`offset*`). Frame, elastic and dynamic parameters are not.

Source code in `src/figaroh/tools/urdf_exporter.py`

```
def is_kinematic_correction(name: str) -> bool:
    """Whether ``name`` is a joint correction that changes the reloaded
    kinematics: a ``full_params`` placement (``d_*``) or a joint offset
    (``offset*``). Frame, elastic and dynamic parameters are not."""
    parsed = _parse_param_name(name)
    return parsed is not None and parsed[0] in ("joint_placement", "joint_offset")
```

## `frame_settings_doc(*, calibration_type=None, verbose=True)`

Return default metrology-frame parameter values with explanations.

These parameters define the transformation between the robot and the external measurement system. They are **not** intrinsic to the URDF and must be configured by the user for each calibration setup.

Parameters:

| Name               | Type            | Description                                                                          | Default |
| ------------------ | --------------- | ------------------------------------------------------------------------------------ | ------- |
| `calibration_type` | `Optional[str]` | Optional hint for context-specific defaults. "mocap", "eye_hand", or None (generic). | `None`  |
| `verbose`          | `bool`          | If True (default), prints descriptions to stderr.                                    | `True`  |

Returns:

| Type   | Description                                                                                                           |
| ------ | --------------------------------------------------------------------------------------------------------------------- |
| `dict` | dict with default values for all base-frame and EE-frame params:: { "base_px": 0.0, ... "pEEx_arm_7_link": 0.0, ... } |

Source code in `src/figaroh/tools/urdf_exporter.py`

```
def frame_settings_doc(
    *, calibration_type: Optional[str] = None, verbose: bool = True
) -> dict:
    """Return default metrology-frame parameter values with explanations.

    These parameters define the transformation between the robot and the
    external measurement system.  They are **not** intrinsic to the URDF
    and must be configured by the user for each calibration setup.

    Args:
        calibration_type: Optional hint for context-specific defaults.
            ``"mocap"``, ``"eye_hand"``, or ``None`` (generic).
        verbose: If True (default), prints descriptions to stderr.

    Returns:
        dict with default values for all base-frame and EE-frame params::

            {
                "base_px": 0.0, ...
                "pEEx_arm_7_link": 0.0, ...
            }
    """
    defaults: dict = {}

    # Base frame defaults — identity (metrology origin = robot base)
    for name, _, _, _ in _METROLOGY_REGISTRY:
        if name.startswith("base_"):
            defaults[name] = 0.0

    # EE frame defaults — identity (sensor/marker frame = last joint)
    if verbose:
        if calibration_type == "mocap":
            target = "arm_7_link"
            note = (
                "Mocap calibration: EE measurement frame is the optical marker "
                "cluster attached to the end-effector (e.g. arm_7_link)."
            )
        elif calibration_type == "eye_hand":
            target = "head_2_link"
            note = (
                "Eye-hand calibration: EE measurement frame is the camera "
                "optical frame (relative to head_2_link) or the chessboard "
                "attached to the gripper."
            )
        else:
            target = "<frame_name>"
            note = (
                "EE measurement frame: typically a marker cluster, camera "
                "optical frame, or chessboard attached to the end-effector.  "
                "Replace ``<frame_name>`` with the actual robot joint/link name."
            )
        logger.info(
            "Metrology frame defaults (see frame_settings_doc()):\n"
            "  Base frame : metrology origin → robot base_link\n"
            "              default = identity (no offset)\n"
            "  EE frame   : last joint → measurement frame\n"
            "              default = identity\n"
            "  %s\n"
            "  To customize, pass e.g. pEEx_%s = <value> to export_urdf() "
            "and configure your\n"
            "  controller or calibration pipeline accordingly.",
            note,
            target,
        )

    return defaults
```

## `export_urdf(nominal_urdf_path, params, *, output_path=None, verbose=False, allow_infeasible=False, merged_bodies='refuse', psd_eig_tol=-1e-10)`

Apply identified/calibrated **joint-level** parameters to a nominal URDF.

This function **auto-applies** parameters that modify the URDF directly (joint placements, mass, inertias, friction, etc. — see module docstring). It does **not** apply metrology frame parameters (`base_*`, `pEE*`, `phiEE*`); those depend on the calibration setup and must be configured by the user — see :func:`frame_settings_doc`.

Parameters:

| Name                | Type                         | Description                                                                                                                                                                                                                                                                                                                                                     | Default    |
| ------------------- | ---------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `nominal_urdf_path` | `Union[str, Path]`           | Path to the nominal (reference) URDF file.                                                                                                                                                                                                                                                                                                                      | *required* |
| `params`            | `dict`                       | Dictionary of {parameter_name: value} pairs. Joint-level params are applied automatically. Metrology frame params (base\_\*, pEE\*, phiEE\*) are logged and collected for the caller but not auto-applied to the URDF — see :func:frame_settings_doc.                                                                                                           | *required* |
| `output_path`       | `Optional[Union[str, Path]]` | Path for the modified URDF. If None (default), writes to <stem>\_modified.urdf beside the nominal URDF.                                                                                                                                                                                                                                                         | `None`     |
| `verbose`           | `bool`                       | If True, log which params were applied.                                                                                                                                                                                                                                                                                                                         | `False`    |
| `allow_infeasible`  | `bool`                       | Write a complete inertial set whose pseudo-inertia is not positive semidefinite, with a warning, instead of raising.                                                                                                                                                                                                                                            | `False`    |
| `merged_bodies`     | `str`                        | What to do when an inertial set names a joint whose Pinocchio body also holds massive links attached by fixed joints. "refuse" (default) raises. "subtract_fixed" writes the child link p10_est - p10_fixed_nominal (the fixed links' nominal share, from the nominal model, removed; they keep their CAD values) and refuses a remainder that is not physical. | `'refuse'` |
| `psd_eig_tol`       | `float`                      | Smallest pseudo-inertia eigenvalue still accepted as feasible. Pass the tolerance the estimate was accepted with, so an estimate accepted upstream is not refused here.                                                                                                                                                                                         | `-1e-10`   |

Returns:

| Type  | Description                                                         |
| ----- | ------------------------------------------------------------------- |
| `str` | Absolute path to the modified URDF file (joint params applied). Use |
| `str` | func:frame_settings_doc to configure metrology frame params         |
| `str` | separately.                                                         |

Raises:

| Type                | Description                                                                                                                                                                                                                                                                                                                                                          |
| ------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| `FileNotFoundError` | If nominal_urdf_path does not exist.                                                                                                                                                                                                                                                                                                                                 |
| `ValueError`        | If a parameter name is unknown, names a joint or link the URDF does not have, belongs to a category the exporter does not write (legacy off\_\*), a joint carries both offset\* and d\_\* corrections, or an inertial set is partial, targets a merged Pinocchio body, has m \<= 0 or (unless allow_infeasible) is physically inconsistent. Nothing is written then. |

Source code in `src/figaroh/tools/urdf_exporter.py`

```
def export_urdf(
    nominal_urdf_path: Union[str, Path],
    params: dict,
    *,
    output_path: Optional[Union[str, Path]] = None,
    verbose: bool = False,
    allow_infeasible: bool = False,
    merged_bodies: str = "refuse",
    psd_eig_tol: float = -1e-10,
) -> str:
    """Apply identified/calibrated **joint-level** parameters to a nominal URDF.

    This function **auto-applies** parameters that modify the URDF directly
    (joint placements, mass, inertias, friction, etc. — see module docstring).
    It does **not** apply metrology frame parameters (``base_*``, ``pEE*``,
    ``phiEE*``); those depend on the calibration setup and must be
    configured by the user — see :func:`frame_settings_doc`.

    Args:
        nominal_urdf_path: Path to the nominal (reference) URDF file.
        params: Dictionary of ``{parameter_name: value}`` pairs.
            **Joint-level params** are applied automatically.
            **Metrology frame params** (``base_*``, ``pEE*``, ``phiEE*``)
            are logged and collected for the caller but **not** auto-applied
            to the URDF — see :func:`frame_settings_doc`.
        output_path: Path for the modified URDF. If ``None`` (default),
            writes to ``<stem>_modified.urdf`` beside the nominal URDF.
        verbose: If True, log which params were applied.
        allow_infeasible: Write a complete inertial set whose pseudo-inertia
            is not positive semidefinite, with a warning, instead of raising.
        merged_bodies: What to do when an inertial set names a joint whose
            Pinocchio body also holds massive links attached by fixed joints.
            ``"refuse"`` (default) raises. ``"subtract_fixed"`` writes the
            child link ``p10_est - p10_fixed_nominal`` (the fixed links'
            nominal share, from the nominal model, removed; they keep their
            CAD values) and refuses a remainder that is not physical.
        psd_eig_tol: Smallest pseudo-inertia eigenvalue still accepted as
            feasible. Pass the tolerance the estimate was accepted with, so
            an estimate accepted upstream is not refused here.

    Returns:
        Absolute path to the modified URDF file (joint params applied).  Use
        :func:`frame_settings_doc` to configure metrology frame params
        separately.

    Raises:
        FileNotFoundError: If *nominal_urdf_path* does not exist.
        ValueError: If a parameter name is unknown, names a joint or link
            the URDF does not have, belongs to a category the exporter does
            not write (legacy ``off_*``), a joint carries both ``offset*``
            and ``d_*`` corrections, or an inertial set is partial, targets
            a merged Pinocchio body, has ``m <= 0`` or (unless
            *allow_infeasible*) is physically inconsistent. Nothing is
            written then.
    """
    if merged_bodies not in ("refuse", "subtract_fixed"):
        raise ValueError(
            f"merged_bodies must be 'refuse' or 'subtract_fixed', "
            f"not {merged_bodies!r}"
        )
    nominal_path = Path(nominal_urdf_path)
    if not nominal_path.exists():
        raise FileNotFoundError(f"URDF not found: {nominal_path}")

    # Determine output path
    if output_path is None:
        output_path = nominal_path.with_stem(nominal_path.stem + "_modified")
    output_path = Path(output_path)

    # Parse URDF
    tree = ET.parse(str(nominal_path))
    doc = tree.getroot()

    # Separate joint params (auto-apply) from frame params (user-defined)
    frame_params: dict = {}
    placements: dict = {}
    inertials: dict = {}
    offsets: set = set()

    for name, value in params.items():
        parsed = _parse_param_name(name)
        if parsed is not None:
            category, target, idx, is_additive = parsed
            if category == "joint_placement":
                placements.setdefault(target, np.zeros(6))[idx] += value
                if verbose:
                    logger.info("%s → joint_placement.%s (%.4f)", name, target, value)
                continue
            if category in _INERTIAL_CATEGORIES:
                inertials.setdefault(target, {})[_inertial_key(name, target)] = value
                if verbose:
                    logger.info("%s → inertial.%s (%.4f)", name, target, value)
                continue
            handler = _HANDLERS.get(category)
            if handler is None:
                raise ValueError(
                    f"Parameter '{name}': the exporter does not write "
                    f"{_UNSUPPORTED.get(category, category)}"
                )
            if category == "joint_offset":
                offsets.add(target)
            if verbose:
                action = "additive" if is_additive else "absolute"
                logger.info(
                    "%s → %s.%s %s (%.4f)", name, category, target, action, value
                )
            handler(doc, target, idx, value, is_additive)
            continue

        # Check if it's a metrology frame param (base_*, pEE*, phiEE*)
        frame_parsed = _parse_frame_param_name(name)
        if frame_parsed is not None:
            cat, f_target, idx = frame_parsed
            frame_params[name] = value
            desc = _FRAME_PARAM_DESCRIPTIONS.get(cat, cat)
            logger.info(
                "Metrology frame param '%s' = %.4f — not auto-applied to URDF.  "
                "This defines: %s  See frame_settings_doc() for defaults and "
                "explanations.",
                name,
                value,
                desc,
            )
            continue

        raise ValueError(
            f"Unknown parameter '{name}'. "
            f"Recognized joint-level categories: joint placement (d_px_*), "
            f"joint offset (offsetRX_*), mass (m_*), friction (fv_*, fs_*), "
            f"armature (Ia_*), elasticity (k_*), inertial (mx_*, Ixx_*).  "
            f"Metrology frame params: base_*, pEE*, phiEE*."
        )

    both = sorted(offsets & set(placements))
    if both:
        # the fit never produces both; their order would be a guess
        raise ValueError(
            f"Joint(s) {both} carry both offset* and d_* corrections; "
            f"use one calibration level per joint"
        )
    for target, xyz_rpy in placements.items():
        _apply_joint_placement(doc, target, xyz_rpy)
    _apply_inertials(
        doc, inertials, allow_infeasible, merged_bodies, nominal_path, psd_eig_tol
    )

    # Write output
    output_path.parent.mkdir(parents=True, exist_ok=True)
    tree.write(str(output_path), xml_declaration=True, encoding="utf-8")

    if frame_params and verbose:
        logger.info(
            "The following metrology frame parameters were **not** applied "
            "to the URDF:\n  %s\nUse frame_settings_doc() to review defaults.",
            "  ".join(f"{k}={v}" for k, v in frame_params.items()),
        )

    return str(output_path.resolve())
```

## Geometric Calibration Export

Produces the PAL Robotics `robot_state_publisher.geometric_calibration` runtime-correction YAML (e.g. `/etc/calibration/master_calibration.yaml`) from a solved calibrator's redistributed parameters — a different deploy target from URDF Export above (a small runtime overlay applied on top of the original URDF, not a modified URDF file).

PAL Robotics `robot_state_publisher` geometric-calibration deploy file.

Produces the runtime joint-correction YAML PAL robots (TIAGo, TIAGo Pro, TALOS, ...) read at `/etc/calibration/master_calibration.yaml` (`robot_state_publisher: geometric_calibration: {<joint>_<axis>: value}`), directly from a solved `BaseCalibration`.

This is a different deploy target from :mod:`figaroh.tools.urdf_exporter`: that module bakes corrections into a *modified URDF file*; this one produces a small *runtime correction overlay* PAL's `robot_state_publisher` applies on top of the original, unmodified URDF at startup. Both read the same `d_px_{joint}`-style (`full_params`) and `offsetRZ_{joint}`-style (`joint_offset`) parameter names — this module reuses :func:`figaroh.tools.urdf_exporter._parse_param_name` rather than re-deriving that parsing. The keys are additive deltas on the URDF `<origin>` xyz and rpy as written; pass `nominal_urdf` so they are exact (figaroh-plus#123).

The values come from :meth:`~figaroh.calibration.base_calibration.BaseCalibration.redistribute_parameters`: for the `structural` method, the weighted minimum-norm lift of the fitted base parameters onto every joint (figaroh-plus#111), so a joint whose correction was represented by another parameter in the fit still gets its share; for the other estimation methods, the fitted joint parameters. Use :meth:`~figaroh.calibration.base_calibration.BaseCalibration.joint_corrections` to write the same values into a URDF.

## `build_geometric_calibration(calibrator, *, min_sigma=None, nominal_urdf=None, allow_failed_fit=False)`

Build a PAL `robot_state_publisher.geometric_calibration` dict from a solved `BaseCalibration` instance's redistributed parameters.

Per-joint kinematic corrections are included: `full_params` placements (`d_px_{joint}` etc.) and `joint_offset` offsets (`offsetRZ_{joint}` etc., figaroh-plus#123), both converted to additive deltas on the joint's URDF origin. Marker/tip, base-frame and elasticity parameters are not kinematic joint corrections in :func:`~figaroh.tools.urdf_exporter._parse_param_name`'s registry and are dropped by the category check. The base frame needs no exclusion: the lift holds the base-frame rows at 0, so every lifted value is a joint correction.

Parameters:

| Name               | Type              | Description                                                                                                                                                                                                                                                   | Default    |
| ------------------ | ----------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `calibrator`       |                   | A solved BaseCalibration instance (solve() already called).                                                                                                                                                                                                   | *required* |
| `min_sigma`        | `Optional[float]` | If given, only include parameters with                                                                                                                                                                                                                        | value      |
| `nominal_urdf`     | `Optional[str]`   | The URDF the robot runs, to which the deltas are added. Its <origin> values, as written, are the reference (figaroh-plus#123). Without it they are recovered from calibrator.model, which is exact unless an origin's rpy is at or near pitch = +-pi/2 or has | pitch      |
| `allow_failed_fit` | `bool`            | export although the fit stage failed, for diagnosis only (warned and recorded; see BaseCalibration.require_exportable).                                                                                                                                       | `False`    |

Returns:

| Type                                     | Description                                                        |
| ---------------------------------------- | ------------------------------------------------------------------ |
| `Dict[str, Dict[str, Dict[str, float]]]` | {"robot_state_publisher": {"geometric_calibration": {key: value}}} |

Raises:

| Type               | Description                                                                                                                                    |
| ------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------- |
| `CalibrationError` | The fit stage failed (export is recorded as failed), or propagated from redistribute_parameters() if solve()/ create_param_list() haven't run. |

Source code in `src/figaroh/tools/geometric_calibration_export.py`

```
def build_geometric_calibration(
    calibrator,
    *,
    min_sigma: Optional[float] = None,
    nominal_urdf: Optional[str] = None,
    allow_failed_fit: bool = False,
) -> Dict[str, Dict[str, Dict[str, float]]]:
    """Build a PAL ``robot_state_publisher.geometric_calibration`` dict
    from a solved ``BaseCalibration`` instance's redistributed parameters.

    Per-joint kinematic corrections are included: ``full_params``
    placements (``d_px_{joint}`` etc.) and ``joint_offset`` offsets
    (``offsetRZ_{joint}`` etc., figaroh-plus#123), both converted to
    additive deltas on the joint's URDF origin. Marker/tip, base-frame and
    elasticity parameters are not kinematic joint corrections in
    :func:`~figaroh.tools.urdf_exporter._parse_param_name`'s registry and
    are dropped by the category check. The base frame needs no exclusion:
    the lift holds the base-frame rows at 0, so every lifted value is a joint
    correction.

    Args:
        calibrator: A solved ``BaseCalibration`` instance (``solve()``
            already called).
        min_sigma: If given, only include parameters with
            ``|value| / std_dev >= min_sigma``: a conservative deploy
            that leaves statistically insignificant corrections at
            nominal. ``None`` (default) includes every joint-placement
            parameter. Whether it generalizes better is robot- and
            data-specific; check on held-out postures.
        nominal_urdf: The URDF the robot runs, to which the deltas are
            added. Its ``<origin>`` values, as written, are the reference
            (figaroh-plus#123). Without it they are recovered from
            ``calibrator.model``, which is exact unless an origin's rpy is
            at or near pitch = +-pi/2 or has |pitch| > pi/2.
        allow_failed_fit: export although the ``fit`` stage ``failed``,
            for diagnosis only (warned and recorded; see
            ``BaseCalibration.require_exportable``).

    Returns:
        ``{"robot_state_publisher": {"geometric_calibration": {key: value}}}``

    Raises:
        CalibrationError: The ``fit`` stage ``failed`` (``export`` is
            recorded as failed), or propagated from
            ``redistribute_parameters()`` if ``solve()``/
            ``create_param_list()`` haven't run.
    """
    require = getattr(calibrator, "require_exportable", None)
    if require is not None:
        require(allow_failed_fit)
    redistributed = calibrator.redistribute_parameters()

    corrections: Dict[str, np.ndarray] = {}
    offsets = set()
    for name, info in redistributed.items():
        parsed = _parse_param_name(name)
        # joint_offset: same joint-frame transform as the matching d_* (#123)
        if parsed is None or parsed[0] not in ("joint_placement", "joint_offset"):
            continue
        category, target, sub_idx, _ = parsed

        value, std_dev = info["value"], info["std_dev"]
        if min_sigma is not None:
            sigma = abs(value) / std_dev if std_dev > 0 else float("inf")
            if sigma < min_sigma:
                continue

        corrections.setdefault(target, np.zeros(6))[sub_idx] = value
        if category == "joint_offset":
            offsets.add(target)

    model = getattr(calibrator, "model", None)
    origins = _urdf_origins(nominal_urdf) if nominal_urdf is not None else {}
    geometric_calibration: Dict[str, float] = {}
    for target, xyz_rpy in corrections.items():
        placement, rpy = pin.SE3.Identity(), None
        if target in origins:
            xyz, rpy = origins[target]
            placement = pin.SE3(pin.rpy.rpyToMatrix(rpy), xyz)
        elif nominal_urdf is not None:
            raise ValueError(f"Joint '{target}' not found in {nominal_urdf}")
        elif model is not None and model.existJointName(target):
            placement = _urdf_origin(model, target)
        delta = _origin_delta(placement, xyz_rpy, offset=target in offsets, rpy=rpy)
        for sub_idx, value in enumerate(delta):
            if abs(value) > 1e-12:
                key = f"{_pal_joint_name(target)}_{_AXIS_SUFFIX[sub_idx]}"
                geometric_calibration[key] = float(value)

    return {"robot_state_publisher": {"geometric_calibration": geometric_calibration}}
```

## `export_geometric_calibration_yaml(calibrator, output_path, *, min_sigma=None, header_comment=None, nominal_urdf=None, allow_failed_fit=False)`

:func:`build_geometric_calibration` + write as YAML, PAL deploy-ready.

PAL's `master_calibration.yaml` layout (`robot_state_publisher: geometric_calibration:` with `<joint>_<axis>` keys), so the output drops in at `/etc/calibration/master_calibration.yaml` on a PAL robot.

Parameters:

| Name               | Type              | Description                                                                                                               | Default    |
| ------------------ | ----------------- | ------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `calibrator`       |                   | A solved BaseCalibration instance.                                                                                        | *required* |
| `output_path`      | `str`             | Destination YAML file path.                                                                                               | *required* |
| `min_sigma`        | `Optional[float]` | See :func:build_geometric_calibration.                                                                                    | `None`     |
| `nominal_urdf`     | `Optional[str]`   | See :func:build_geometric_calibration; pass it.                                                                           | `None`     |
| `header_comment`   | `Optional[str]`   | Optional single-line comment written above the YAML document (e.g. source data file, sample count, RMSE), for provenance. | `None`     |
| `allow_failed_fit` | `bool`            | See :func:build_geometric_calibration.                                                                                    | `False`    |

Returns:

| Type  | Description                           |
| ----- | ------------------------------------- |
| `str` | output_path, unchanged, for chaining. |

Source code in `src/figaroh/tools/geometric_calibration_export.py`

```
def export_geometric_calibration_yaml(
    calibrator,
    output_path: str,
    *,
    min_sigma: Optional[float] = None,
    header_comment: Optional[str] = None,
    nominal_urdf: Optional[str] = None,
    allow_failed_fit: bool = False,
) -> str:
    """:func:`build_geometric_calibration` + write as YAML, PAL deploy-ready.

    PAL's ``master_calibration.yaml`` layout (``robot_state_publisher:
    geometric_calibration:`` with ``<joint>_<axis>`` keys), so the output
    drops in at ``/etc/calibration/master_calibration.yaml`` on a PAL robot.

    Args:
        calibrator: A solved ``BaseCalibration`` instance.
        output_path: Destination YAML file path.
        min_sigma: See :func:`build_geometric_calibration`.
        nominal_urdf: See :func:`build_geometric_calibration`; pass it.
        header_comment: Optional single-line comment written above the
            YAML document (e.g. source data file, sample count, RMSE), for
            provenance.
        allow_failed_fit: See :func:`build_geometric_calibration`.

    Returns:
        ``output_path``, unchanged, for chaining.
    """
    data = build_geometric_calibration(
        calibrator,
        min_sigma=min_sigma,
        nominal_urdf=nominal_urdf,
        allow_failed_fit=allow_failed_fit,
    )
    with open(output_path, "w") as f:
        if header_comment:
            f.write(f"# {header_comment}\n")
        yaml.dump(data, f, sort_keys=True, default_flow_style=False)
    logger.info("Geometric calibration written to %s", output_path)
    return output_path
```

## Export Validation

FK consistency checks and interactive viser visualization for a calibration-exported URDF.

URDF model comparison and validation utilities.

Provides tools to compare two URDF models (nominal vs. modified after parameter application) through forward-kinematics analysis and viser-based visualization.

Typical usage::

```
from figaroh.tools.export_validation import URDFComparison

comp = URDFComparison("robot.urdf", "robot_modified.urdf")
print(comp.fk_consistency_check())
# → FkConsistencyResult(rmse_position=0.051, rmse_orientation=0.098, ...)

# Interactive viser visualization:
comp.show_trajectory_animation()
comp.show_static_grid()
```

## `PoseDelta(translation, rotation, twist, q)`

FK pose difference between two models at a single configuration.

The delta is expressed as `M_nominal⁻¹ · M_modified`, i.e. the SE3 transform from the nominal end-effector frame to the modified one.

### `position_error()`

Euclidean distance in meters.

Source code in `src/figaroh/tools/export_validation.py`

```
def position_error(self) -> float:
    """Euclidean distance in meters."""
    return float(np.linalg.norm(self.translation))
```

### `orientation_error()`

Angle-axis magnitude in radians.

Source code in `src/figaroh/tools/export_validation.py`

```
def orientation_error(self) -> float:
    """Angle-axis magnitude in radians."""
    return float(np.linalg.norm(self.twist[3:]))
```

## `FkConsistencyResult(rmse_position, rmse_orientation, max_position, max_orientation, per_sample)`

Aggregated FK consistency check — compares nominal vs. exported URDF FK.

## `PoseError(q, position_error_mm, orientation_error_deg, pose_delta, label='')`

FK error at a single static configuration with user label.

## `URDFComparison(nominal_urdf, modified_urdf)`

Compare two URDF models via FK analysis and viser visualization.

Loads both URDFs into Pinocchio for FK computation and into yourdfpy (when needed) for viser rendering.

Parameters:

| Name            | Type               | Description                                   | Default    |
| --------------- | ------------------ | --------------------------------------------- | ---------- |
| `nominal_urdf`  | `Union[str, Path]` | Path to the original/reference URDF.          | *required* |
| `modified_urdf` | `Union[str, Path]` | Path to the URDF after parameter application. | *required* |

Source code in `src/figaroh/tools/export_validation.py`

```
def __init__(
    self,
    nominal_urdf: Union[str, Path],
    modified_urdf: Union[str, Path],
):
    if pin is None:
        raise ImportError("pinocchio is required for URDFComparison")

    self.nominal_path = Path(nominal_urdf)
    self.modified_path = Path(modified_urdf)

    for p in (self.nominal_path, self.modified_path):
        if not p.exists():
            raise FileNotFoundError(f"URDF not found: {p}")

    # Pinocchio models for FK computation
    self.model_a = pin.buildModelFromUrdf(str(self.nominal_path))
    self.model_b = pin.buildModelFromUrdf(str(self.modified_path))
    self.data_a = self.model_a.createData()
    self.data_b = self.model_b.createData()

    # Identify end-effector: deepest frame in the model tree.
    # We skip frames attached directly to the universe (joint parent = 0)
    # to avoid picking fixed-base sensor/camera links.
    self.ee_frame_a, self.ee_id_a = self._detect_ee(self.model_a)
    self.ee_frame_b, self.ee_id_b = self._detect_ee(self.model_b)
```

### `fk_consistency_check(n_samples=100, seed=42)`

Check the exported URDF reproduces the calibrated FK numerically.

This is a file-integrity check — it verifies the exported URDF file is self-consistent with the calibration results. It does NOT test against ground-truth measurements (use calibration validation for that).

Parameters:

| Name        | Type  | Description                                | Default |
| ----------- | ----- | ------------------------------------------ | ------- |
| `n_samples` | `int` | Number of random configurations to sample. | `100`   |
| `seed`      | `int` | Random seed for reproducibility.           | `42`    |

Returns:

| Type                  | Description                                                 |
| --------------------- | ----------------------------------------------------------- |
| `FkConsistencyResult` | FkConsistencyResult with aggregated FK consistency metrics. |

Source code in `src/figaroh/tools/export_validation.py`

```
def fk_consistency_check(
    self,
    n_samples: int = 100,
    seed: int = 42,
) -> FkConsistencyResult:
    """Check the exported URDF reproduces the calibrated FK numerically.

    This is a file-integrity check — it verifies the exported URDF file
    is self-consistent with the calibration results. It does NOT test
    against ground-truth measurements (use calibration validation for
    that).

    Args:
        n_samples: Number of random configurations to sample.
        seed: Random seed for reproducibility.

    Returns:
        FkConsistencyResult with aggregated FK consistency metrics.
    """
    configs = self._sample_configs(n_samples, seed)
    deltas = [self._compute_delta(q) for q in configs]
    return self._aggregate(deltas)
```

### `static_poses(poses=None)`

Compute FK errors at specific joint configurations.

Parameters:

| Name    | Type                                          | Description                                                                                                                                   | Default |
| ------- | --------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- | ------- |
| `poses` | `Optional[List[Union[List[float], ndarray]]]` | List of joint configuration vectors. If None, a default set of 20 meaningful poses is used (covering home, limits, mixed-angle combinations). | `None`  |

Returns:

| Type              | Description                               |
| ----------------- | ----------------------------------------- |
| `List[PoseError]` | List of PoseError, one per configuration. |

Source code in `src/figaroh/tools/export_validation.py`

```
def static_poses(
    self,
    poses: Optional[List[Union[List[float], np.ndarray]]] = None,
) -> List[PoseError]:
    """Compute FK errors at specific joint configurations.

    Args:
        poses: List of joint configuration vectors. If None, a default
            set of 20 meaningful poses is used (covering home, limits,
            mixed-angle combinations).

    Returns:
        List of PoseError, one per configuration.
    """
    if poses is None:
        poses = self._default_poses()
    results = []
    for i, q in enumerate(poses):
        q_arr = np.asarray(q, dtype=float)
        delta = self._compute_delta(q_arr)
        results.append(
            PoseError(
                q=q_arr,
                position_error_mm=delta.position_error() * 1000,
                orientation_error_deg=delta.orientation_error() * 180 / np.pi,
                pose_delta=delta,
                label=f"pose_{i}",
            )
        )
    return results
```

### `show_overlay(server=None, port=8080, orig_color=(51, 102, 229), mod_color=(229, 51, 51), duration=5.0)`

Display both models overlaid in viser.

Parameters:

| Name         | Type                   | Description                                       | Default          |
| ------------ | ---------------------- | ------------------------------------------------- | ---------------- |
| `server`     |                        | Existing viser server, or None to create one.     | `None`           |
| `port`       | `int`                  | Port for new server (ignored if server is given). | `8080`           |
| `orig_color` | `Tuple[int, int, int]` | RGB color for the original model (0-255).         | `(51, 102, 229)` |
| `mod_color`  | `Tuple[int, int, int]` | RGB color for the modified model (0-255).         | `(229, 51, 51)`  |
| `duration`   | `float`                | Seconds to keep the display open.                 | `5.0`            |

Source code in `src/figaroh/tools/export_validation.py`

```
def show_overlay(
    self,
    server=None,
    port: int = 8080,
    orig_color: Tuple[int, int, int] = (51, 102, 229),
    mod_color: Tuple[int, int, int] = (229, 51, 51),
    duration: float = 5.0,
):
    """Display both models overlaid in viser.

    Args:
        server: Existing viser server, or None to create one.
        port: Port for new server (ignored if *server* is given).
        orig_color: RGB color for the original model (0-255).
        mod_color: RGB color for the modified model (0-255).
        duration: Seconds to keep the display open.
    """
    viz = self._get_viser_server(server, port)
    urdf_orig, urdf_mod = self._load_yourdfpy()

    from viser.extras import ViserUrdf

    try:
        viz.scene.remove("/robot")
    except Exception:
        pass

    ViserUrdf(
        viz,
        urdf_or_path=urdf_orig,
        root_node_name="/robot/original",
        load_meshes=True,
    )
    ViserUrdf(
        viz,
        urdf_or_path=urdf_mod,
        root_node_name="/robot/modified",
        load_meshes=True,
    )
    print(f"[vis] Both models displayed at http://localhost:{viz.get_port()}")
    print(f"[vis] Original (RGB={orig_color}) / Modified (RGB={mod_color})")
    time.sleep(duration)
```

### `show_trajectory_animation(n_configs=50, seed=42, server=None, port=8080, duration=10.0)`

Animate through random joint configs, tracing end-effector paths.

Parameters:

| Name        | Type    | Description                                       | Default |
| ----------- | ------- | ------------------------------------------------- | ------- |
| `n_configs` | `int`   | Number of random configurations to animate.       | `50`    |
| `seed`      | `int`   | Random seed.                                      | `42`    |
| `server`    |         | Existing viser server, or None to create one.     | `None`  |
| `port`      | `int`   | Port for new server.                              | `8080`  |
| `duration`  | `float` | Seconds to keep the display open after animation. | `10.0`  |

Source code in `src/figaroh/tools/export_validation.py`

```
def show_trajectory_animation(
    self,
    n_configs: int = 50,
    seed: int = 42,
    server=None,
    port: int = 8080,
    duration: float = 10.0,
):
    """Animate through random joint configs, tracing end-effector paths.

    Args:
        n_configs: Number of random configurations to animate.
        seed: Random seed.
        server: Existing viser server, or None to create one.
        port: Port for new server.
        duration: Seconds to keep the display open after animation.
    """
    import trimesh

    viz = self._get_viser_server(server, port)
    urdf_orig, urdf_mod = self._load_yourdfpy()
    from viser.extras import ViserUrdf

    vis_orig = ViserUrdf(
        viz,
        urdf_or_path=urdf_orig,
        root_node_name="/robot/orig_anim",
        load_meshes=True,
    )
    vis_mod = ViserUrdf(
        viz,
        urdf_or_path=urdf_mod,
        root_node_name="/robot/mod_anim",
        load_meshes=True,
    )

    configs = self._sample_configs(n_configs, seed)
    orig_trace: List[np.ndarray] = []
    mod_trace: List[np.ndarray] = []

    for q in configs:
        cfg_dict = self._pinocchio_config_to_dict(q)
        vis_orig.update_cfg(cfg_dict)
        vis_mod.update_cfg(cfg_dict)
        pin.forwardKinematics(self.model_a, self.data_a, q)
        pin.updateFramePlacements(self.model_a, self.data_a)
        pin.forwardKinematics(self.model_b, self.data_b, q)
        pin.updateFramePlacements(self.model_b, self.data_b)
        orig_trace.append(self.data_a.oMf[self.ee_id_a].translation.copy())
        mod_trace.append(self.data_b.oMf[self.ee_id_b].translation.copy())
        time.sleep(0.05)

    # Draw path traces
    r = 0.01
    sphere_mesh = trimesh.primitives.Sphere(radius=r)
    for pts, color, label in [
        (orig_trace, (51, 102, 229), "orig"),
        (mod_trace, (229, 51, 51), "mod"),
    ]:
        for i, pt in enumerate(pts):
            viz.scene.add_mesh_simple(
                f"/traces/{label}_{i}",
                sphere_mesh.vertices,
                sphere_mesh.faces,
                color=color,
                position=tuple(pt),
            )
    print(f"[vis] Traced {n_configs} configs at http://localhost:{viz.get_port()}")
    time.sleep(duration)
```

### `show_static_grid(poses=None, server=None, port=8080, duration=15.0, spacing=2.0)`

Display a grid of static configurations with error labels.

Each cell shows both models at the same configuration, with the position (mm) and orientation (deg) error overlaid.

Parameters:

| Name       | Type                                          | Description                                           | Default |
| ---------- | --------------------------------------------- | ----------------------------------------------------- | ------- |
| `poses`    | `Optional[List[Union[List[float], ndarray]]]` | Joint configurations. If None, uses 24 default poses. | `None`  |
| `server`   |                                               | Existing viser server, or None to create one.         | `None`  |
| `port`     | `int`                                         | Port for new server.                                  | `8080`  |
| `duration` | `float`                                       | Seconds to keep the display open.                     | `15.0`  |
| `spacing`  | `float`                                       | Distance between grid cells in meters.                | `2.0`   |

Source code in `src/figaroh/tools/export_validation.py`

```
def show_static_grid(
    self,
    poses: Optional[List[Union[List[float], np.ndarray]]] = None,
    server=None,
    port: int = 8080,
    duration: float = 15.0,
    spacing: float = 2.0,
):
    """Display a grid of static configurations with error labels.

    Each cell shows both models at the same configuration, with the
    position (mm) and orientation (deg) error overlaid.

    Args:
        poses: Joint configurations. If None, uses 24 default poses.
        server: Existing viser server, or None to create one.
        port: Port for new server.
        duration: Seconds to keep the display open.
        spacing: Distance between grid cells in meters.
    """
    from viser.extras import ViserUrdf

    viz = self._get_viser_server(server, port)
    urdf_orig, urdf_mod = self._load_yourdfpy()

    if poses is None:
        poses = self._default_poses()

    for i, q in enumerate(poses):
        row, col = divmod(i, 4)
        x_off = col * spacing
        z_off = row * spacing
        q_arr = np.asarray(q, dtype=float)

        vis_orig = ViserUrdf(
            viz,
            urdf_or_path=urdf_orig,
            root_node_name=f"/grid/{i}/orig",
            load_meshes=True,
        )
        vis_mod = ViserUrdf(
            viz,
            urdf_or_path=urdf_mod,
            root_node_name=f"/grid/{i}/mod",
            load_meshes=True,
        )
        cfg_dict = self._pinocchio_config_to_dict(q_arr)
        vis_orig.update_cfg(cfg_dict)
        vis_mod.update_cfg(cfg_dict)

        delta = self._compute_delta(q_arr)
        pos_err = delta.position_error() * 1000  # mm
        orient_err = delta.orientation_error() * 180 / np.pi  # deg

        viz.scene.add_label(
            f"/grid/{i}/label",
            f"{pos_err:.1f}mm / {orient_err:.1f}°",
            position=(x_off, z_off + 1.2, 0),
        )
    print(
        f"[vis] {len(poses)} configs in grid at http://localhost:{viz.get_port()}"
    )
    time.sleep(duration)
```

### `show_interactive_validation(n_trajectory=50, seed=42, port=8080)`

Interactive combined visualization with trajectory, static comparison, error plots, replay, and opacity controls.

Opens a single viser server containing:

- **Trajectory animation** — 50 random configurations with path traces for both nominal and modified models. Replay button allows re-running.
- **Static pose comparison** — overlays both models at the same configuration with opacity slider for the modified model.
- **Error plots** — uplot charts for trajectory and static pose errors (position in mm, orientation in degrees).
- **Pose selector** — slider to cycle through static configurations.

Parameters:

| Name           | Type  | Description                                 | Default |
| -------------- | ----- | ------------------------------------------- | ------- |
| `n_trajectory` | `int` | Number of random configurations to animate. | `50`    |
| `seed`         | `int` | Random seed for reproducibility.            | `42`    |
| `port`         | `int` | Port for the viser server.                  | `8080`  |

Source code in `src/figaroh/tools/export_validation.py`

```
def show_interactive_validation(
    self,
    n_trajectory: int = 50,
    seed: int = 42,
    port: int = 8080,
):
    """Interactive combined visualization with trajectory, static comparison,
    error plots, replay, and opacity controls.

    Opens a single viser server containing:

    * **Trajectory animation** — 50 random configurations with path traces
      for both nominal and modified models. Replay button allows re-running.
    * **Static pose comparison** — overlays both models at the same
      configuration with opacity slider for the modified model.
    * **Error plots** — uplot charts for trajectory and static pose errors
      (position in mm, orientation in degrees).
    * **Pose selector** — slider to cycle through static configurations.

    Args:
        n_trajectory: Number of random configurations to animate.
        seed: Random seed for reproducibility.
        port: Port for the viser server.
    """
    import threading

    from viser import uplot
    from viser.extras import ViserUrdf

    server = self._get_viser_server(None, port)
    urdf_orig, urdf_mod = self._load_yourdfpy()
    traj_configs = self._sample_configs(n_trajectory, seed)
    static_configs = self._default_poses()

    # --- 3D Scene: trajectory robots ---
    # Parent frames to offset trajectory models from static comparison
    server.scene.add_frame("/trajectory", show_axes=False)
    traj_vis_orig = ViserUrdf(
        server,
        urdf_or_path=urdf_orig,
        root_node_name="/trajectory/orig",
        load_meshes=True,
        mesh_color_override=(51, 102, 229),
    )
    traj_vis_mod = ViserUrdf(
        server,
        urdf_or_path=urdf_mod,
        root_node_name="/trajectory/mod",
        load_meshes=True,
        mesh_color_override=(229, 51, 51),
    )

    # --- 3D Scene: static comparison robots (offset to the right) ---
    server.scene.add_frame(
        "/static",
        show_axes=False,
        position=(2.5, 0, 0),
    )
    static_vis_nom = ViserUrdf(
        server,
        urdf_or_path=urdf_orig,
        root_node_name="/static/nominal",
        load_meshes=True,
        mesh_color_override=(51, 102, 229),
    )
    static_vis_mod = ViserUrdf(
        server,
        urdf_or_path=urdf_mod,
        root_node_name="/static/modified",
        load_meshes=True,
        mesh_color_override=(229, 51, 51, 0.5),
    )
    # Labels for static section
    server.scene.add_label(
        "/static/label_nom",
        "Nominal",
        position=(0, 0, 1.8),
    )
    server.scene.add_label(
        "/static/label_mod",
        "Modified",
        position=(0, 0, 2.0),
    )

    # --- Track static error state ---
    static_state = {
        "idx": 0,
        "opacity": 0.5,
        "configs": static_configs,
        "errors": [self._compute_delta(q) for q in static_configs],
    }

    # --- Compute trajectory errors upfront ---
    traj_errors = [self._compute_delta(q) for q in traj_configs]
    traj_pos_mm = np.array([e.position_error() * 1000 for e in traj_errors])
    traj_orient_deg = np.array(
        [e.orientation_error() * 180 / np.pi for e in traj_errors]
    )
    static_pos_mm = np.array(
        [e.position_error() * 1000 for e in static_state["errors"]]
    )
    static_orient_deg = np.array(
        [e.orientation_error() * 180 / np.pi for e in static_state["errors"]]
    )

    # --- Animation state ---
    anim_state: dict = {
        "traj_configs": traj_configs,
        "running": False,
        "speed": 0.05,
        "orig_trace": [],
        "mod_trace": [],
    }

    def _clear_traces():
        try:
            server.scene.remove_by_name("/traces/orig_line")
        except Exception:
            pass
        try:
            server.scene.remove_by_name("/traces/mod_line")
        except Exception:
            pass

    def _draw_traces():
        """Draw EE path traces as line segments."""
        orig_pts = np.array(anim_state["orig_trace"])
        mod_pts = np.array(anim_state["mod_trace"])
        if len(orig_pts) < 2:
            return

        # Reshape to (N-1, 2, 3) for line segments
        def _to_segments(pts: np.ndarray) -> np.ndarray:
            return np.stack([pts[:-1], pts[1:]], axis=1)

        # Nominal trace (blue)
        server.scene.add_line_segments(
            "/traces/orig_line",
            _to_segments(orig_pts),
            colors=(51, 102, 229),
            line_width=2,
        )
        # Modified trace (red)
        server.scene.add_line_segments(
            "/traces/mod_line",
            _to_segments(mod_pts),
            colors=(229, 51, 51),
            line_width=2,
        )

    def _run_animation():
        if anim_state["running"]:
            return
        anim_state["running"] = True
        _clear_traces()
        anim_state["orig_trace"].clear()
        anim_state["mod_trace"].clear()

        for i, q in enumerate(anim_state["traj_configs"]):
            cfg_dict = self._pinocchio_config_to_dict(q)
            traj_vis_orig.update_cfg(cfg_dict)
            traj_vis_mod.update_cfg(cfg_dict)

            pin.forwardKinematics(self.model_a, self.data_a, q)
            pin.updateFramePlacements(self.model_a, self.data_a)
            pin.forwardKinematics(self.model_b, self.data_b, q)
            pin.updateFramePlacements(self.model_b, self.data_b)

            anim_state["orig_trace"].append(
                self.data_a.oMf[self.ee_id_a].translation.copy()
            )
            anim_state["mod_trace"].append(
                self.data_b.oMf[self.ee_id_b].translation.copy()
            )
            time.sleep(anim_state["speed"])

        _draw_traces()
        anim_state["running"] = False
        replay_btn.disabled = False

    def _update_static_pose():
        """Apply the current static pose to both overlaid models."""
        idx = static_state["idx"]
        q = static_state["configs"][idx]
        cfg_dict = self._pinocchio_config_to_dict(q)
        static_vis_nom.update_cfg(cfg_dict)
        static_vis_mod.update_cfg(cfg_dict)
        error = static_state["errors"][idx]
        pos_mm = error.position_error() * 1000
        orient_deg = error.orientation_error() * 180 / np.pi
        pose_label.content = f"Pose {idx}: {pos_mm:.1f}mm / {orient_deg:.1f}°"

    # --- GUI Controls ---
    with server.gui.add_folder("Controls"):
        # Trajectory section
        server.gui.add_markdown("**Trajectory**")
        replay_btn = server.gui.add_button(
            "Replay Trajectory", icon="player-play-filled"
        )
        speed_slider = server.gui.add_slider(
            "Animation Speed",
            0.01,
            0.2,
            0.01,
            0.05,
            hint="Delay between configurations (seconds)",
        )

        # Static comparison section
        server.gui.add_markdown("**Static Comparison**")
        pose_slider = server.gui.add_slider(
            "Pose Index",
            0,
            len(static_configs) - 1,
            1,
            0,
            hint="Select which static configuration to display",
        )
        opacity_slider = server.gui.add_slider(
            "Modified Model Opacity",
            0.0,
            1.0,
            0.01,
            0.5,
            hint="Transparency of the modified model overlay",
        )
        pose_label = server.gui.add_markdown(
            "Pose 0: ...",
        )

    # --- Error Plots ---
    traj_x = np.arange(len(traj_pos_mm))
    static_x = np.arange(len(static_pos_mm))

    with server.gui.add_folder("Error Plots"):
        server.gui.add_markdown("**Trajectory FK Errors**")
        server.gui.add_uplot(
            data=(traj_x, traj_pos_mm, traj_orient_deg),
            series=(
                uplot.Series(),
                uplot.Series(
                    label="Position Error (mm)",
                    color=(51, 102, 229),
                ),
                uplot.Series(
                    label="Orientation Error (deg)",
                    color=(229, 51, 51),
                ),
            ),
            title="Trajectory",
            aspect=2.0,
        )

        server.gui.add_markdown("**Static Pose FK Errors**")
        server.gui.add_uplot(
            data=(static_x, static_pos_mm, static_orient_deg),
            series=(
                uplot.Series(),
                uplot.Series(
                    label="Position Error (mm)",
                    color=(51, 102, 229),
                ),
                uplot.Series(
                    label="Orientation Error (deg)",
                    color=(229, 51, 51),
                ),
            ),
            title="Static Poses",
            aspect=2.0,
        )

    # --- Callbacks ---
    @replay_btn.on_click
    def _on_replay(_event):
        replay_btn.disabled = True
        thread = threading.Thread(target=_run_animation, daemon=True)
        thread.start()

    @speed_slider.on_update
    def _on_speed_change(_event):
        anim_state["speed"] = _event.target.value

    @pose_slider.on_update
    def _on_pose_change(_event):
        static_state["idx"] = int(_event.target.value)
        _update_static_pose()

    @opacity_slider.on_update
    def _on_opacity_change(_event):
        static_state["opacity"] = _event.target.value
        for mesh in static_vis_mod._meshes:
            mesh.opacity = _event.target.value

    # --- Initial state ---
    _update_static_pose()

    print("\n" + "=" * 60)
    print(f"Interactive validation at http://localhost:{server.get_port()}")
    print("  Trajectory:  blue (nominal), red (modified)")
    print("  Static:      overlaid models (offset right)")
    print("  Controls:    Replay | Speed | Pose | Opacity")
    print("  Error plots: scroll GUI panel")
    print("=" * 60)

    # --- Run initial animation ---
    replay_btn.disabled = True
    _run_animation()

    # Block until user stops the server
    try:
        server.sleep_forever()
    except KeyboardInterrupt:
        pass
```

# 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
- Schema validation

## `ConfigurationError`

Bases: `FigarohExampleError`

Exception raised for configuration-related issues.

## `ConfigMetadata(schema_version='2.0', config_type='robot_configuration', source_file=None, variant=None, resolved_at=None)`

Configuration metadata container.

## `UnifiedConfigParser(config_path, variant=None)`

Unified configuration parser supporting inheritance, templates, and validation.

Initialize parser with configuration file path.

Parameters:

| Name          | Type               | Description                                     | Default    |
| ------------- | ------------------ | ----------------------------------------------- | ---------- |
| `config_path` | `Union[str, Path]` | Path to configuration file                      | *required* |
| `variant`     | `Optional[str]`    | Optional variant name to use from configuration | `None`     |

Source code in `src/figaroh/utils/config_parser.py`

```
def __init__(self, config_path: Union[str, Path], variant: Optional[str] = None):
    """Initialize parser with configuration file path.

    Args:
        config_path: Path to configuration file
        variant: Optional variant name to use from configuration
    """
    self.config_path = Path(config_path).resolve()
    self.variant = variant
    self.logger = logging.getLogger(self.__class__.__name__)
    self._template_cache = {}
    self._variable_cache = {}

    # Validate config file exists
    if not self.config_path.exists():
        raise ConfigurationError(
            f"Configuration file not found: {self.config_path}"
        )
```

### `parse()`

Parse configuration with inheritance and validation.

Returns:

| Type             | Description                                     |
| ---------------- | ----------------------------------------------- |
| `Dict[str, Any]` | Complete configuration dictionary with metadata |

Raises:

| Type                 | Description                    |
| -------------------- | ------------------------------ |
| `ConfigurationError` | If parsing or validation fails |

Source code in `src/figaroh/utils/config_parser.py`

```
def parse(self) -> Dict[str, Any]:
    """Parse configuration with inheritance and validation.

    Returns:
        Complete configuration dictionary with metadata

    Raises:
        ConfigurationError: If parsing or validation fails
    """
    try:
        self.logger.info(f"Parsing configuration: {self.config_path}")

        # Load base configuration
        config = self._load_config_file(self.config_path)

        # Handle inheritance/extensions
        if "extends" in config:
            config = self._resolve_inheritance(config)

        # Apply variant if specified
        if self.variant:
            config = self._apply_variant(config, self.variant)

        # Resolve task inheritance
        config = self._resolve_task_inheritance(config)

        # Expand variables
        config = self._expand_variables(config)

        # Validate configuration
        self._validate_configuration(config)

        # Add metadata
        config["_metadata"] = ConfigMetadata(
            source_file=str(self.config_path),
            variant=self.variant,
            resolved_at=str(Path.cwd()),
        ).__dict__

        self.logger.info("Configuration parsing completed successfully")
        return config

    except Exception as e:
        self.logger.error(f"Failed to parse configuration: {e}")
        raise ConfigurationError(
            f"Failed to parse configuration {self.config_path}: {e}"
        ) from e
```

## `create_task_config(robot, unified_config, task_name)`

Create task-specific configuration from unified config.

Parameters:

| Name             | Type             | Description                               | Default    |
| ---------------- | ---------------- | ----------------------------------------- | ---------- |
| `robot`          |                  | Robot instance                            | *required* |
| `unified_config` | `Dict[str, Any]` | Parsed unified configuration              | *required* |
| `task_name`      | `str`            | Name of task to extract configuration for | *required* |

Returns:

| Type             | Description                            |
| ---------------- | -------------------------------------- |
| `Dict[str, Any]` | Task-specific configuration dictionary |

Raises:

| Type                 | Description                  |
| -------------------- | ---------------------------- |
| `ConfigurationError` | If task not found or invalid |

Source code in `src/figaroh/utils/config_parser.py`

```
def create_task_config(
    robot, unified_config: Dict[str, Any], task_name: str
) -> Dict[str, Any]:
    """Create task-specific configuration from unified config.

    Args:
        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
    """
    if "tasks" not in unified_config or task_name not in unified_config["tasks"]:
        available_tasks = list(unified_config.get("tasks", {}).keys())
        raise ConfigurationError(
            f"Task '{task_name}' not found in configuration. Available tasks: {available_tasks}"
        )

    task_config = unified_config["tasks"][task_name]
    robot_config = unified_config.get("robot", {})

    # Check if task is enabled
    if not task_config.get("enabled", True):
        raise ConfigurationError(f"Task '{task_name}' is disabled in configuration")

    # Combine robot properties with task configuration
    combined_config = {
        "robot_name": robot_config.get("name", robot.model.name),
        "task_type": task_config.get("type", task_name),
    }

    # Add robot properties
    if "properties" in robot_config:
        combined_config.update(robot_config["properties"])

    # Physical-asset identity, distinct from the model type above (e.g.
    # "ur10" is shared by every UR10; robot.instance.asset_id identifies
    # one physical unit). Optional — omitted configs simply carry no
    # instance block, and provenance capture renders that as
    # "unspecified unit" rather than guessing.
    if "instance" in robot_config:
        combined_config["instance"] = robot_config["instance"]

    # Add task configuration (excluding metadata)
    task_data = {
        k: v
        for k, v in task_config.items()
        if not k.startswith("_") and k not in ["enabled", "type"]
    }
    combined_config.update(task_data)

    # Add custom parameters
    if "custom" in unified_config:
        combined_config["custom"] = unified_config["custom"]

    # Add environment settings that might be relevant
    if "environment" in unified_config:
        env = unified_config["environment"]
        combined_config.update(
            {
                "working_directory": env.get("working_directory", "."),
                "data_directory": env.get("data_directory", "data"),
                "results_directory": env.get("results_directory", "results"),
            }
        )

    return combined_config
```

## `parse_configuration(config_path, variant=None)`

Convenience function to parse a configuration file.

Parameters:

| Name          | Type               | Description                    | Default    |
| ------------- | ------------------ | ------------------------------ | ---------- |
| `config_path` | `Union[str, Path]` | Path to configuration file     | *required* |
| `variant`     | `Optional[str]`    | Optional variant name to apply | `None`     |

Returns:

| Type             | Description                     |
| ---------------- | ------------------------------- |
| `Dict[str, Any]` | Parsed configuration dictionary |

Source code in `src/figaroh/utils/config_parser.py`

```
def parse_configuration(
    config_path: Union[str, Path], variant: Optional[str] = None
) -> Dict[str, Any]:
    """Convenience function to parse a configuration file.

    Args:
        config_path: Path to configuration file
        variant: Optional variant name to apply

    Returns:
        Parsed configuration dictionary
    """
    parser = UnifiedConfigParser(config_path, variant)
    return parser.parse()
```

## `get_param_from_yaml(robot, config_data, task_type='auto', variant=None)`

Unified parameter parser.

Parameters:

| Name          | Type                               | Description                                                                     | Default    |
| ------------- | ---------------------------------- | ------------------------------------------------------------------------------- | ---------- |
| `robot`       |                                    | Robot instance                                                                  | *required* |
| `config_data` | `Union[Dict[str, Any], str, Path]` | Configuration data (dict), or path to config file                               | *required* |
| `task_type`   | `str`                              | Type of task configuration to extract ("calibration", "identification", "auto") | `'auto'`   |
| `variant`     | `Optional[str]`                    | Optional variant to apply                                                       | `None`     |

Returns:

| Type             | Description                        |
| ---------------- | ---------------------------------- |
| `Dict[str, Any]` | Task-specific parameter dictionary |

Raises:

| Type                 | Description                                                                                   |
| -------------------- | --------------------------------------------------------------------------------------------- |
| `ConfigurationError` | If parsing fails, the task is not found, or the data is a legacy flat config (removed in 1.0) |

Source code in `src/figaroh/utils/config_parser.py`

```
def get_param_from_yaml(
    robot,
    config_data: Union[Dict[str, Any], str, Path],
    task_type: str = "auto",
    variant: Optional[str] = None,
) -> Dict[str, Any]:
    """Unified parameter parser.

    Args:
        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, the task is not found, or the
            data is a legacy flat config (removed in 1.0)
    """
    # Handle file path input
    if isinstance(config_data, (str, Path)):
        unified_config = parse_configuration(config_data, variant)

        # Auto-detect task type if needed
        if task_type == "auto":
            tasks = unified_config.get("tasks", {})
            enabled_tasks = [
                name for name, config in tasks.items() if config.get("enabled", True)
            ]
            if len(enabled_tasks) == 1:
                task_type = enabled_tasks[0]
            else:
                raise ConfigurationError(
                    f"Cannot auto-detect task type. Available tasks: {enabled_tasks}"
                )

        return create_task_config(robot, unified_config, task_type)

    # Handle dictionary input (must be unified)
    if not isinstance(config_data, dict):
        raise ConfigurationError("config_data must be a dictionary or file path")

    # Check if it's already a unified configuration
    if "tasks" in config_data:
        # Unified format
        if task_type == "auto":
            tasks = config_data.get("tasks", {})
            enabled_tasks = [
                name for name, config in tasks.items() if config.get("enabled", True)
            ]
            if len(enabled_tasks) == 1:
                task_type = enabled_tasks[0]
            else:
                raise ConfigurationError(
                    f"Cannot auto-detect task type. Available tasks: {enabled_tasks}"
                )

        return create_task_config(robot, config_data, task_type)

    # Legacy flat format (removed in 1.0)
    raise legacy_config_error()
```

## `legacy_config_error(config_file=None)`

Build the error raised when a legacy flat config is loaded.

Parameters:

| Name          | Type                         | Description                                      | Default |
| ------------- | ---------------------------- | ------------------------------------------------ | ------- |
| `config_file` | `Optional[Union[str, Path]]` | Offending file, named in the message when given. | `None`  |

Returns:

| Type                 | Description                                              |
| -------------------- | -------------------------------------------------------- |
| `ConfigurationError` | An error naming the migration CLI. The caller raises it. |

Source code in `src/figaroh/utils/config_parser.py`

```
def legacy_config_error(
    config_file: Optional[Union[str, Path]] = None,
) -> ConfigurationError:
    """Build the error raised when a legacy flat config is loaded.

    Args:
        config_file: Offending file, named in the message when given.

    Returns:
        An error naming the migration CLI. The caller raises it.
    """
    where = f" ({config_file})" if config_file is not None else ""
    return ConfigurationError(f"{LEGACY_CONFIG_REMOVED_MESSAGE}{where}")
```

## `require_unified_config(config_file)`

Raise unless `config_file` is an existing unified-format config.

Raises:

| Type                 | Description                                                 |
| -------------------- | ----------------------------------------------------------- |
| `FileNotFoundError`  | The file does not exist.                                    |
| `ConfigurationError` | The file is a legacy flat config (names the migration CLI). |

Source code in `src/figaroh/utils/config_parser.py`

```
def require_unified_config(config_file: Union[str, Path]) -> None:
    """Raise unless ``config_file`` is an existing unified-format config.

    Raises:
        FileNotFoundError: The file does not exist.
        ConfigurationError: The file is a legacy flat config (names the
            migration CLI).
    """
    if not Path(config_file).exists():
        raise FileNotFoundError(f"Configuration file not found: {config_file}")
    if not is_unified_config(config_file):
        raise legacy_config_error(config_file)
```

## `is_unified_config(config_file)`

Check if a configuration file is using the unified format.

Parameters:

| Name          | Type               | Description                | Default    |
| ------------- | ------------------ | -------------------------- | ---------- |
| `config_file` | `Union[str, Path]` | Path to configuration file | *required* |

Returns:

| Type   | Description                                    |
| ------ | ---------------------------------------------- |
| `bool` | True if unified format, False if legacy format |

Source code in `src/figaroh/utils/config_parser.py`

```
def is_unified_config(config_file: Union[str, Path]) -> bool:
    """Check if a configuration file is using the unified format.

    Args:
        config_file: Path to configuration file

    Returns:
        True if unified format, False if legacy format
    """
    try:
        config_path = Path(config_file)
        if not config_path.exists():
            return False

        with open(config_path, "r", encoding="utf-8") as f:
            config = yaml.safe_load(f)

        if not isinstance(config, dict):
            return False

        # Check for unified format indicators
        unified_indicators = [
            "_metadata",
            "schema_version",
            "extends",
            "variants",
            "tasks",
            "common",
            "variables",
        ]

        # If any unified indicators are present, consider it unified
        for indicator in unified_indicators:
            if indicator in config:
                logger.debug(f"Detected unified config indicator: {indicator}")
                return True

        # Check if the file has task inheritance patterns
        for key, value in config.items():
            if isinstance(value, dict) and "inherits_from" in value:
                logger.debug(f"Detected task inheritance in {key}")
                return True

        # Default to legacy format
        logger.debug("No unified format indicators found, treating as legacy")
        return False

    except Exception as e:
        logger.warning(f"Error checking config format for {config_file}: {e}")
        return False
```

## 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:

```
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:

```
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

## `CubicSpline(robot, num_waypoints, active_joints, soft_lim=0)`

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.

Attributes:

| Name            | Type                       | Description                        |
| --------------- | -------------------------- | ---------------------------------- |
| `robot`         |                            | Robot model instance               |
| `rmodel`        |                            | Robot kinematic model              |
| `num_waypoints` | `int`                      | Number of waypoints in trajectory  |
| `act_Jid`       | `List[int]`                | Active joint IDs                   |
| `act_Jname`     | `List[str]`                | Active joint names                 |
| `act_J`         | `List`                     | Active joint objects               |
| `act_idxq`      | `List[int]`                | Position indices for active joints |
| `act_idxv`      | `List[int]`                | Velocity indices for active joints |
| `dim_q`         | `Tuple[int, int]`          | Position vector dimensions         |
| `dim_v`         | `Tuple[int, int]`          | Velocity vector dimensions         |
| `upper_q,`      | `lower_q (np.ndarray`      | Position limits for active joints  |
| `upper_dq,`     | `lower_dq (np.ndarray`     | Velocity limits for active joints  |
| `upper_effort,` | `lower_effort (np.ndarray` | Effort limits for active joints    |

Examples:

Basic usage:

```
# 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:

```
# 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.

Initialize the cubic spline trajectory generator.

Parameters:

| Name            | Type    | Description                                                                                         | Default    |
| --------------- | ------- | --------------------------------------------------------------------------------------------------- | ---------- |
| `robot`         |         | Robot model instance containing kinematic information                                               | *required* |
| `num_waypoints` | `int`   | Number of waypoints for the trajectory                                                              | *required* |
| `active_joints` | `list`  | List of joint names to include in trajectory                                                        | *required* |
| `soft_lim`      | `float` | Soft limit reduction factor (0-1). Reduces joint limits by this fraction for safety. Defaults to 0. | `0`        |

Raises:

| Type             | Description                                 |
| ---------------- | ------------------------------------------- |
| `AssertionError` | If joint names are not found in robot model |

Examples:

```
# Basic initialization
spline = CubicSpline(robot, 5, ['joint1', 'joint2'])

# With 10% safety margin on joint limits
spline = CubicSpline(robot, 5, ['joint1', 'joint2'],
                   soft_lim=0.1)
```

Source code in `src/figaroh/utils/cubic_spline.py`

````
def __init__(self, robot, num_waypoints: int, active_joints: list, soft_lim=0):
    """
    Initialize the cubic spline trajectory generator.

    Args:
        robot: Robot model instance containing kinematic information
        num_waypoints (int): Number of waypoints for the trajectory
        active_joints (list): List of joint names to include in trajectory
        soft_lim (float, optional): Soft limit reduction factor (0-1).
            Reduces joint limits by this fraction for safety.
            Defaults to 0.

    Raises:
        AssertionError: If joint names are not found in robot model

    Examples:
        ```python
        # Basic initialization
        spline = CubicSpline(robot, 5, ['joint1', 'joint2'])

        # With 10% safety margin on joint limits
        spline = CubicSpline(robot, 5, ['joint1', 'joint2'],
                           soft_lim=0.1)
        ```
    """

    self.robot = robot
    self.rmodel = self.robot.model
    self.num_waypoints = num_waypoints
    # joint id of active joints
    self.act_Jid = [self.rmodel.getJointId(i) for i in active_joints]
    # active joint objects and their names
    self.act_Jname = [self.rmodel.names[jid] for jid in self.act_Jid]
    self.act_J = [self.rmodel.joints[jid] for jid in self.act_Jid]
    # joint config id (e.g one joint might have >1 DOF)
    self.act_idxq = [J.idx_q for J in self.act_J]
    # joint velocity id
    self.act_idxv = [J.idx_v for J in self.act_J]

    # size of waypoints vector for all active joints
    self.dim_q = (len(self.act_idxq), self.num_waypoints)
    self.dim_v = (len(self.act_idxv), self.num_waypoints)

    # joint limits on active joints
    self.upper_q = self.rmodel.upperPositionLimit[self.act_idxq]
    self.lower_q = self.rmodel.lowerPositionLimit[self.act_idxq]

    self.upper_dq = self.rmodel.velocityLimit[self.act_idxv]
    self.lower_dq = -self.rmodel.velocityLimit[self.act_idxv]

    self.upper_effort = self.rmodel.effortLimit[self.act_idxv]
    self.lower_effort = -self.rmodel.effortLimit[self.act_idxv]

    # joint limits on active joints with soft limit on both limit ends
    if soft_lim > 0:
        self.upper_q = self.upper_q - soft_lim * abs(self.upper_q - self.lower_q)
        self.lower_q = self.lower_q + soft_lim * abs(self.upper_q - self.lower_q)

        self.upper_dq = self.upper_dq - soft_lim * abs(
            self.upper_dq - self.lower_dq
        )
        self.lower_dq = self.lower_dq + soft_lim * abs(
            self.upper_dq - self.lower_dq
        )

        self.upper_effort = self.upper_effort - soft_lim * abs(
            self.upper_effort - self.lower_effort
        )
        self.lower_effort = self.lower_effort + soft_lim * abs(
            self.upper_effort - self.lower_effort
        )
````

### `get_active_config(freq, time_points, waypoints, vel_waypoints=None, acc_waypoints=None)`

Generate cubic splines on active joints

Source code in `src/figaroh/utils/cubic_spline.py`

```
def get_active_config(
    self,
    freq: int,
    time_points: np.ndarray,
    waypoints: np.ndarray,
    vel_waypoints=None,
    acc_waypoints=None,
):
    """Generate cubic splines on active joints"""
    # dimensions
    assert (
        self.dim_q == waypoints.shape
    ), "(Pos) Check size \
                                    (num_active_joints,num_waypoints)!"
    self.pc = ndcurves.piecewise()  # set piecewise object to join segments

    # C_2 continuous at waypoints
    if vel_waypoints is not None and acc_waypoints is not None:
        # dimensions
        assert (
            self.dim_v == vel_waypoints.shape
        ), "(Vel) Check size\
                                    (num_active_joints, num_waypoints)!"
        assert (
            self.dim_v == acc_waypoints.shape
        ), "(Acc) Check size\
                                    (num_active_joints, num_waypoints)!"

        # make exact cubic WITH constraints on vel and acc on both ends
        for i in range(self.num_waypoints - 1):
            self.c = ndcurves.curve_constraints()
            self.c.init_vel = np.matrix(vel_waypoints[:, i]).transpose()
            self.c.end_vel = np.matrix(vel_waypoints[:, i + 1]).transpose()
            self.c.init_acc = np.matrix(acc_waypoints[:, i]).transpose()
            self.c.end_acc = np.matrix(acc_waypoints[:, i + 1]).transpose()
            ec = ndcurves.exact_cubic(
                waypoints[:, range(i, i + 2)],
                time_points[range(i, i + 2), 0],
                self.c,
            )
            self.pc.append(ec)

    # make exact cubic WITHOUT constraints on vel and acc on both ends
    else:
        for i in range(self.num_waypoints - 1):
            ec = ndcurves.exact_cubic(
                waypoints[:, range(i, i + 2)], time_points[range(i, i + 2), 0]
            )  # Added spaces around '+'
            self.pc.append(ec)

    # time step
    self.delta_t = 1 / freq  # Added spaces around '/'
    # total travel time
    self.T = self.pc.max() - self.pc.min()
    # get number sample points from generated trajectory
    self.N = int(self.T / self.delta_t) + 1
    # create time stamps on all sample points
    self.t = (
        self.pc.min()
        + np.matrix([i * self.delta_t for i in range(self.N)]).transpose()
    )

    # compute derivatives to obtain pos/vel/acc on all samples (bad)
    self.q_act = np.array(
        [self.pc(self.t[i, 0]) for i in range(self.N)], dtype="float"
    )
    self.dq_act = np.array(
        [self.pc.derivate(self.t[i, 0], 1) for i in range(self.N)], dtype="float"
    )
    self.ddq_act = np.array(
        [self.pc.derivate(self.t[i, 0], 2) for i in range(self.N)], dtype="float"
    )
    t, p_act, v_act, a_act = self.t, self.q_act, self.dq_act, self.ddq_act

    return t, p_act, v_act, a_act
```

### `get_full_config(freq, time_points, waypoints, 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
1. Filling inactive joints with zero values
1. Ensuring C2 continuity at waypoints

Parameters:

| Name            | Type      | Description                                                                                                                | Default    |
| --------------- | --------- | -------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `freq`          | `int`     | Sampling frequency for trajectory generation (Hz)                                                                          | *required* |
| `time_points`   | `ndarray` | Time stamps for waypoints, shape (num_waypoints, 1)                                                                        | *required* |
| `waypoints`     | `ndarray` | Position waypoints for active joints, shape (num_active_joints, num_waypoints)                                             | *required* |
| `vel_waypoints` | `ndarray` | Velocity constraints at waypoints, same shape as waypoints. If provided, enforces specific velocities at waypoints.        | `None`     |
| `acc_waypoints` | `ndarray` | Acceleration constraints at waypoints, same shape as waypoints. If provided, enforces specific accelerations at waypoints. | `None`     |

Returns:

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

Raises:

| Type             | Description                                      |
| ---------------- | ------------------------------------------------ |
| `AssertionError` | If waypoint dimensions don't match active joints |

Examples:

Position-only trajectory:

```
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:

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

Source code in `src/figaroh/utils/cubic_spline.py`

````
def get_full_config(
    self,
    freq: int,
    time_points: np.ndarray,
    waypoints: np.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

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

    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.
    """
    t, p_act, v_act, a_act = self.get_active_config(
        freq, time_points, waypoints, vel_waypoints, acc_waypoints
    )
    # create array of zero configuration times N samples
    self.q_full = np.array([self.robot.q0] * self.N)
    self.dq_full = np.array([np.zeros_like(self.robot.v0)] * self.N)
    self.ddq_full = np.array([np.zeros_like(self.robot.v0)] * self.N)

    # fill in trajectory with active joints values
    self.q_full[:, self.act_idxq] = p_act
    self.dq_full[:, self.act_idxv] = v_act
    self.ddq_full[:, self.act_idxv] = a_act

    p_full, v_full, a_full = self.q_full, self.dq_full, self.ddq_full
    return t, p_full, v_full, a_full
````

### `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:

| Name       | Type      | Description                                                                                               | Default    |
| ---------- | --------- | --------------------------------------------------------------------------------------------------------- | ---------- |
| `q`        | `ndarray` | Position trajectory to check, shape (N, robot_nq)                                                         | *required* |
| `v`        | `ndarray` | Velocity trajectory to check, shape (N, robot_nv). If None, velocity checks are skipped.                  | `None`     |
| `tau`      | `ndarray` | Effort trajectory to check, shape (N, robot_nv). If None, effort checks are skipped.                      | `None`     |
| `soft_lim` | `float`   | Additional safety margin factor (0-1). Adds this fraction of joint range as safety buffer. Defaults to 0. | `0`        |

Returns:

| Name   | Type | Description                                                                |
| ------ | ---- | -------------------------------------------------------------------------- |
| `bool` |      | True if any constraint is violated, False if all constraints are satisfied |

Examples:

Basic position check:

```
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:

```
is_violated = spline.check_cfg_constraints(
    q, v=dq, tau=torques, soft_lim=0.1
)
```

Note

Constraint violations are logged at DEBUG level with specific joint indices and violation types; callers report the outcome.

Source code in `src/figaroh/utils/cubic_spline.py`

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

    Args:
        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:
        bool: True if any constraint is violated, False if all
            constraints are satisfied

    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 logged at DEBUG level with specific
        joint indices and violation types; callers report the outcome.
    """
    __isViolated = False
    for i in range(q.shape[0]):
        for j in self.act_idxq:
            delta_lim = soft_lim * abs(
                self.rmodel.upperPositionLimit[j]
                - self.rmodel.lowerPositionLimit[j]
            )
            if q[i, j] > self.rmodel.upperPositionLimit[j] - delta_lim:
                logger.debug("Joint q %d upper limit violated!", j)
                __isViolated_pos = True

            elif q[i, j] < self.rmodel.lowerPositionLimit[j] + delta_lim:
                logger.debug("Joint position idx_q %d lower limit violated!", j)
                __isViolated_pos = True
            else:
                __isViolated_pos = False
            __isViolated = __isViolated or __isViolated_pos
            # print(__isViolated)
    if v is not None:
        for i in range(v.shape[0]):
            for j in self.act_idxv:
                if abs(v[i, j]) > (1 - soft_lim) * abs(
                    self.rmodel.velocityLimit[j]
                ):
                    logger.debug("Joint vel idx_v %d limits violated!", j)
                    __isViolated_vel = True
                else:
                    __isViolated_vel = False
                __isViolated = __isViolated or __isViolated_vel
            # print(__isViolated)
    if tau is not None:
        for i in range(tau.shape[0]):
            for j in self.act_idxv:
                if abs(tau[i, j]) > (1 - soft_lim) * abs(
                    self.rmodel.effortLimit[j]
                ):
                    logger.debug("Joint effort idx_v %d limits violated!", j)
                    __isViolated_eff = True
                else:
                    __isViolated_eff = False
                __isViolated = __isViolated or __isViolated_eff
                # print(__isViolated)
    if not __isViolated:
        logger.debug(
            "SUCCEEDED to generate waypoints for a feasible initial cubic spline"
        )
    else:
        logger.debug("FAILED to generate a feasible cubic spline")
    return __isViolated
````

## `WaypointsGeneration(robot, num_waypoints, active_joints, 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.

Attributes:

| Name                      | Type      | Description                               |
| ------------------------- | --------- | ----------------------------------------- |
| `n_set`                   | `int`     | Size of waypoint pools (default: 10)      |
| `pool_q`                  | `ndarray` | Pool of valid position configurations     |
| `pool_dq`                 | `ndarray` | Pool of valid velocity configurations     |
| `pool_ddq`                | `ndarray` | Pool of valid acceleration configurations |
| `soft_limit_pool_default` | `ndarray` | Default soft limit values                 |

Examples:

Basic waypoint generation:

```
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:

```
# 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.

Initialize waypoint generation for cubic spline trajectories.

Sets up pools for generating random feasible waypoints that respect robot joint constraints. Initializes configuration pools for positions, velocities, and accelerations.

Parameters:

| Name            | Type    | Description                                           | Default    |
| --------------- | ------- | ----------------------------------------------------- | ---------- |
| `robot`         |         | Robot model instance containing kinematic information | *required* |
| `num_waypoints` | `int`   | Number of waypoints for trajectory generation         | *required* |
| `active_joints` | `list`  | List of joint names to include in trajectory          | *required* |
| `soft_lim`      | `float` | Soft limit reduction factor (0-1). Defaults to 0.     | `0`        |

Note

The soft_lim parameter affects the base CubicSpline initialization but does not set the waypoint generation pools. Use set_soft_limit_pool() for custom pool limits.

Source code in `src/figaroh/utils/cubic_spline.py`

```
def __init__(self, robot, num_waypoints: int, active_joints: list, soft_lim=0):
    """
    Initialize waypoint generation for cubic spline trajectories.

    Sets up pools for generating random feasible waypoints that respect
    robot joint constraints. Initializes configuration pools for
    positions, velocities, and accelerations.

    Args:
        robot: Robot model instance containing kinematic information
        num_waypoints (int): Number of waypoints for trajectory generation
        active_joints (list): List of joint names to include in trajectory
        soft_lim (float, optional): Soft limit reduction factor (0-1).
            Defaults to 0.

    Note:
        The soft_lim parameter affects the base CubicSpline initialization
        but does not set the waypoint generation pools. Use
        set_soft_limit_pool() for custom pool limits.
    """
    super().__init__(robot, num_waypoints, active_joints, soft_lim=0)

    self.n_set = 10  # size of waypoints pool
    self.pool_q = np.zeros((self.n_set, len(self.act_idxq)))
    self.pool_dq = np.zeros((self.n_set, len(self.act_idxv)))
    self.pool_ddq = np.zeros((self.n_set, len(self.act_idxv)))
    self.soft_limit_pool_default = np.zeros((3, len(self.act_idxq)))
```

### `gen_rand_pool(soft_limit_pool=None)`

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

Source code in `src/figaroh/utils/cubic_spline.py`

```
def gen_rand_pool(self, soft_limit_pool=None):
    """Generate a uniformly distributed waypoint pool of pos/vel/acc over
    a specific range
    """
    if soft_limit_pool is None:
        soft_limit_pool = self.soft_limit_pool_default
    assert np.array(soft_limit_pool).shape == (
        3,
        len(self.act_idxq),
    ), "input a vector of soft limit pool with a shape of (3, len(activejoints)"
    lim_q = soft_limit_pool[0, :]
    lim_dq = soft_limit_pool[1, :]
    lim_ddq = soft_limit_pool[2, :]

    new_upper_q = np.zeros_like(self.upper_q)
    new_lower_q = np.zeros_like(self.lower_q)
    new_upper_dq = np.zeros_like(self.upper_dq)
    new_lower_dq = np.zeros_like(self.lower_dq)
    new_upper_ddq = np.zeros_like(self.upper_dq)
    new_lower_ddq = np.zeros_like(self.lower_dq)

    for i in range(len(self.act_idxq)):
        new_upper_q[i] = self.upper_q[i] - lim_q[i] * abs(
            self.upper_q[i] - self.lower_q[i]
        )
        new_lower_q[i] = self.lower_q[i] + lim_q[i] * abs(
            self.upper_q[i] - self.lower_q[i]
        )

        step_q = (new_upper_q[i] - new_lower_q[i]) / (self.n_set - 1)
        self.pool_q[:, i] = np.array(
            [new_lower_q[i] + j * step_q for j in range(self.n_set)]
        )

    for i in range(len(self.act_idxv)):
        new_upper_dq[i] = self.upper_dq[i] - lim_dq[i] * abs(
            self.upper_dq[i] - self.lower_dq[i]
        )
        new_lower_dq[i] = self.lower_dq[i] + lim_dq[i] * abs(
            self.upper_dq[i] - self.lower_dq[i]
        )

        new_upper_ddq[i] = k * (
            self.upper_dq[i] - lim_ddq[i] * abs(self.upper_dq[i] - self.lower_dq[i])
        )  # Fixed line break
        new_lower_ddq[i] = k * (
            self.lower_dq[i] + lim_ddq[i] * abs(self.upper_dq[i] - self.lower_dq[i])
        )  # Fixed line break

        step_dq = (new_upper_dq[i] - new_lower_dq[i]) / (self.n_set - 1)
        self.pool_dq[:, i] = np.array(
            [new_lower_dq[i] + j * step_dq for j in range(self.n_set)]
        )

        step_ddq = (new_upper_ddq[i] - new_lower_ddq[i]) / (self.n_set - 1)
        self.pool_ddq[:, i] = np.array(
            [new_lower_ddq[i] + j * step_ddq for j in range(self.n_set)]
        )
```

### `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

Source code in `src/figaroh/utils/cubic_spline.py`

```
def gen_rand_wp(
    self,
    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
    """
    wps_rand = np.zeros((self.num_waypoints, len(self.act_idxq)))
    vel_wps_rand = np.zeros((self.num_waypoints, len(self.act_idxv)))
    acc_wps_rand = np.zeros((self.num_waypoints, len(self.act_idxv)))
    if wp_init is not None:
        wps_rand[0, :] = wp_init
        for i in range(len(self.act_idxq)):
            repeat_ = True
            while repeat_:
                wps_rand[range(1, self.num_waypoints), i] = np.random.choice(
                    self.pool_q[:, i], self.num_waypoints - 1
                )
                repeat_ = self.check_repeat_wp(
                    list(wps_rand[range(1, self.num_waypoints), i])
                )
    else:
        for i in range(len(self.act_idxq)):
            repeat_ = True
            while repeat_:
                wps_rand[:, i] = np.random.choice(
                    self.pool_q[:, i], self.num_waypoints
                )
                repeat_ = self.check_repeat_wp(list(wps_rand[:, i]))
    if vel_wp_init is not None:
        vel_wps_rand[0, :] = vel_wp_init
        if not vel_set_zero:
            for i in range(len(self.act_idxv)):
                repeat_ = True
                while repeat_:
                    vel_wps_rand[range(1, self.num_waypoints), i] = (
                        np.random.choice(self.pool_dq[:, i], self.num_waypoints - 1)
                    )
                    repeat_ = self.check_repeat_wp(
                        list(vel_wps_rand[range(1, self.num_waypoints), i])
                    )
    else:
        if not vel_set_zero:
            for i in range(len(self.act_idxv)):
                repeat_ = True
                while repeat_:
                    vel_wps_rand[:, i] = np.random.choice(
                        self.pool_dq[:, i], self.num_waypoints
                    )
                    repeat_ = self.check_repeat_wp(list(vel_wps_rand[:, i]))
    if vel_wp_init is not None:
        acc_wps_rand[0, :] = vel_wp_init
        if not acc_set_zero:
            for i in range(len(self.act_idxv)):
                repeat_ = True
                while repeat_:
                    acc_wps_rand[range(1, self.num_waypoints), i] = (
                        np.random.choice(
                            self.pool_ddq[:, i], self.num_waypoints - 1
                        )
                    )
                    repeat_ = self.check_repeat_wp(
                        list(acc_wps_rand[range(1, self.num_waypoints), i])
                    )
    else:
        if not acc_set_zero:
            for i in range(len(self.act_idxv)):
                repeat_ = True
                while repeat_:
                    acc_wps_rand[:, i] = np.random.choice(
                        self.pool_ddq[:, i], self.num_waypoints
                    )
                    repeat_ = self.check_repeat_wp(list(acc_wps_rand[:, i]))
    return wps_rand.transpose(), vel_wps_rand.transpose(), acc_wps_rand.transpose()
```

### `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

Source code in `src/figaroh/utils/cubic_spline.py`

```
def gen_equal_wp(self, 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
    """
    wps_equal = np.zeros((self.num_waypoints, len(self.act_idxq)))
    vel_wps_equal = np.zeros((self.num_waypoints, len(self.act_idxv)))
    acc_wps_equal = np.zeros((self.num_waypoints, len(self.act_idxv)))

    step_q = (self.upper_q - self.lower_q) / 20
    if wp_init is not None:
        wps_equal = np.tile(wp_init, (self.num_waypoints, 1))
        for jj in range(1, self.num_waypoints):
            wps_equal[jj, :] = wps_equal[jj - 1, :] + step_q

    if vel_wp_init is not None:
        vel_wps_equal = np.tile(vel_wp_init, (self.num_waypoints, 1))

    if acc_wp_init is not None:
        acc_wps_equal = np.tile(acc_wp_init, (self.num_waypoints, 1))
    return (
        wps_equal.transpose(),
        vel_wps_equal.transpose(),
        acc_wps_equal.transpose(),
    )
```

## Results Manager

Shared fallback-plotting helper and the joint-major torque-plotting fix used by both calibration and identification (`plot_with_fallback`, `plot_calibration_results`, `plot_identification_results`).

Unified results management for FIGAROH examples.

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

## `ResultsManager(task_type, robot_name='robot', results_data=None)`

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.

Initialize results manager.

Parameters:

| Name           | Type                       | Description                                                                                 | Default    |
| -------------- | -------------------------- | ------------------------------------------------------------------------------------------- | ---------- |
| `task_type`    | `str`                      | Type of task ('calibration', 'identification', 'optimal_calibration', 'optimal_trajectory') | *required* |
| `robot_name`   | `str`                      | Name of the robot for file naming                                                           | `'robot'`  |
| `results_data` | `Optional[Dict[str, Any]]` | Optional dictionary of results data                                                         | `None`     |

Source code in `src/figaroh/utils/results_manager.py`

```
def __init__(
    self,
    task_type: str,
    robot_name: str = "robot",
    results_data: Optional[Dict[str, Any]] = None,
):
    """
    Initialize results manager.

    Args:
        task_type: Type of task ('calibration', 'identification',
              'optimal_calibration', 'optimal_trajectory')
        robot_name: Name of the robot for file naming
        results_data: Optional dictionary of results data
    """
    self.task_type = task_type.lower()
    self.robot_name = robot_name
    assert (
        self.task_type in self.PLOT_STYLES
    ), f"Unsupported task type: {self.task_type}"
    assert (
        self.task_type == results_data["task type"] if results_data else True
    ), "Results data must match task type"
    self.result = results_data or {}
    self.logger = setup_example_logging()

    if not HAS_MATPLOTLIB:
        self.logger.warning("Matplotlib not available. Plotting disabled.")
```

### `plot_calibration_results(measured_poses=None, estimated_poses=None, residuals=None, parameter_values=None, parameter_names=None, outlier_indices=None, title='Calibration Results')`

Plot calibration results with pose comparison and residual analysis.

Parameters:

| Name               | Type                  | Description                                                 | Default                 |
| ------------------ | --------------------- | ----------------------------------------------------------- | ----------------------- |
| `measured_poses`   | `Optional[ndarray]`   | Measured poses (optional, uses self.result)                 | `None`                  |
| `estimated_poses`  | `Optional[ndarray]`   | Estimated poses (optional, uses self.result)                | `None`                  |
| `residuals`        | `Optional[ndarray]`   | Position/orientation residuals (optional, uses self.result) | `None`                  |
| `parameter_values` | `Optional[ndarray]`   | Calibrated parameters (optional, uses self.result)          | `None`                  |
| `parameter_names`  | `Optional[List[str]]` | Parameter names (optional, uses self.result)                | `None`                  |
| `outlier_indices`  | `Optional[List[int]]` | Outlier indices (optional, uses self.result)                | `None`                  |
| `title`            | `str`                 | Plot title                                                  | `'Calibration Results'` |

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_calibration_results(
    self,
    measured_poses: Optional[np.ndarray] = None,
    estimated_poses: Optional[np.ndarray] = None,
    residuals: Optional[np.ndarray] = None,
    parameter_values: Optional[np.ndarray] = None,
    parameter_names: Optional[List[str]] = None,
    outlier_indices: Optional[List[int]] = None,
    title: str = "Calibration Results",
) -> None:
    """
    Plot calibration results with pose comparison and residual analysis.

    Args:
        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
    """
    if not HAS_MATPLOTLIB:
        self.logger.warning("Cannot plot: matplotlib not available")
        return

    try:
        # Extract data from self.result if not provided as arguments
        if measured_poses is None:
            measured_poses = self.result.get(
                "PEE measured (2D array)", np.array([])
            )
        if estimated_poses is None:
            estimated_poses = self.result.get(
                "PEE estimated (2D array)", np.array([])
            )
        if residuals is None:
            residuals = self.result.get("residuals", np.array([]))
        if parameter_values is None:
            param_vals = self.result.get("calibrated parameters values", [])
            parameter_values = np.array(param_vals) if param_vals else np.array([])
        if parameter_names is None:
            parameter_names = self.result.get("calibrated parameters names", [])
        if outlier_indices is None:
            outlier_indices = self.result.get("outlier indices", [])

        # Validate we have the minimum required data
        if measured_poses.size == 0:
            self.logger.warning("No measured pose data available for plotting")
            return

        fig = plt.figure(figsize=self.PLOT_STYLES["calibration"]["figsize"])
        gs = GridSpec(3, 2, figure=fig, hspace=0.3, wspace=0.3)

        # Check position + orientation (6 DOF) or position only (3 DOF)
        n_dims = measured_poses.shape[1]

        if n_dims >= 3 and estimated_poses.size > 0:
            # Position comparison
            ax1 = fig.add_subplot(gs[0, :])
            self._plot_pose_comparison(
                ax1,
                measured_poses[:, :3],
                estimated_poses[:, :3],
                "Position Comparison",
                "Position (m)",
                outlier_indices,
            )

        if n_dims >= 6 and estimated_poses.size > 0:
            # Orientation comparison
            ax2 = fig.add_subplot(gs[1, :])
            self._plot_pose_comparison(
                ax2,
                measured_poses[:, 3:6],
                estimated_poses[:, 3:6],
                "Orientation Comparison",
                "Orientation (rad)",
                outlier_indices,
            )
        elif n_dims >= 3 and residuals.size > 0:
            # Position residuals if we don't have orientation data
            ax2 = fig.add_subplot(gs[1, :])
            self._plot_position_residuals(ax2, residuals[:, :3], outlier_indices)

        # Residual analysis
        if residuals.size > 0:
            ax3 = fig.add_subplot(gs[2, 0])
            self._plot_residuals(ax3, residuals, outlier_indices)

        # Parameter values
        if parameter_values.size > 0:
            ax5 = fig.add_subplot(gs[2, 1])
            self._plot_parameters(ax5, parameter_values, parameter_names)

            # Add statistics text
            num_params = self.result.get(
                "number of calibrated parameters", len(parameter_values)
            )
            num_samples = self.result.get("number of samples", "N/A")
            rmse = self.result.get("rmse", "N/A")
            mae = self.result.get("mae", "N/A")

            stats_text = f"Parameters: {num_params}\n" f"Samples: {num_samples}"
            if rmse != "N/A":
                stats_text += f"\nRMSE: {rmse:.6f}"
            if mae != "N/A":
                stats_text += f"\nMAE: {mae:.6f}"

            ax5.text(
                0.02,
                0.98,
                stats_text,
                transform=ax5.transAxes,
                verticalalignment="top",
                bbox=dict(boxstyle="round", facecolor="wheat", alpha=0.5),
            )

        fig.suptitle(f"{self.robot_name.upper()} {title}", fontsize=16)
        # plt.tight_layout()
        plt.show()

    except Exception as e:
        self.logger.error(f"Error plotting calibration results: {e}")
        # Log debug information
        logger.debug("Debug info:")
        measured_shape = (
            measured_poses.shape if hasattr(measured_poses, "shape") else "N/A"
        )
        logger.debug(f"  measured_poses shape: {measured_shape}")

        estimated_shape = (
            estimated_poses.shape if hasattr(estimated_poses, "shape") else "N/A"
        )
        logger.debug(f"  estimated_poses shape: {estimated_shape}")

        residuals_shape = residuals.shape if hasattr(residuals, "shape") else "N/A"
        logger.debug(f"  residuals shape: {residuals_shape}")

        result_keys = list(self.result.keys()) if self.result else "None"
        logger.debug(f"  Available result keys: {result_keys}")
```

### `plot_identification_results(tau_measured=None, tau_identified=None, parameter_values=None, parameter_names=None, time_vector=None, joint_names=None, n_joints=None, title='Identification Results')`

Plot identification results with torque comparison and analysis.

Parameters:

| Name               | Type                  | Description                                                                                                                                                                                                       | Default                    |
| ------------------ | --------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------- |
| `tau_measured`     | `Optional[ndarray]`   | Measured joint torques (optional, uses self.result)                                                                                                                                                               | `None`                     |
| `tau_identified`   | `Optional[ndarray]`   | Estimated torques (optional, uses self.result)                                                                                                                                                                    | `None`                     |
| `parameter_values` | `Optional[ndarray]`   | Parameter values (optional, uses self.result)                                                                                                                                                                     | `None`                     |
| `parameter_names`  | `Optional[List[str]]` | Parameter names (optional, uses self.result)                                                                                                                                                                      | `None`                     |
| `time_vector`      | `Optional[ndarray]`   | Time vector for plots                                                                                                                                                                                             | `None`                     |
| `joint_names`      | `Optional[List[str]]` | Names of robot joints                                                                                                                                                                                             | `None`                     |
| `n_joints`         | `Optional[int]`       | Number of active joints. Required when a torque array is 1D, since a flattened array is joint-major (all samples of joint 0, then joint 1, ...) and cannot be reshaped correctly without knowing the joint count. | `None`                     |
| `title`            | `str`                 | Plot title                                                                                                                                                                                                        | `'Identification Results'` |

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_identification_results(
    self,
    tau_measured: Optional[np.ndarray] = None,
    tau_identified: Optional[np.ndarray] = None,
    parameter_values: Optional[np.ndarray] = None,
    parameter_names: Optional[List[str]] = None,
    time_vector: Optional[np.ndarray] = None,
    joint_names: Optional[List[str]] = None,
    n_joints: Optional[int] = None,
    title: str = "Identification Results",
) -> None:
    """
    Plot identification results with torque comparison and analysis.

    Args:
        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
        n_joints: Number of active joints. Required when a torque array is
            1D, since a flattened array is joint-major (all samples of
            joint 0, then joint 1, ...) and cannot be reshaped correctly
            without knowing the joint count.
        title: Plot title
    """
    if not HAS_MATPLOTLIB:
        self.logger.warning("Cannot plot: matplotlib not available")
        return

    try:
        # Use data from self.result if parameters not provided
        if tau_measured is None:
            tau_measured = self.result.get("torque processed", np.array([]))
        if tau_identified is None:
            tau_identified = self.result.get("torque estimated", np.array([]))
        if parameter_values is None:
            parameter_values = self.result.get(
                "base parameters values", np.array([])
            )
        if parameter_names is None:
            parameter_names = self.result.get("base parameters names", [])
        if joint_names is None:
            joint_names = self.result.get("active joints", None)
        if n_joints is None:
            n_joints = self.result.get("n_active_joints", None)

        tau_measured = np.asarray(tau_measured)
        tau_identified = np.asarray(tau_identified)

        # Validate data availability
        if tau_measured.size == 0 or tau_identified.size == 0:
            self.logger.error("No torque data available for plotting")
            return

        fig = plt.figure(figsize=self.PLOT_STYLES["identification"]["figsize"])
        gs = GridSpec(2, 2, figure=fig, hspace=0.3, wspace=0.3)

        # A 1D array is joint-major (all samples of joint 0, then joint
        # 1, ...), not sample-major — reshape(-1, 1) would silently
        # treat the whole concatenated array as a single joint. Require
        # n_joints to reshape correctly; without it, fall back to a
        # single-column plot rather than guessing.
        if tau_measured.ndim == 1:
            if n_joints:
                tau_measured = tau_measured.reshape(n_joints, -1).T
            else:
                tau_measured = tau_measured.reshape(-1, 1)
        if tau_identified.ndim == 1:
            if n_joints:
                tau_identified = tau_identified.reshape(n_joints, -1).T
            else:
                tau_identified = tau_identified.reshape(-1, 1)

        n_samples = tau_measured.shape[0]

        if time_vector is None:
            time_vector = np.arange(n_samples)

        # Torque comparison
        ax1 = fig.add_subplot(gs[0, :])
        self._plot_torque_comparison(
            ax1, time_vector, tau_measured, tau_identified, joint_names
        )

        # Residual analysis
        ax2 = fig.add_subplot(gs[1, :])
        residuals = tau_measured - tau_identified
        self._plot_torque_residuals(ax2, time_vector, residuals, joint_names)

        # Parameter values
        # ax3 = fig.add_subplot(gs[1, 1])
        # self._plot_parameters(ax3, parameter_values, parameter_names)

        # Add quality metrics to title if available
        condition_num = self.result.get("condition number", "N/A")
        rmse_norm = self.result.get("rmse norm (N/m)", "N/A")

        fig.suptitle(
            f"{self.robot_name.upper()} {title}\n"
            f"Condition: {condition_num:.2e} | "
            f"RMSE: {rmse_norm:.6f}",
            fontsize=16,
        )
        # plt.tight_layout()
        plt.show()

    except Exception as e:
        self.logger.error(f"Error plotting identification results: {e}")
        logger.debug("Debug info:", exc_info=True)
```

### `plot_optimal_calibration_results(configurations, weights, condition_numbers=None, information_matrix=None, title='Optimal Calibration Results')`

Plot optimal calibration configuration results.

Parameters:

| Name                 | Type                 | Description                            | Default                         |
| -------------------- | -------------------- | -------------------------------------- | ------------------------------- |
| `configurations`     | `Dict[str, ndarray]` | Dictionary of configuration data       | *required*                      |
| `weights`            | `ndarray`            | Weights assigned to each configuration | *required*                      |
| `condition_numbers`  | `Optional[ndarray]`  | Condition numbers for analysis         | `None`                          |
| `information_matrix` | `Optional[ndarray]`  | Fisher information matrix              | `None`                          |
| `title`              | `str`                | Plot title                             | `'Optimal Calibration Results'` |

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_optimal_calibration_results(
    self,
    configurations: Dict[str, np.ndarray],
    weights: np.ndarray,
    condition_numbers: Optional[np.ndarray] = None,
    information_matrix: Optional[np.ndarray] = None,
    title: str = "Optimal Calibration Results",
) -> None:
    """
    Plot optimal calibration configuration results.

    Args:
        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
    """
    if not HAS_MATPLOTLIB:
        self.logger.warning("Cannot plot: matplotlib not available")
        return

    try:
        fig = plt.figure(figsize=self.PLOT_STYLES["optimal_calibration"]["figsize"])
        gs = GridSpec(2, 2, figure=fig, hspace=0.3, wspace=0.3)

        # Configuration weights
        ax1 = fig.add_subplot(gs[0, 0])
        self._plot_configuration_weights(ax1, weights)

        # Selected configurations visualization
        ax2 = fig.add_subplot(gs[0, 1])
        self._plot_selected_configurations(ax2, configurations, weights)

        # Condition number analysis (if available)
        if condition_numbers is not None:
            ax3 = fig.add_subplot(gs[1, 0])
            self._plot_condition_analysis(ax3, condition_numbers)

        # Information matrix visualization (if available)
        if information_matrix is not None:
            ax4 = fig.add_subplot(gs[1, 1])
            self._plot_information_matrix(ax4, information_matrix)

        fig.suptitle(f"{self.robot_name.upper()} {title}", fontsize=16)
        plt.tight_layout()
        plt.show()

    except Exception as e:
        self.logger.error(f"Error plotting optimal calibration results: {e}")
```

### `plot_optimal_trajectory_results(trajectories, condition_number, joint_names=None, title='Optimal Trajectory Results', q_indices=None, v_indices=None)`

Plot optimal trajectory generation results.

Positions span the model's `nq` coordinates and velocities and accelerations its `nv` coordinates, which differ for continuous, spherical or free-flyer joints. Pass `q_indices` and `v_indices` (e.g. each plotted joint's `idx_q` and `idx_v`) to plot those joints; without them every column is plotted, which requires `nq == nv`.

Parameters:

| Name               | Type                  | Description                                        | Default                        |
| ------------------ | --------------------- | -------------------------------------------------- | ------------------------------ |
| `trajectories`     | `Dict[str, Any]`      | Dictionary containing trajectory data              | *required*                     |
| `condition_number` | `float`               | Final condition number achieved                    | *required*                     |
| `joint_names`      | `Optional[List[str]]` | Names of the plotted joints, used as row labels    | `None`                         |
| `title`            | `str`                 | Plot title                                         | `'Optimal Trajectory Results'` |
| `q_indices`        | `Optional[List[int]]` | Position column of each plotted joint              | `None`                         |
| `v_indices`        | `Optional[List[int]]` | Velocity/acceleration column of each plotted joint | `None`                         |

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_optimal_trajectory_results(
    self,
    trajectories: Dict[str, Any],
    condition_number: float,
    joint_names: Optional[List[str]] = None,
    title: str = "Optimal Trajectory Results",
    q_indices: Optional[List[int]] = None,
    v_indices: Optional[List[int]] = None,
) -> None:
    """
    Plot optimal trajectory generation results.

    Positions span the model's ``nq`` coordinates and velocities and
    accelerations its ``nv`` coordinates, which differ for continuous,
    spherical or free-flyer joints. Pass ``q_indices`` and ``v_indices``
    (e.g. each plotted joint's ``idx_q`` and ``idx_v``) to plot those
    joints; without them every column is plotted, which requires
    ``nq == nv``.

    Args:
        trajectories: Dictionary containing trajectory data
        condition_number: Final condition number achieved
        joint_names: Names of the plotted joints, used as row labels
        title: Plot title
        q_indices: Position column of each plotted joint
        v_indices: Velocity/acceleration column of each plotted joint
    """
    if not HAS_MATPLOTLIB:
        self.logger.warning("Cannot plot: matplotlib not available")
        return
    if (q_indices is None) != (v_indices is None) or (
        q_indices is not None and len(q_indices) != len(v_indices)
    ):
        raise ValueError(
            "q_indices and v_indices must be given together, one per joint"
        )

    try:
        # Extract trajectory data
        if "T_F" in trajectories:
            # Multiple segments format
            times = trajectories["T_F"]
            positions = trajectories["P_F"]
            velocities = trajectories["V_F"]
            accelerations = trajectories["A_F"]
        else:
            # Single trajectory format
            times = [
                trajectories.get("time", np.arange(trajectories["q"].shape[0]))
            ]
            positions = [trajectories["q"]]
            velocities = [trajectories.get("dq", np.zeros_like(trajectories["q"]))]
            accelerations = [
                trajectories.get("ddq", np.zeros_like(trajectories["q"]))
            ]

        if q_indices is None:
            nq, nv = positions[0].shape[1], velocities[0].shape[1]
            if nq != nv:
                self.logger.error(
                    "Cannot plot trajectory: positions have %d columns and "
                    "velocities %d (nq != nv); pass q_indices and v_indices",
                    nq,
                    nv,
                )
                return
            q_indices = v_indices = list(range(nq))
        n_joints = len(q_indices)
        if joint_names is None or len(joint_names) != n_joints:
            joint_names = [f"Joint {i+1}" for i in range(n_joints)]

        fig = plt.figure(figsize=self.PLOT_STYLES["optimal_trajectory"]["figsize"])
        gs = GridSpec(n_joints, 3, figure=fig, hspace=0.4, wspace=0.3)

        # Plot each joint's trajectory
        colors = plt.cm.tab10(np.linspace(0, 1, len(times)))

        for joint_idx, (iq, iv, name) in enumerate(
            zip(q_indices, v_indices, joint_names)
        ):
            # Position
            ax_pos = fig.add_subplot(gs[joint_idx, 0])
            for seg_idx, (t, pos) in enumerate(zip(times, positions)):
                ax_pos.plot(
                    t,
                    pos[:, iq],
                    color=colors[seg_idx],
                    label=f"Segment {seg_idx+1}" if joint_idx == 0 else "",
                )
            ax_pos.set_ylabel(f"{name}\nPosition (rad)")
            ax_pos.grid(True, alpha=0.3)
            if joint_idx == 0:
                ax_pos.legend()
                ax_pos.set_title("Joint Positions")

            # Velocity
            ax_vel = fig.add_subplot(gs[joint_idx, 1])
            for seg_idx, (t, vel) in enumerate(zip(times, velocities)):
                ax_vel.plot(t, vel[:, iv], color=colors[seg_idx])
            ax_vel.set_ylabel(f"{name}\nVelocity (rad/s)")
            ax_vel.grid(True, alpha=0.3)
            if joint_idx == 0:
                ax_vel.set_title("Joint Velocities")

            # Acceleration
            ax_acc = fig.add_subplot(gs[joint_idx, 2])
            for seg_idx, (t, acc) in enumerate(zip(times, accelerations)):
                ax_acc.plot(t, acc[:, iv], color=colors[seg_idx])
            ax_acc.set_ylabel(f"{name}\nAcceleration (rad/s²)")
            ax_acc.grid(True, alpha=0.3)
            if joint_idx == 0:
                ax_acc.set_title("Joint Accelerations")

        # Label the bottom row's existing axes (a new add_subplot on the
        # same cell would stack an empty axes on top of them).
        for ax in (ax_pos, ax_vel, ax_acc):
            ax.set_xlabel("Time (s)")

        fig.suptitle(
            f"{self.robot_name.upper()} {title}\nCondition Number: {condition_number:.2e}",
            fontsize=16,
        )
        plt.tight_layout()
        plt.show()

    except Exception as e:
        self.logger.error(f"Error plotting trajectory results: {e}")
```

### `save_results(results=None, output_dir='results', file_prefix=None, save_formats=['yaml', 'csv'])`

Save results to files with standardized format.

Parameters:

| Name           | Type                       | Description                                                      | Default           |
| -------------- | -------------------------- | ---------------------------------------------------------------- | ----------------- |
| `results`      | `Optional[Dict[str, Any]]` | Dictionary containing results to save (uses self.result if None) | `None`            |
| `output_dir`   | `str`                      | Output directory path                                            | `'results'`       |
| `file_prefix`  | `Optional[str]`            | Prefix for output files (default: robot_name_task_type)          | `None`            |
| `save_formats` | `List[str]`                | List of formats to save ('yaml', 'csv', 'json', 'npz')           | `['yaml', 'csv']` |

Returns:

| Type             | Description                            |
| ---------------- | -------------------------------------- |
| `Dict[str, str]` | Dictionary mapping format to file path |

Source code in `src/figaroh/utils/results_manager.py`

```
def save_results(
    self,
    results: Optional[Dict[str, Any]] = None,
    output_dir: str = "results",
    file_prefix: Optional[str] = None,
    save_formats: List[str] = ["yaml", "csv"],
) -> Dict[str, str]:
    """
    Save results to files with standardized format.

    Args:
        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
    """
    try:
        # Use self.result if no results provided
        if results is None:
            results = self.result

        if not results:
            self.logger.error("No results data available to save")
            return {}

        # Create output directory
        output_path = Path(output_dir)
        output_path.mkdir(parents=True, exist_ok=True)

        # Generate file prefix
        if file_prefix is None:
            timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
            file_prefix = f"{self.robot_name}_{self.task_type}_{timestamp}"

        saved_files = {}

        # Save in requested formats
        for fmt in save_formats:
            if fmt.lower() == "yaml":
                file_path = output_path / f"{file_prefix}.yaml"
                self._save_yaml(results, file_path)
                saved_files["yaml"] = str(file_path)

            elif fmt.lower() == "csv":
                file_path = output_path / f"{file_prefix}.csv"
                self._save_csv(results, file_path)
                saved_files["csv"] = str(file_path)

            elif fmt.lower() == "json":
                file_path = output_path / f"{file_prefix}.json"
                self._save_json(results, file_path)
                saved_files["json"] = str(file_path)

            elif fmt.lower() == "npz":
                file_path = output_path / f"{file_prefix}.npz"
                self._save_npz(results, file_path)
                saved_files["npz"] = str(file_path)

        # Save metadata
        metadata_path = output_path / f"{file_prefix}_metadata.yaml"
        self._save_metadata(results, metadata_path)
        saved_files["metadata"] = str(metadata_path)

        self.logger.info(f"Results saved to {output_dir}")
        for fmt, path in saved_files.items():
            self.logger.info(f"  {fmt.upper()}: {path}")

        return saved_files

    except Exception as e:
        self.logger.error(f"Error saving results: {e}")
        raise FigarohExampleError(f"Failed to save results: {e}")
```

## `plot_with_fallback(primary, fallback, logger, context)`

Run `primary()`; on any exception, log it and run `fallback()`.

Shared by every `Base*.plot_results()` method, each of which previously duplicated this same try/except pattern around a call into :class:`ResultsManager` with a raw-matplotlib fallback.

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_with_fallback(
    primary: Callable[[], None],
    fallback: Callable[[], None],
    logger: logging.Logger,
    context: str,
) -> None:
    """Run ``primary()``; on any exception, log it and run ``fallback()``.

    Shared by every ``Base*.plot_results()`` method, each of which
    previously duplicated this same try/except pattern around a call into
    :class:`ResultsManager` with a raw-matplotlib fallback.
    """
    try:
        primary()
    except Exception as e:
        logger.error(f"Error plotting with ResultsManager ({context}): {e}")
        logger.info("Falling back to basic plotting...")
        fallback()
```

## `plot_calibration_results(measured_poses, estimated_poses, residuals, **kwargs)`

Plot calibration results (convenience function).

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_calibration_results(measured_poses, estimated_poses, residuals, **kwargs):
    """Plot calibration results (convenience function)."""
    warnings.warn(
        "plot_calibration_results is deprecated in 1.0, will be removed in 2.0; use ResultsManager.plot_calibration_results",
        DeprecationWarning,
        stacklevel=2,
    )
    manager = ResultsManager("calibration", kwargs.get("robot_name", "robot"))
    manager.plot_calibration_results(
        measured_poses, estimated_poses, residuals, **kwargs
    )
```

## `plot_identification_results(tau_measured, tau_identified, parameters, **kwargs)`

Plot identification results (convenience function).

Source code in `src/figaroh/utils/results_manager.py`

```
def plot_identification_results(tau_measured, tau_identified, parameters, **kwargs):
    """Plot identification results (convenience function)."""
    warnings.warn(
        "plot_identification_results is deprecated in 1.0, will be removed in 2.0; use ResultsManager.plot_identification_results",
        DeprecationWarning,
        stacklevel=2,
    )
    manager = ResultsManager("identification", kwargs.get("robot_name", "robot"))
    manager.plot_identification_results(
        tau_measured, tau_identified, parameters, **kwargs
    )
```

## `save_results(results, task_type, output_dir='results', **kwargs)`

Save results (convenience function).

Source code in `src/figaroh/utils/results_manager.py`

```
def save_results(results, task_type, output_dir="results", **kwargs):
    """Save results (convenience function)."""
    warnings.warn(
        "save_results is deprecated in 1.0, will be removed in 2.0; use ResultsManager.save_results",
        DeprecationWarning,
        stacklevel=2,
    )
    manager = ResultsManager(task_type, kwargs.get("robot_name", "robot"))
    return manager.save_results(results, output_dir, **kwargs)
```

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

## `FigarohExampleError`

Bases: `Exception`

Base exception for FIGAROH examples.

## `RobotInitializationError`

Bases: `FigarohExampleError`

Exception raised when robot initialization fails.

## `ConfigurationError`

Bases: `FigarohExampleError`

Exception raised for configuration-related issues.

## `DataProcessingError`

Bases: `FigarohExampleError`

Exception raised during data processing operations.

## `CalibrationError`

Bases: `FigarohExampleError`

Exception raised during calibration procedures.

## `IdentificationError`

Bases: `FigarohExampleError`

Exception raised during identification procedures.

## `ValidationError`

Bases: `FigarohExampleError`

Exception raised when validation fails.

## `ErrorContext(operation_name, raise_on_error=True)`

Context manager for structured error handling.

Source code in `src/figaroh/utils/error_handling.py`

```
def __init__(self, operation_name: str, raise_on_error: bool = True):
    self.operation_name = operation_name
    self.raise_on_error = raise_on_error
    self.error = None
```

## `validate_robot_config(config)`

Validate robot configuration dictionary.

Parameters:

| Name     | Type             | Description                          | Default    |
| -------- | ---------------- | ------------------------------------ | ---------- |
| `config` | `Dict[str, Any]` | Configuration dictionary to validate | *required* |

Raises:

| Type              | Description                 |
| ----------------- | --------------------------- |
| `ValidationError` | If configuration is invalid |

Source code in `src/figaroh/utils/error_handling.py`

```
def validate_robot_config(config: Dict[str, Any]) -> None:
    """
    Validate robot configuration dictionary.

    Args:
        config: Configuration dictionary to validate

    Raises:
        ValidationError: If configuration is invalid
    """
    if not isinstance(config, dict):
        raise ValidationError("Configuration must be a dictionary")

    # Check for required fields
    required_fields = ["robot_name"]
    missing_fields = [field for field in required_fields if field not in config]

    if missing_fields:
        raise ValidationError(f"Missing required fields: {missing_fields}")
```

## `validate_trajectory_data(q, qd=None, qdd=None, tau=None)`

Validate trajectory data arrays.

Parameters:

| Name  | Type                | Description                    | Default    |
| ----- | ------------------- | ------------------------------ | ---------- |
| `q`   | `ndarray`           | Joint positions                | *required* |
| `qd`  | `Optional[ndarray]` | Joint velocities (optional)    | `None`     |
| `qdd` | `Optional[ndarray]` | Joint accelerations (optional) | `None`     |
| `tau` | `Optional[ndarray]` | Joint torques (optional)       | `None`     |

Raises:

| Type              | Description        |
| ----------------- | ------------------ |
| `ValidationError` | If data is invalid |

Source code in `src/figaroh/utils/error_handling.py`

```
def validate_trajectory_data(
    q: np.ndarray,
    qd: Optional[np.ndarray] = None,
    qdd: Optional[np.ndarray] = None,
    tau: Optional[np.ndarray] = None,
) -> None:
    """
    Validate trajectory data arrays.

    Args:
        q: Joint positions
        qd: Joint velocities (optional)
        qdd: Joint accelerations (optional)
        tau: Joint torques (optional)

    Raises:
        ValidationError: If data is invalid
    """
    if not isinstance(q, np.ndarray):
        raise ValidationError("Joint positions must be numpy array")

    if q.ndim != 2:
        raise ValidationError("Joint positions must be 2D array (n_samples, n_joints)")

    n_samples, n_joints = q.shape

    # Validate other arrays if provided
    arrays_to_check = [("velocities", qd), ("accelerations", qdd), ("torques", tau)]

    for name, array in arrays_to_check:
        if array is not None:
            if not isinstance(array, np.ndarray):
                raise ValidationError(f"{name} must be numpy array")

            if array.shape != (n_samples, n_joints):
                raise ValidationError(
                    f"{name} shape {array.shape} doesn't match "
                    f"positions shape {(n_samples, n_joints)}"
                )

            if np.any(np.isnan(array)):
                raise ValidationError(f"{name} contains NaN values")

            if np.any(np.isinf(array)):
                raise ValidationError(f"{name} contains infinite values")
```

## `validate_numeric_range(value, min_val=None, max_val=None, name='value')`

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

Parameters:

| Name      | Type                         | Description                          | Default    |
| --------- | ---------------------------- | ------------------------------------ | ---------- |
| `value`   | `Union[float, int, ndarray]` | Value or array to validate           | *required* |
| `min_val` | `Optional[float]`            | Minimum allowed value                | `None`     |
| `max_val` | `Optional[float]`            | Maximum allowed value                | `None`     |
| `name`    | `str`                        | Name of the value for error messages | `'value'`  |

Raises:

| Type              | Description              |
| ----------------- | ------------------------ |
| `ValidationError` | If value is out of range |

Source code in `src/figaroh/utils/error_handling.py`

```
def validate_numeric_range(
    value: Union[float, int, np.ndarray],
    min_val: Optional[float] = None,
    max_val: Optional[float] = None,
    name: str = "value",
) -> None:
    """
    Validate that numeric value(s) are within specified range.

    Args:
        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
    """
    if isinstance(value, np.ndarray):
        if min_val is not None and np.any(value < min_val):
            raise ValidationError(f"{name} contains values below {min_val}")

        if max_val is not None and np.any(value > max_val):
            raise ValidationError(f"{name} contains values above {max_val}")
    else:
        if min_val is not None and value < min_val:
            raise ValidationError(f"{name} {value} is below minimum {min_val}")

        if max_val is not None and value > max_val:
            raise ValidationError(f"{name} {value} is above maximum {max_val}")
```

## `validate_robot_initialization(func)`

Decorator to validate robot initialization.

Parameters:

| Name   | Type | Description          | Default    |
| ------ | ---- | -------------------- | ---------- |
| `func` | `F`  | Function to decorate | *required* |

Returns:

| Type | Description                              |
| ---- | ---------------------------------------- |
| `F`  | Decorated function with robot validation |

Source code in `src/figaroh/utils/error_handling.py`

```
def validate_robot_initialization(func: F) -> F:
    """
    Decorator to validate robot initialization.

    Args:
        func: Function to decorate

    Returns:
        Decorated function with robot validation
    """

    @functools.wraps(func)
    def wrapper(*args, **kwargs):
        try:
            return func(*args, **kwargs)
        except Exception as e:
            raise RobotInitializationError(f"Robot initialization failed: {e}") from e

    return wrapper
```

## `validate_input_data(func)`

Decorator to validate input data for processing functions.

Parameters:

| Name   | Type | Description          | Default    |
| ------ | ---- | -------------------- | ---------- |
| `func` | `F`  | Function to decorate | *required* |

Returns:

| Type | Description                              |
| ---- | ---------------------------------------- |
| `F`  | Decorated function with input validation |

Source code in `src/figaroh/utils/error_handling.py`

```
def validate_input_data(func: F) -> F:
    """
    Decorator to validate input data for processing functions.

    Args:
        func: Function to decorate

    Returns:
        Decorated function with input validation
    """

    @functools.wraps(func)
    def wrapper(*args, **kwargs):
        try:
            # Basic validation - can be extended based on function signature
            for arg in args:
                if isinstance(arg, np.ndarray):
                    if np.any(np.isnan(arg)):
                        raise ValidationError("Input data contains NaN values")
                    if np.any(np.isinf(arg)):
                        raise ValidationError("Input data contains infinite values")

            return func(*args, **kwargs)
        except ValidationError:
            raise
        except Exception as e:
            raise DataProcessingError(f"Data processing failed: {e}") from e

    return wrapper
```

## `handle_calibration_errors(func)`

Decorator to handle calibration-specific errors.

Parameters:

| Name   | Type | Description          | Default    |
| ------ | ---- | -------------------- | ---------- |
| `func` | `F`  | Function to decorate | *required* |

Returns:

| Type | Description                                        |
| ---- | -------------------------------------------------- |
| `F`  | Decorated function with calibration error handling |

Source code in `src/figaroh/utils/error_handling.py`

```
def handle_calibration_errors(func: F) -> F:
    """
    Decorator to handle calibration-specific errors.

    Args:
        func: Function to decorate

    Returns:
        Decorated function with calibration error handling
    """

    @functools.wraps(func)
    def wrapper(*args, **kwargs):
        try:
            return func(*args, **kwargs)
        except (ValueError, np.linalg.LinAlgError) as e:
            raise CalibrationError(f"Calibration failed: {e}") from e
        except Exception as e:
            logger.error(f"Unexpected error in calibration: {e}")
            raise CalibrationError(f"Unexpected calibration error: {e}") from e

    return wrapper
```

## `handle_identification_errors(func)`

Decorator to handle identification-specific errors.

Parameters:

| Name   | Type | Description          | Default    |
| ------ | ---- | -------------------- | ---------- |
| `func` | `F`  | Function to decorate | *required* |

Returns:

| Type | Description                                           |
| ---- | ----------------------------------------------------- |
| `F`  | Decorated function with identification error handling |

Source code in `src/figaroh/utils/error_handling.py`

```
def handle_identification_errors(func: F) -> F:
    """
    Decorator to handle identification-specific errors.

    Args:
        func: Function to decorate

    Returns:
        Decorated function with identification error handling
    """

    @functools.wraps(func)
    def wrapper(*args, **kwargs):
        try:
            return func(*args, **kwargs)
        except (ValueError, np.linalg.LinAlgError) as e:
            raise IdentificationError(f"Identification failed: {e}") from e
        except Exception as e:
            logger.error(f"Unexpected error in identification: {e}")
            raise IdentificationError(f"Unexpected identification error: {e}") from e

    return wrapper
```

## `safe_execute(func, *args, **kwargs)`

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

Parameters:

| Name       | Type | Description                           | Default    |
| ---------- | ---- | ------------------------------------- | ---------- |
| `func`     | `F`  | Function to execute                   | *required* |
| `*args`    |      | Positional arguments for the function | `()`       |
| `**kwargs` |      | Keyword arguments for the function    | `{}`       |

Returns:

| Type    | Description                                  |
| ------- | -------------------------------------------- |
| `tuple` | Tuple of (success_flag, result_or_exception) |

Source code in `src/figaroh/utils/error_handling.py`

```
def safe_execute(func: F, *args, **kwargs) -> tuple:
    """
    Safely execute a function and return (success, result_or_error).

    Args:
        func: Function to execute
        *args: Positional arguments for the function
        **kwargs: Keyword arguments for the function

    Returns:
        Tuple of (success_flag, result_or_exception)
    """
    try:
        result = func(*args, **kwargs)
        return True, result
    except Exception as e:
        logger.error(f"Function {func.__name__} failed: {e}")
        return False, e
```

## `setup_example_logging(log_level='INFO', log_file=None)`

Setup logging for FIGAROH examples.

Parameters:

| Name        | Type            | Description                                 | Default  |
| ----------- | --------------- | ------------------------------------------- | -------- |
| `log_level` | `str`           | Logging level (DEBUG, INFO, WARNING, ERROR) | `'INFO'` |
| `log_file`  | `Optional[str]` | Optional log file path                      | `None`   |

Returns:

| Type     | Description                |
| -------- | -------------------------- |
| `Logger` | Configured logger instance |

Source code in `src/figaroh/utils/error_handling.py`

```
def setup_example_logging(
    log_level: str = "INFO", log_file: Optional[str] = None
) -> logging.Logger:
    """
    Setup logging for FIGAROH examples.

    Args:
        log_level: Logging level (DEBUG, INFO, WARNING, ERROR)
        log_file: Optional log file path

    Returns:
        Configured logger instance
    """
    logger = logging.getLogger("figaroh_examples")
    logger.setLevel(getattr(logging, log_level.upper()))

    # Remove existing handlers
    for handler in logger.handlers[:]:
        logger.removeHandler(handler)

    # Create formatter
    formatter = logging.Formatter(
        "%(asctime)s - %(name)s - %(levelname)s - %(message)s"
    )

    # Console handler
    console_handler = logging.StreamHandler()
    console_handler.setFormatter(formatter)
    logger.addHandler(console_handler)

    # File handler if specified
    if log_file:
        file_handler = logging.FileHandler(log_file)
        file_handler.setFormatter(formatter)
        logger.addHandler(file_handler)

    return logger
```

## Config Migration

Converts a legacy flat (`calibration:`/`identification:`) config to the unified `extends:`/`tasks:` format, including an optional round-trip self-check against a URDF. See "Migrating from legacy to unified format" in [Config Parameters](https://thanhndv212.github.io/figaroh-plus/concepts/config_parameters/index.md).

Legacy -> unified config migration.

Converts a legacy flat `calibration:`/`identification:` config into the unified `extends:`/`tasks:` format, using the exact field mapping documented in `docs/source/concepts/config_parameters.md`. This is the reverse of :func:`figaroh.calibration.config.unified_to_legacy_config` and :func:`figaroh.identification.config.unified_to_legacy_identif_config`.

Fields with no unified-format consumer today (`is_static_regressor`, `is_inertia_regressor`, `embedded_forces`, `sync_joint_motion`, and the parsed-but-unused joint limit / `ratio_essential` fields — see config_parameters.md) are preserved under the `custom:` section rather than silently dropped.

CLI usage::

```
python -m figaroh.utils.config_migration \
    --input legacy_config.yaml --output unified_config.yaml \
    [--task auto|calibration|identification] \
    [--template base|manipulator|humanoid] \
    [--urdf robot.urdf]   # enables the round-trip self-check
```

## `legacy_calibration_to_unified(calib_data)`

Convert a legacy `calibration:` section to unified `tasks.calibration`.

Parameters:

| Name         | Type             | Description                                                                                          | Default    |
| ------------ | ---------------- | ---------------------------------------------------------------------------------------------------- | ---------- |
| `calib_data` | `Dict[str, Any]` | The dict under the calibration: key of a legacy config file (e.g. yaml.safe_load(f)["calibration"]). | *required* |

Returns:

| Type             | Description                                                  |
| ---------------- | ------------------------------------------------------------ |
| `Dict[str, Any]` | A {"tasks": {"calibration": {...}}} fragment, ready to merge |
| `Dict[str, Any]` | with the identification fragment and an extends: root.       |

Source code in `src/figaroh/utils/config_migration.py`

```
def legacy_calibration_to_unified(calib_data: Dict[str, Any]) -> Dict[str, Any]:
    """Convert a legacy ``calibration:`` section to unified ``tasks.calibration``.

    Args:
        calib_data: The dict under the ``calibration:`` key of a legacy
            config file (e.g. ``yaml.safe_load(f)["calibration"]``).

    Returns:
        A ``{"tasks": {"calibration": {...}}}`` fragment, ready to merge
        with the identification fragment and an ``extends:`` root.
    """
    markers = calib_data.get("markers") or []
    unified_markers = [
        {
            "reference_joint": m.get("ref_joint"),
            "measurable_dof": m.get("measure", [True, True, True, False, False, False]),
        }
        for m in markers
    ]

    coeff = calib_data.get("coeff_regularize", 0.0)
    if coeff:
        logger.warning(
            "coeff_regularize (%s) was removed in 1.0: the migrated config "
            "sets regularization_coefficient to 0. Use estimation.method: map "
            "(docs/source/concepts/calibration_estimation.md).",
            coeff,
        )
        coeff = 0.0

    calibration_task: Dict[str, Any] = {
        "enabled": True,
        "type": "kinematic_calibration",
        "parameters": {
            "calibration_level": calib_data.get("calib_level", "full_params"),
            "include_non_geometric": bool(calib_data.get("non_geom", False)),
            "regularization_coefficient": coeff,
            "outlier_threshold": calib_data.get("outlier_eps", 0.05),
        },
        "kinematics": {
            "base_frame": calib_data.get("base_frame", "universe"),
            "tool_frame": calib_data.get("tool_frame"),
            "free_flying_base": bool(calib_data.get("free_flyer", False)),
        },
        "measurements": {
            "markers": unified_markers,
            "poses": {
                "base_pose": calib_data.get("base_pose"),
                "tool_pose": calib_data.get("tip_pose"),
            },
        },
        "data": {
            "source_file": calib_data.get("data_file"),
            "sample_configurations_file": calib_data.get("sample_configs_file"),
            "number_of_samples": calib_data.get("nb_sample", 500),
        },
    }

    base_to_ref_frame = calib_data.get("base_to_ref_frame")
    ref_frame = calib_data.get("ref_frame")
    if base_to_ref_frame is not None or ref_frame is not None:
        calibration_task["eye_hand"] = {
            "enabled": True,
            "camera_frame": base_to_ref_frame,
            "reference_frame": ref_frame,
        }

    return {"tasks": {"calibration": calibration_task}}
```

## `legacy_identification_to_unified(identif_data)`

Convert a legacy `identification:` section to unified format.

Parameters:

| Name           | Type             | Description                                                     | Default    |
| -------------- | ---------------- | --------------------------------------------------------------- | ---------- |
| `identif_data` | `Dict[str, Any]` | The dict under the identification: key of a legacy config file. | *required* |

Returns:

| Type             | Description                                                        |
| ---------------- | ------------------------------------------------------------------ |
| `Dict[str, Any]` | A \`\`{"robot": {"properties": {...}}, "tasks": {"identification": |
| `Dict[str, Any]` | {...}}, "custom": {...}}fragment.robot.properties\`\` holds        |
| `Dict[str, Any]` | fields shared across tasks (mechanics, joints); custom holds       |
| `Dict[str, Any]` | fields with no current unified-format consumer.                    |

Source code in `src/figaroh/utils/config_migration.py`

```
def legacy_identification_to_unified(identif_data: Dict[str, Any]) -> Dict[str, Any]:
    """Convert a legacy ``identification:`` section to unified format.

    Args:
        identif_data: The dict under the ``identification:`` key of a
            legacy config file.

    Returns:
        A ``{"robot": {"properties": {...}}, "tasks": {"identification":
        {...}}, "custom": {...}}`` fragment. ``robot.properties`` holds
        fields shared across tasks (mechanics, joints); ``custom`` holds
        fields with no current unified-format consumer.
    """
    robot_params = (identif_data.get("robot_params") or [{}])[0]
    problem_params = (identif_data.get("problem_params") or [{}])[0]
    process_params = (identif_data.get("processing_params") or [{}])[0]
    tls_params = (identif_data.get("tls_params") or [{}])[0]

    robot_properties: Dict[str, Any] = {
        "joints": {
            "joint_limits": {
                "position": robot_params.get("q_lim_def"),
                "velocity": robot_params.get("dq_lim_def"),
                "acceleration": robot_params.get("ddq_lim_def"),
                "torque": robot_params.get("tau_lim_def"),
            }
        },
        "mechanics": {
            "friction_coefficients": {
                "viscous": robot_params.get("fv"),
                "static": robot_params.get("fs"),
            },
            "actuator_inertias": robot_params.get("Ia"),
            "joint_offsets": robot_params.get("offset"),
            "reduction_ratios": robot_params.get("reduction_ratio"),
            "ratio_essential": robot_params.get("ratio_essential"),
        },
        "coupling": {
            "has_coupled_wrist": bool(problem_params.get("has_coupled_wrist", False)),
            "Iam6": robot_params.get("Iam6", 0),
            "fvm6": robot_params.get("fvm6", 0),
            "fsm6": robot_params.get("fsm6", 0),
        },
    }
    active_joints = problem_params.get("active_joints")
    if active_joints:
        robot_properties["joints"]["active_joints"] = active_joints

    ts = process_params.get("ts")
    sampling_frequency = (1.0 / ts) if ts else 5000.0

    force_torque = problem_params.get("force_torque")
    force_torque_sensors: List[Any] = (
        list(force_torque)
        if isinstance(force_torque, list)
        else ([force_torque] if force_torque else [])
    )

    identification_task: Dict[str, Any] = {
        "enabled": True,
        "type": "dynamic_identification",
        "problem": {
            "include_external_forces": bool(
                problem_params.get("is_external_wrench", False)
            ),
            "use_joint_torques": bool(problem_params.get("is_joint_torques", True)),
            "force_torque_sensors": force_torque_sensors,
            "external_wrench_offsets": bool(
                problem_params.get("external_wrench_offsets", False)
            ),
            "model_components": {
                "friction": bool(problem_params.get("has_friction", True)),
                "actuator_inertia": bool(
                    problem_params.get("has_actuator_inertia", True)
                ),
                "joint_offset": bool(problem_params.get("has_joint_offset", True)),
            },
        },
        "signal_processing": {
            "sampling_frequency": sampling_frequency,
            "cutoff_frequency": process_params.get(
                "cut_off_frequency_butterworth", 100.0
            ),
        },
        "load_configuration": {
            "additional_mass": tls_params.get("mass_load", 0.0),
            "loaded_body_index": tls_params.get("which_body_loaded", 0),
        },
    }

    custom: Dict[str, Any] = {}
    unconsumed_problem = {
        k: problem_params[k] for k in _UNCONSUMED_PROBLEM_PARAMS if k in problem_params
    }
    unconsumed_tls = {
        k: tls_params[k] for k in _UNCONSUMED_TLS_PARAMS if k in tls_params
    }
    if unconsumed_problem:
        custom["legacy_problem_params"] = unconsumed_problem
    if unconsumed_tls:
        custom["legacy_tls_params"] = unconsumed_tls

    result: Dict[str, Any] = {
        "robot": {"properties": robot_properties},
        "tasks": {"identification": identification_task},
    }
    if custom:
        result["custom"] = custom
    return result
```

## `merge_unified_sections(*docs)`

Merge fragments produced by `legacy_*_to_unified` into one document.

A legacy file commonly has both `calibration:` and `identification:` sections; each converts independently and the results are merged here.

Source code in `src/figaroh/utils/config_migration.py`

```
def merge_unified_sections(*docs: Dict[str, Any]) -> Dict[str, Any]:
    """Merge fragments produced by ``legacy_*_to_unified`` into one document.

    A legacy file commonly has both ``calibration:`` and ``identification:``
    sections; each converts independently and the results are merged here.
    """
    merged: Dict[str, Any] = {"tasks": {}}
    for doc in docs:
        for key, value in doc.items():
            if key == "tasks":
                merged["tasks"].update(value)
            elif key == "robot":
                merged.setdefault("robot", {"properties": {}})
                _deep_merge(
                    merged["robot"].setdefault("properties", {}),
                    value.get("properties", {}),
                )
            elif key == "custom":
                merged.setdefault("custom", {}).update(value)
            else:
                merged[key] = value
    return merged
```

## `convert_legacy_to_unified(legacy_config, template='base')`

Convert a full legacy config dict (both sections, if present).

Parameters:

| Name            | Type             | Description                                                                                                                  | Default    |
| --------------- | ---------------- | ---------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `legacy_config` | `Dict[str, Any]` | The parsed legacy YAML (top-level dict with calibration/identification keys).                                                | *required* |
| `template`      | `str`            | Which template to extends: — "base" (default, safest — no inherited defaults to reason about), "manipulator", or "humanoid". | `'base'`   |

Returns:

| Type             | Description                                                |
| ---------------- | ---------------------------------------------------------- |
| `Dict[str, Any]` | A complete unified-format config dict, ready to yaml.dump. |

Raises:

| Type         | Description                                                                           |
| ------------ | ------------------------------------------------------------------------------------- |
| `ValueError` | If neither calibration nor identification is present, or template is not a known key. |

Source code in `src/figaroh/utils/config_migration.py`

```
def convert_legacy_to_unified(
    legacy_config: Dict[str, Any], template: str = "base"
) -> Dict[str, Any]:
    """Convert a full legacy config dict (both sections, if present).

    Args:
        legacy_config: The parsed legacy YAML (top-level dict with
            ``calibration``/``identification`` keys).
        template: Which template to ``extends:`` — ``"base"`` (default,
            safest — no inherited defaults to reason about),
            ``"manipulator"``, or ``"humanoid"``.

    Returns:
        A complete unified-format config dict, ready to ``yaml.dump``.

    Raises:
        ValueError: If neither ``calibration`` nor ``identification`` is
            present, or ``template`` is not a known key.
    """
    if template not in TEMPLATE_PATHS:
        raise ValueError(
            f"Unknown template {template!r}; choose from {sorted(TEMPLATE_PATHS)}"
        )

    docs = []
    if "calibration" in legacy_config:
        docs.append(legacy_calibration_to_unified(legacy_config["calibration"]))
    if "identification" in legacy_config:
        docs.append(legacy_identification_to_unified(legacy_config["identification"]))
    if not docs:
        raise ValueError(
            "No 'calibration' or 'identification' section found in legacy config"
        )

    merged = merge_unified_sections(*docs)
    for task_name in ("calibration", "identification"):
        merged["tasks"].setdefault(task_name, {"enabled": False})

    result = {"extends": TEMPLATE_PATHS[template]}
    result.update(merged)
    return result
```

## `self_check(robot, legacy_config, unified_doc)`

Round-trip `unified_doc` back through the real unified parser and diff the recovered `calib_config`/`identif_config` against the original legacy sections.

This reuses the exact same code path production uses (`create_task_config` + `unified_to_legacy_*config`) rather than reimplementing the flattening logic, so it can't silently diverge from what the pipeline actually does.

Parameters:

| Name            | Type             | Description                                                                                                                                                                                                                                                                        | Default    |
| --------------- | ---------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- |
| `robot`         |                  | A robot instance with .model/.data (needed for frame validation inside unified_to_legacy_config).                                                                                                                                                                                  | *required* |
| `legacy_config` | `Dict[str, Any]` | The original parsed legacy YAML.                                                                                                                                                                                                                                                   | *required* |
| `unified_doc`   | `Dict[str, Any]` | The output of :func:convert_legacy_to_unified (with extends: already resolved away, i.e. pass the merged dict as if template inheritance had already happened — for a from-scratch check, resolve inheritance first via UnifiedConfigParser if the template adds required fields). | *required* |

Returns:

| Type             | Description                                       |
| ---------------- | ------------------------------------------------- |
| `Dict[str, Any]` | \`\`{"ok": bool, "mismatches": {".": {"expected": |
| `Dict[str, Any]` | ..., "got": ...}}}\`\`                            |

Source code in `src/figaroh/utils/config_migration.py`

```
def self_check(
    robot, legacy_config: Dict[str, Any], unified_doc: Dict[str, Any]
) -> Dict[str, Any]:
    """Round-trip ``unified_doc`` back through the real unified parser and
    diff the recovered ``calib_config``/``identif_config`` against the
    original legacy sections.

    This reuses the exact same code path production uses
    (``create_task_config`` + ``unified_to_legacy_*config``) rather than
    reimplementing the flattening logic, so it can't silently diverge from
    what the pipeline actually does.

    Args:
        robot: A robot instance with ``.model``/``.data`` (needed for frame
            validation inside ``unified_to_legacy_config``).
        legacy_config: The original parsed legacy YAML.
        unified_doc: The output of :func:`convert_legacy_to_unified`
            (with ``extends:`` already resolved away, i.e. pass the merged
            dict as if template inheritance had already happened — for a
            from-scratch check, resolve inheritance first via
            ``UnifiedConfigParser`` if the template adds required fields).

    Returns:
        ``{"ok": bool, "mismatches": {"<section>.<field>": {"expected":
        ..., "got": ...}}}``
    """
    from figaroh.utils.config_parser import create_task_config
    from figaroh.calibration.config import unified_to_legacy_config
    from figaroh.identification.config import unified_to_legacy_identif_config

    mismatches: Dict[str, Dict[str, Any]] = {}

    if "calibration" in legacy_config:
        original = legacy_config["calibration"]
        task_config = create_task_config(robot, unified_doc, "calibration")
        recovered = unified_to_legacy_config(robot, task_config)
        checks = {
            "calib_level": ("calib_model", "full_params"),
            "non_geom": ("non_geom", False),
            "base_frame": ("start_frame", "universe"),
            "tool_frame": ("end_frame", None),
            "free_flyer": ("free_flyer", False),
            "base_to_ref_frame": ("base_to_ref_frame", None),
            "ref_frame": ("ref_frame", None),
            "coeff_regularize": ("coeff_regularize", 0.0),
            "outlier_eps": ("outlier_eps", 0.05),
            "data_file": ("data_file", None),
            "nb_sample": ("NbSample", 500),
        }
        for legacy_key, (recovered_key, default) in checks.items():
            expected = original.get(legacy_key, default)
            if legacy_key == "coeff_regularize":
                expected = 0.0  # removed in 1.0: migrated as 0
            got = recovered.get(recovered_key, default)
            if expected != got:
                mismatches[f"calibration.{legacy_key}"] = {
                    "expected": expected,
                    "got": got,
                }

    if "identification" in legacy_config:
        original = legacy_config["identification"]
        task_config = create_task_config(robot, unified_doc, "identification")
        recovered = unified_to_legacy_identif_config(robot, task_config)
        robot_params = (original.get("robot_params") or [{}])[0]
        problem_params = (original.get("problem_params") or [{}])[0]
        checks = {
            "fv": (robot_params.get("fv"), recovered.get("fv")),
            "fs": (robot_params.get("fs"), recovered.get("fs")),
            "Ia": (robot_params.get("Ia"), recovered.get("Ia")),
            "has_friction": (
                problem_params.get("has_friction", True),
                recovered.get("has_friction"),
            ),
            "has_coupled_wrist": (
                problem_params.get("has_coupled_wrist", True),
                recovered.get("has_coupled_wrist"),
            ),
        }
        for field, (expected, got) in checks.items():
            if expected != got:
                mismatches[f"identification.{field}"] = {
                    "expected": expected,
                    "got": got,
                }

    return {"ok": not mismatches, "mismatches": mismatches}
```

# Visualisation

## `MeshcatVisualizer(robot=None, model=None, collision_model=None, visual_model=None, url=None)`

Bases: `MeshcatVisualizer`

Extension of Pinocchio MeshcatVisualizer with additional features.

Initialize visualizer.

Parameters:

| Name              | Type | Description                  | Default |
| ----------------- | ---- | ---------------------------- | ------- |
| `robot`           |      | Robot object                 | `None`  |
| `model`           |      | Pinocchio model              | `None`  |
| `collision_model` |      | Collision model              | `None`  |
| `visual_model`    |      | Visual model                 | `None`  |
| `url`             |      | Server URL for visualization | `None`  |

Source code in `src/figaroh/visualisation/visualizer.py`

```
def __init__(
    self, robot=None, model=None, collision_model=None, visual_model=None, url=None
):
    """Initialize visualizer.

    Args:
        robot: Robot object
        model: Pinocchio model
        collision_model: Collision model
        visual_model: Visual model
        url: Server URL for visualization
    """
    if robot is not None:
        PMV.__init__(self, robot.model, robot.collision_model, robot.visual_model)
    elif model is not None:
        PMV.__init__(self, model, collision_model, visual_model)

    if url is not None:
        if url == "classical":
            url = "tcp://127.0.0.1:6000"
        logger.info("Wrapper tries to connect to server <%s>", url)
        server = meshcat.Visualizer(zmq_url=url)
    else:
        server = None

    if robot is not None or model is not None:
        self.initViewer(loadModel=True, viewer=server)
    else:
        self.viewer = server if server is not None else meshcat.Visualizer()
```

## `materialFromColor(color)`

Convert color to Material.

Parameters:

| Name    | Type | Description                                           | Default    |
| ------- | ---- | ----------------------------------------------------- | ---------- |
| `color` |      | Color specification (Material, string, list, or None) | *required* |

Returns:

| Type | Description                                |
| ---- | ------------------------------------------ |
|      | meshcat.geometry.Material: Material object |

Source code in `src/figaroh/visualisation/visualizer.py`

```
def materialFromColor(color):
    """Convert color to Material.

    Args:
        color: Color specification (Material, string, list, or None)

    Returns:
        meshcat.geometry.Material: Material object
    """
    if isinstance(color, meshcat.geometry.Material):
        return color
    elif isinstance(color, str):
        material = colors.colormap[color]
    elif isinstance(color, list):
        from .colors import rgb2int

        material = meshcat.geometry.Material()
        material.color = rgb2int(*[int(c * 255) for c in color[:3]])
        if len(color) == 3:
            material.transparent = False
        else:
            material.transparent = color[3] < 1
            material.opacity = float(color[3])
    elif color is None:
        import random

        material = random.sample(list(colors.colormap), 1)[0]
    else:
        material = colors.black
    return material
```
# Project

# Changelog

The canonical changelog lives at the repository root ([`CHANGELOG.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/CHANGELOG.md)) so it stays next to the code it describes. This page embeds it directly so the same content is browsable as part of the docs site, without a second copy to keep in sync.

## Changelog

All notable changes to this project will be documented in this file.

The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).

### [Unreleased]

### [1.0.2] - 2026-10-13

Documentation release. No library code changed; the figaroh-examples `v1.0.0` tag remains the validated pair.

#### Added

- Documentation site redesigned around user journeys (#201): a landing page with a task picker, Start / Workflows / AI agents / Concepts / Reference / Project navigation, five-step calibration and identification guides (Plan, Design, Configure, Fit, Verify) with an example/own-robot toggle and choice cards, Quickstart and "Add your robot" pages.
- AI agents section: agent setup, environment doctor, prompt library and skills reference; the site publishes `llms.txt` and `llms-full.txt` (`mkdocs-llmstxt`, added to the `docs` extra) and every page has a Copy as Markdown button.
- README screenshot of the documentation landing page.

#### Changed

- pre-commit `check-yaml` runs with `--unsafe` so `mkdocs.yml` can use mkdocs-material `!!python/name` tags.

### [1.0.1] - 2026-10-12

Documentation and housekeeping release. No library code changed; the figaroh-examples `v1.0.0` tag remains the validated pair.

#### Added

- FIGAROH logo: the mark and a README banner in `docs/source/assets/`, used by the README, the docs home page and the docs site header and favicon. `docs/development/make_logo.py` regenerates them (#198).

#### Changed

- Copyright notices updated to 2026 for the maintainer and corrected to 2022–2024 for CNRS/Toward SAS; `CONTRIBUTING.md` release step 2 now checks that the notices include the release year (#197).
- `ROADMAP.md` records the 1.0.0 release (#196).

### [1.0.0] - 2026-10-12

First stable release. Roadmap milestone M1 (trustworthy physical-model workflows) is accepted: both reference workflows, calibration and dynamic identification, are recorded on core `779e830` + examples `b167298` (#181). This release declares the public API and its stability policy, removes the features deprecated before 1.0, and makes calibration refuse to export a failed fit.

Scope notes for 1.0:

- The real-data references (TIAGo calibration and identification, UR10 calibration) are verified at **execution scope**: held-out error is reported, not gated. Gated prediction exists only on simulated or synthetic truth (UR10 dynamic truth, TIAGo calibration truth fixture).
- The TIAGo effort scale reads 13.5 % low, outside its ±10 % diagnostic tolerance; the joint torque constants are tracked in figaroh-examples#126.
- The TIAGo URDF export is lumped; the PAL calibration export was not run on hardware; TALOS contact calibration is a regression consumer only.

Upgrading from 0.7: convert any legacy flat config once with `python -m figaroh.utils.config_migration`, drop a non-zero `regularization_coefficient`, and use Python 3.10 or newer (see Removed and Changed below).

#### Added

- Public API and stability policy for 1.0 (#184): a new "Public API and stability" docs page (`docs/source/api/stability.md`) lists the supported entry points per subpackage, the experimental areas (MuJoCo backend and `from_mjcf`, `integration`, `visualisation`, `measurements`, log-Cholesky LC1), the internal modules, the deprecated features and the deprecation policy: a public name warns with `DeprecationWarning` for at least one minor release, is removed only in a major release after 1.0, and has a migration note here.
- The `__all__` lists now match that page, and a unit test keeps them in sync. `import figaroh` also imports `data`, `backends` and `integration` (about 0.1 s more, mostly the optional `mujoco` import when it is installed). `calibration.__all__` adds `calibration_tools`, `data_loader` and `parameter`; `identification.__all__` adds `identification_tools`, `parameter` and `physical_fit`; `tools.__all__` adds `run_archive`, `stages` and `solver`; `utils.__all__` adds `config_parser` and `error_handling`; `optimal.__all__` adds `BaseTrajectoryIPOPTProblem`; `measurements.__all__` is `["Measurement"]`. The public tools, config and error-handling modules gained their own `__all__`. Nothing was renamed or removed; names outside an `__all__` stay importable but are not covered.

#### Deprecated

Each warns with `DeprecationWarning` from 1.0 and will be removed in 2.0 (#185):

- `figaroh.tools.qrdecomposition.QR_pivoting` and `double_QR`: use `QRDecomposer.decompose_with_pivoting` / `double_decomposition`.
- `figaroh.tools.robotcollisions.CollisionWrapper`: use `CollisionManager`.
- The module-level wrappers in `figaroh.tools.robotvisualization` (`place`, `display_COM`, `display_axes`, `rotation_matrix_from_vectors`, `display_force`, `display_bounding_boxes`, `display_joints`): use the `RobotVisualizer` methods.
- `plot_calibration_results`, `plot_identification_results` and `save_results` in `figaroh.utils.results_manager`: use `ResultsManager`.
- The flat `physical_consistency_enabled` identification config key: use `physical_consistency.enabled`.

#### Changed

- `requires-python` is now `>=3.10`, following Pinocchio (conda-forge builds 3.8 and 4.1 for Python 3.10-3.14; `pin` 4.1 ships wheels for cp310-cp314). The Python 3.8 and 3.9 classifiers are dropped and 3.13 and 3.14 are added. CI adds Python 3.10 (lowest) and 3.14 (highest) test jobs next to the Python 3.12 jobs. Migration: Python 3.8 and 3.9 users stay on 0.7.x.
- The package classifier is `Development Status :: 5 - Production/Stable`.
- `RobotIdentificationSystem.identify_parameters` also reads a `data_dir` stored under `tasks.identification` of a unified config, since the flat `identification:` section it was read from is gone.

#### Removed

Features deprecated before 1.0 are removed (#185, paired examples migration figaroh-examples#125). Migration notes:

- The legacy flat config format (deprecated since 0.4.6). Loading one through `load_param`, `figaroh.utils.config_parser.get_param_from_yaml` or the optimal-trajectory loader now raises a `ConfigurationError` that names `python -m figaroh.utils.config_migration --input <legacy.yaml> --output <unified.yaml>`; run that once per file. The migration tool still reads legacy files. Also removed with the format: `figaroh.calibration.config.get_param_from_yaml`, `figaroh.identification.config.get_param_from_yaml`, both `get_param_from_yaml_legacy`, and the `get_param_from_yaml` re-exports of `calibration_tools` and `identification_tools`. Use `figaroh.utils.config_parser.get_param_from_yaml` with a unified config.
- `get_param_from_yaml_with_warning` in `figaroh.calibration.config` and `figaroh.identification.config` (since 0.2.0): call `figaroh.utils.config_parser.get_param_from_yaml`.
- `figaroh.utils.config_parser.get_calibration_param_from_yaml` and `get_identification_param_from_yaml` (since 0.2.0): call `get_param_from_yaml(robot, data, "calibration")` or `get_param_from_yaml(robot, data, "identification")`.
- A non-zero `regularization_coefficient` / `coeff_regularize` (since 0.6.0) is a `ConfigurationError`. Remove the key (zero or absent is still accepted) and use `estimation.method: map`, which sets priors of physical size per parameter group. `config_migration` writes 0 and warns when it meets a non-zero legacy `coeff_regularize`.

#### Fixed

- A calibration whose solver failed can no longer be exported silently (#182, M1 G-1). `BaseCalibration.STATUS` is `"FAILED"` instead of `"CALIBRATED"` when the solver did not converge, and `joint_corrections()` (the URDF export gateway), `build_geometric_calibration` and `export_geometric_calibration_yaml` raise `CalibrationError` when the `fit` stage is `failed`, recording stage `export` as failed, as `BaseIdentification.export_urdf` already does. `allow_failed_fit=True` exports for diagnosis: it warns, records `export` as `fallback` and notes the override in the run provenance. A solver failure on the first fit now returns a failed result instead of raising an `UnboundLocalError`. Fits that converged export exactly as before.

### [0.7.0] - 2026-10-11

This release ships the **dynamic identification reference workflow** (roadmap M1.2–M1.4, dynamic side; delivery packages D4 and D7): select an identification stage, check its physical and solver verdict, verify it on held-out data with per-joint units, export a URDF that reloads with RNEA parity, and archive the run. The D4 comparison selected the direct LMI effort fit (`physical_fit`) as the supported physical baseline; the nonlinear log-Cholesky fit is not included (D5 no-go, LC1 #155). Release tracker: #177.

Behaviour changes when upgrading from 0.6.0, detailed in the entries below:

- Base parameters are selected deterministically (#116). `phi_base` is reproducible across platforms and Pinocchio versions and differs from 0.6.0 wherever the old pivoting was a tie; `QRDecomposer(deterministic=False)` restores LAPACK pivoting.
- The physical-consistency projection projects the reconstruction, not the nominal model (#163). Without `reconstruction` enabled it is `skipped` and the `physical` stage is `not_run`; it was recorded `ok`.
- `identification.select_stage` and `problem.torque_fit_joints` are new and opt-in; unset, results are as before. A rejected requested stage is a hard reject (#61).
- Package metadata declares the license as Apache-2.0; it declared BSD.

Known limitations: `projected` is not a selectable stage; `merged_bodies="subtract_fixed"` refuses on real fits, so the TIAGo reference exports a lumped URDF; TIAGo held-out prediction is reported, not gated (no ground truth, a temporal held-out block of one recording), and arm_1's torque constant is not identifiable from the 2021-07 recordings; export parity shows file consistency, not hardware accuracy.

#### Added

- `identification.select_stage` (`fit`, `reconstruction`, `physical_fit`) chooses the reported, validated, archived and exported estimate. A rejected requested stage is a hard reject (`selected_stage` `none`, verify fails, export refuses). The default is unchanged. New `BaseIdentification.export_urdf`, and `merged_bodies="subtract_fixed"` in the URDF exporter. The direct LMI effort fit is public as `figaroh.identification.physical_fit` (`_physical_comparator` stays as an alias); `ReconstructionResult.effective_method` is new. (#61)
- `problem.torque_fit_joints` names the active joints whose measured effort is fitted and scored. The other active joints keep their kinematics in the regressor; their effort is left out of the fit, the WLS weights, the per-joint statistics and the validation metrics, which list them under `not_fitted_joints`. Unset, every active joint is fitted as before. (figaroh-examples#68)
- Record the log-Cholesky feasibility spike with frozen synthetic trajectories, independent physical/Jacobian checks and machine-readable Pinocchio 3.7/4.1 results. The decision is revise: convergence/scaling must improve before a production solver is approved. (#22)
- Diagnose why the log-Cholesky spike's fits stop at their evaluation budget and freeze a revised protocol (budget 2000, convergence by termination status, three fresh seed sets) before confirmation. The fresh-seed confirmation is no-go: 8 of 24 gated fits still hit the budget. (#30)

#### Changed

- Rewrite `README.md` for 0.7: a quickstart that runs (the UR10 identification reference in figaroh-examples), the identification stages, tested platforms, and links to the documentation site in place of the module tables and an invalid YAML example. The release sequence in `CONTRIBUTING.md` now includes revising both READMEs.

#### Fixed

- Package metadata declares the license as `Apache-2.0` (an SPDX `License-Expression`, with `LICENSE` as the license file), matching `LICENSE`. It declared a BSD classifier and copied the whole license text into the `License` field, so PyPI listed the wrong license.
- The physical-consistency projection projects the identified fit, not the nominal model. It now reads the full-parameter reconstruction, which runs first, and projects only the joints the reconstruction covers. Without `reconstruction` enabled the block is `skipped` and the `physical` stage is `not_run`; it was previously recorded `ok` for a projection of the CAD prior. (#163)
- A configured `max_seconds` in the SDP reconstruction and the LMI projection is passed to picos as `timelimit`; picos rejected the old option name and the solve was reported as failed. (#164)
- Base-parameter selection no longer depends on BLAS rounding or on the order of regressor columns. `QRDecomposer` picks base columns with the new `deterministic_column_pivots`: largest residual first, columns within a relative `1e-6` of the largest are tied and the smallest parameter name wins. `phi_base` is now reproducible across macOS/Linux and Pinocchio versions (the base parameters it reports differ from before wherever the old selection was a tie). `QRDecomposer(deterministic=False)` restores LAPACK pivoting; geometric calibration keeps it until #113. (#116)
- Optimal-trajectory plots work for robots whose position and velocity dimensions differ (`nq != nv`, e.g. TIAGo) (#149). Positions were indexed with velocity columns, so the plot failed (`index 24 is out of bounds`) and was skipped with a logged error. Both plot paths now draw only the active joints, positions through each joint's `idx_q` and velocities and accelerations through its `idx_v`, labelled with the joint names. `ResultsManager.plot_optimal_trajectory_results` gains `q_indices` and `v_indices`; without them it plots every column and refuses (logged error) when `nq != nv`. The bottom row no longer gets a second, empty set of axes stacked on it for the time labels.

### [0.6.0] - 2026-10-07

This release ships the **calibration reference workflow** (roadmap M1.1–M1.4, calibration side; delivery packages D2, C1–C4, W3 and S1) and the inertial URDF export (D6), with the core fixes found by the UR10 truth fixture (D3:

## 142, #143). The dynamic identification reference (D4, D7) is not in this

release; it is planned for 0.7.0. Release tracker: #146.

Behaviour changes when upgrading from 0.5.0, detailed in the entries below:

- `solve(outlier_threshold=...)` is a position error in metres (it was a number of standard deviations) and outliers are now excluded (#98).
- `regularization_coefficient` is deprecated and defaults to 0; use `estimation.method: map` (#120). It is planned for removal in 1.0.
- Exciting-trajectory optimisation adds collision constraints for robots without an SRDF and checks clearance along the whole spline (#143); such robots may need a collision-free joint range (UR10 example: `joint_box`).
- Results change on rerun: base parameters at full precision (#142), a seeded identifiable-parameter selection (#99), and corrected `full_params` and `joint_offset` semantics (#101, #110).
- `export_urdf` refuses what it cannot write instead of skipping it (#62) and writes complete inertial sets (#60).

Known limitations: calibration accuracy is demonstrated on TIAGo mocap only (held-out marker RMSE 3.8–4.2 mm); nothing is applied to a running robot, and export parity shows file consistency, not hardware accuracy; inertial export is verified with known parameters, not yet with identified ones.

#### Added

- Calibration with several points of one rigid body per sample (#119). Each `measurements.markers` entry is a point on the same tool frame with its own offset (`pEEx_k`, ...); rows are marker-major, as the CSV loader already ordered them. `calc_updated_fkm` no longer raises for `NbMarkers > 1`; the closed-form frame guess fits the base frame and all point offsets; the structural selection uses all six pose components when there are several points; metrics, validation, outlier distances and plots count every point of every sample; the terminal and HTML validation tables list the position RMSE of each point. The loader now refuses non-finite marker measurements (e.g. an occluded point), naming the CSV rows. Several points identify the same joint parameters as one and roughly halve standard errors; they did not improve held-out prediction on the TIAGo mocap reference, so examples adopt them case by case (`docs/source/concepts/config_parameters.md`, "Several points on the tool").
- Inertial URDF export (#60): `export_urdf` writes identified standard inertial parameters (`m_`, `mx_`…`mz_`, `Ixx_`…`Izz_`, Pinocchio `toDynamicParameters()` convention) as URDF mass, centre of mass and centre-of-mass tensor (parallel-axis theorem), keeping an existing inertial `rpy`. Targets may be links or moving joints (as identification names them). Partial sets, joints whose Pinocchio body merges fixed-attached massive links, `m <= 0` and, unless `allow_infeasible=True`, physically inconsistent sets are refused; each link's physical verdict is logged. Previously these names raised (#62).
- Identification validation per joint (#103): `validation_metrics` gains `per_joint` (RMSE identified/nominal in the joint's unit, N for prismatic, N·m for revolute; measured std; normalised RMSE; R²; `predictive`), `unpredictable_joints` (held-out RMSE not below the measured std: no better than a constant) and `correlation_normalised` (correlation of per-joint standardised signals). The verdict's `validation_correlation` now uses the normalised value, and `validation_unpredictable_joints` counts flagged joints; the pooled `correlation` is kept. Terminal and HTML reports show the per-joint table and flag unpredictable joints.
- Stage-aware verdicts and reports (#63).
- `verify()` separates per-stage verdicts: `verdict.stages` gains `data`, `fit`, `validation` (`fallback` on training data, never held-out evidence), `physical` and `export` beside the existing scoped entries.
- New verdict fields: `stage_records`, `selected_stage`, `splits` (the files and sessions that trained and validated the run).
- Provenance records the figaroh checkout's own commit (`software.figaroh_revision`) beside the working directory's (`git_commit`), so a run names its core/examples pair.
- The terminal and HTML reports show the stages and data; the run archive index carries the stage verdicts.
- Fixed: the exported verification JSON's `stages` (the scoped status map) was overwritten by the stage records; they are now under `stage_records`.
- `figaroh.data`: data contract types from the accepted decision record `docs/decisions/data-result-contract.md` (#55, part 1 of 2; additive).
- `TrajectoryData` (identification): time with a `recorded`/`assumed` clock, joint names, per-signal origin, effort in its recorded units with a per-joint kind and unit (`joint_torque` N·m, `joint_force` N, `motor_current`, ...), the conversion applied, the recorded signal, and a valid-sample mask. `check_effort(model, drive_gain_joints)` refuses effort that is not joint effort unless a drive gain is identified.
- `PoseObservations` (calibration): postures with named points or poses, their frame, measurability, mask and sessions; constraint-only datasets (contact) declare a constraint instead of zero "measurements". `from_csv`/`to_legacy` reproduce `load_data` exactly, with deleted rows masked instead, and leave `calib_config` unchanged.
- `DataSource` and `Session` (files with sha256, adapter, recording identity) and `Protocol` (versioned roles per session, checked against the files' sha256).
- Data contract: `TrajectoryData.sample_index` / `PoseObservations.sample_index` record each sample's source row (default `0..n-1`; `from_csv` sets CSV rows, so masked postures are named by row), and `PoseObservations.select_points(names)` restricts observations to some tracked points, e.g. one of TIAGo's four mocap points (#131).
- The base classes use the data contract (#55, part 2).
- `BaseIdentification`: `load_trajectory_data()` may return a `TrajectoryData`. Its effort is checked against the model (drive-side kinds are refused: no drive-gain parameter exists yet), and its mask leaves samples out of the regressor rows after filtering and differentiation (with decimation, decimated sample `k` is sample `k * factor`). A subclass returning one must convert the effort with `TrajectoryData.converted()`, not override `process_torque_data()`. Legacy dicts work as before.
- `BaseCalibration.load_data_set()` reads the CSV through `PoseObservations` (same arrays as `load_data`); `del_list` rows are masked, and `calibrator.observations` keeps every posture.
- `figaroh.tools.stages`: each run records `data`, `fit`, `validation` (`fallback` when evaluated on training data), `physical` and `export` (`failed` when `joint_corrections()` refuses parameters) stages on `obj.stages`, in the results dict under `"stages"`, in the run archive's `stages.json`, and in the verification JSON, which also gains `schema_version`.
- Identification results gain `"effort rmse"` (N·m or N); the mislabelled `"rmse norm (N/m)"` is kept for existing readers.
- `BaseCalibration.metrology_frames()`: the fitted base frame and tool point, which a URDF does not carry; applied outside an exported URDF they reproduce the calibrated forward kinematics. Tested on a known TIAGo correction fixture for both levels and three methods, on fitted and unused postures (#62).
- `BaseCalibration.joint_corrections(lift=True)`: the joint corrections to write into a URDF, the same values the PAL export uses (#111).
- Calibration estimation methods, chosen per robot and dataset with `parameters.estimation.method` (#113); the default `structural` is unchanged.
- `excitation`: select over every joint parameter on the measured postures and drop those whose predicted standard error exceeds `excitation_k` times their expected size.
- `map`: estimate every joint parameter with Gaussian priors of the expected sizes (`priors`); posterior standard deviations in `std_dev`.
- `map_cv` / `cv_subset`: prior scale or parameter-set size chosen by k-fold cross-validation over the training postures; no robot-specific sizes needed.
- Diagnostics in `calib_config["estimation_report"]`; guide in `docs/source/concepts/calibration_estimation.md`.

#### Changed

- `regularization_coefficient` (`calib_config["coeff_regularize"]`) is deprecated and defaults to 0 instead of 0.01 (#120). Robot cost functions appended `sqrt(c) * theta` rows for every joint parameter, so a metre and a radian were penalised alike. Core never applies the coefficient; a non-zero value now raises a `DeprecationWarning` pointing to `estimation.method: map`, which places priors of physical size on each parameter group. Design record: `docs/decisions/calibration-regularisation.md`. The figaroh-examples robots drop their regularisation rows in the paired PR.
- PAL `geometric_calibration` export (`build_geometric_calibration`, `export_geometric_calibration_yaml`) fixed and given `nominal_urdf` (#123).
- **Joint offsets exported:** `joint_offset`-level corrections (`offsetRZ_*` etc.) were dropped, so the TIAGo reference wrote an empty `geometric_calibration`.
- **Deltas against the URDF origin:** they were taken against Pinocchio's joint placement, which merges preceding fixed joints (wrong frame behind a rotated fixed joint, e.g. UR10 `shoulder_pan_joint`).
- **Exact rpy:** the rpy delta is solved so that the origin's rpy as written plus the delta is the corrected rotation, including at pitch +-pi/2 (TIAGo arm_4..arm_6). Pass `nominal_urdf`; without it the origin is recovered from the model. Where the joint axis is the axis roll and yaw share at pitch +-pi/2, no small rpy delta exists and the exact one trades roll against yaw (e.g. `droll` -pi/2, `dyaw` +pi/2): the same small rotation.
- **Behaviour change:** PAL files gain the joint-offset keys; values behind rotated fixed joints change.
- `export_urdf` refuses what it cannot write instead of skipping it (#62). A correction for a joint or link missing from the URDF, inertia tensors, first moments, legacy `off_*`, and a joint with both `offset*` and `d_*` now raise `ValueError`; no file is written. `joint_corrections()` raises `CalibrationError` for fitted parameters a URDF cannot carry (elastic, contact planes); pass `drop_unsupported=True` to export the kinematic corrections only. Values are written with 12 significant digits instead of 6.
- **Behaviour change:** calls that relied on the logged skip now fail. Exported URDF text changes in the trailing digits.
- `redistribute_parameters()` (and so the PAL `geometric_calibration` export and the HTML report) lifts `structural` base parameters by a weighted minimum-norm lift instead of the Moore-Penrose one (#111).
- **Why:** the old lift weighed metres and radians equally, ignored the rows the fit held at zero (base-frame and dropped rows), so it did not reproduce the fit, and the PAL export removed "the first six rows" by position instead.
- **Now:** expected sizes from `parameters.estimation.priors` weight the lift; base-frame and dropped rows are held at 0; Gauss-Newton steps make the lifted model reproduce the fit at the measured postures (skipped, first order only, when the calibrator has non-joint parameters such as contact planes). The result does not depend on which representative the QR chose. The PAL export no longer excludes the first joint's parameters.
- **Behaviour change:** PAL and report values for `structural` change.
- Calibration drops joint parameters that the estimated base/tip frames absorb, starts the frames at a closed-form estimate, and selects parameters deterministically (#102, #99).
- **Absorbed parameters dropped:** after the structural selection, `BaseCalibration.create_param_list` builds the full measurement Jacobian at the measured configurations (configured measurability, base and tip columns included) and drops every joint parameter that is a combination of the frames and the joint parameters before it (`eliminate_absorbed_parameters`, tolerance 1e-4 on unit-normalised columns). The names are kept in `calib_config["absorbed_param_name"]`. TIAGo mocap: `joint_offset` drops the torso and arm_1 (rank 14/14 instead of 14/16); `full_params` drops the tool-absorbed `d_pz_arm_7_joint` (with #110; before it, 4–9 parameters including an RPY near-singularity).
- **Closed-form start:** `solve_optimisation` starts unknown base/tip frames at `estimate_frames_closed_form` (Kabsch / chordal mean alternated with linear least squares) instead of zero.
- **Deterministic draw:** the structural random configurations come from a seeded NumPy generator (`calib_config["random_seed"]`, default 0) instead of Pinocchio's global RNG.
- **Behaviour change:** the calibrated parameter set is smaller when frames are estimated. Set `calib_config["eliminate_absorbed_parameters"] = False` for the previous set.
- `full_params` placement errors `d_{px,py,pz,phix,phiy,phiz}_<joint>` now act in the joint frame, `placement · SE3(exp3(d_phi), d_p)`, the convention of the kinematic regressor that selects the base parameters (#110).
- **Cause:** `update_joint_placement` added `d_p*` to the parent-frame translation and `d_phi*` to the placement's RPY angles. Wherever the nominal placement is rotated, the fitted model then had different dependencies from the ones selection assumed: FK derivatives differed from the regressor columns by O(1) on TIAGo arm_1–arm_7 and UR10 shoulder_pan, shoulder_lift, wrist_2 and wrist_3.
- **Effect:** TIAGo mocap `full_params` (figaroh-examples held-out protocol, macOS): 31 instead of 28 parameters, norm RMSE 1.64 / 3.07 / 2.83 / 2.44 mm (training / validation / two confirmation sets) instead of 1.75 / 3.68 / 3.35 / 2.89 mm.
- **Export:** `export_urdf` applies a joint's six `d_*` values together as `origin · SE3(exp3(d_phi), d_p)`; `build_geometric_calibration` converts them to PAL origin xyz/RPY deltas against the nominal placement.
- **Behaviour change:** `full_params` values from earlier versions use the old convention and are not comparable on rotated placements (UR10, Talos, TIAGo). `joint_offset` is unaffected.

#### Fixed

- The exciting-trajectory optimiser enforces collision clearance along the trajectory (#143). Its collision constraint was empty when the robot's geometry model had no collision pairs (no SRDF, as for the UR10): every two geometries on non-adjacent bodies are now paired (`srdf` removes pairs). Collision was checked at waypoints only, so splines crossed obstacles between them; initial guesses and every solved segment must now keep `collision_margin` along the whole spline (`collision_check_frequency`, default 200 Hz), and `collision_checks_per_interval` adds check points inside the constraint. `collision_screen` computes exact distances only for nearby pairs. Robots whose trajectories crossed obstacles now need a collision-free joint range (UR10 example: `joint_box`).
- Identified base parameters are returned at full precision (#142). `QRDecomposer` rounded `phi_b` to six decimals on every path, and the base values computed from priors to five; both weighted-least-squares refinements rounded to six. Small base parameters (UR10 wrist terms are ~1e-5) lost up to 5 %, and noise-free effort was fitted to ~5e-6 N·m instead of rounding error. Rounding remains only in displayed text.
- Calibration's identifiable parameter set no longer depends on global random state (#99). The structural selection draws its random configurations from a generator seeded by the new `parameters.random_seed` (default 0, legacy `random_seed`), and the seed is recorded in the run provenance. Repeated runs in one process give the same parameter set and result, whatever ran before. The seeded draw itself landed with #102.
- Calibration outlier removal now removes outliers (#98). The loop in `BaseCalibration.solve()` used to flag samples by a 3-sigma rule and refit the same full dataset, while the configured `parameters.outlier_threshold` was unused. Samples whose position error exceeds `outlier_threshold` (metres, `outlier_eps`; rotational components do not take part) are now excluded from the fit and the rest refitted, for up to `max_iterations` fits. `solve(outlier_threshold=...)` now overrides the config in metres (default `None`: the config; it was a number of standard deviations). RMSE, MAE, max error, per-DOF statistics and the residual degrees of freedom cover the kept samples; the evaluation adds `excluded_samples`, `excluded_sample_errors` and `outlier_threshold`, and the terminal and HTML reports list them. The `fit` stage metric `flagged_outliers` is now `excluded_outliers`.
- Calibration quality reports label residual rows from `measurability` (#100). A contact calibration measuring `[z, roll, pitch]` was printed as `X/Y/Z (mm)` with the two rotations folded into "Position RMSE". Rows are now named and scaled per measured component (mm for positions, deg for rotations), position/orientation aggregates cover only the measured components of each kind (NaN and "not measured" when none), and `verify()` picks `position_rmse_mm`/`orientation_rmse_deg` from the measured kinds rather than the component count. With no residual degrees of freedom, `calc_stddev()` reports the uncertainty as not estimable (NaN, a warning, `residual_dof` in the evaluation metrics) instead of dividing by zero. Reports no longer claim there is no validation data when a subclass evaluates its held-out set itself.
- `calc_updated_fkm` with an empty parameter list (nominal FK) raised `UnboundLocalError`; it now returns the nominal model's measured frame (#129).
- URDF export wrote corrections into `<transmission>` joints instead of the robot joint when a transmission came first in the file (#114).
- **Cause:** `_find_joint` searched every `<joint>` in the document; ros_control `<transmission>` blocks contain `<joint name=...>` too. It now searches only the `<robot>` element's direct children (same for links).
- **Effect:** figaroh-examples UR10 exports were identical to the nominal URDF (its transmissions precede the joints); TIAGo and Talos were not affected.
- Calibration parameter standard errors (`std_dev`, `std_pctg`, the covariance behind `redistribute_parameters`) were ~100× too small (#107).
- **Cause:** `calc_stddev` estimated the residual variance as `result.cost**2 / (m - n)`, but `least_squares`' `cost` is already `0.5 * sum(fun**2)`.
- **Fix:** it now uses `sum(r**2) / (m - n)` over the measurement residuals, excluding regularisation rows a subclass `cost_function` appends. The covariance still uses the full Jacobian.
- **TIAGo mocap:** arm_5's offset SE goes from 0.088 to 10.1 mrad. The PAL "conservative (≥ 2σ)" YAML keeps 19 of 41 `full_params` corrections instead of all 41.
- `add_base_name` shifts `base_mapping_slice` when it prepends the base names at `joint_offset` level; the slice pointed six entries too early.
- Calibration held-out data (`validation_data_file`) works (#105).
- **Config key read:** `tasks.calibration.data.validation_data_file` is now read from the unified config. Before, only a CLI override reached `calib_config`.
- **Training count preserved:** loading the validation CSV no longer overwrites the training `calib_config["NbSample"]`. A larger validation set crashed `solve()`, and a smaller one silently truncated training.
- **Unpack order fixed:** `_load_validation_data` no longer swaps poses and joint configurations.
- **All samples used:** validation metrics now evaluate every validation sample (`_compute_logmap_residuals` takes `n_samples`).
- **Failures logged:** a validation file that fails to load now logs a warning instead of being skipped silently.
- Calibration `joint_offset` parameters (`offset{PX,PY,PZ,RX,RY,RZ}_<joint>`) are now offsets of the joint configuration, `q + offset` (#101). They were added to the joint placement's parent-frame RPY, a rotation about the parent axis, which differs from a joint offset whenever the joint axis is not the parent's: up to 47 mm at the TIAGo tool for 0.05 rad on arm_2. New `calibration_tools.apply_joint_offset` composes the offset in the joint frame; elastic deflections (`k_*`, `non_geom`) use it too, and placements are restored exactly after each FK evaluation. `full_params` (`d_p*`, `d_phi*`) is unchanged. **Behaviour change:** `joint_offset` results from ≤ 0.5.0 on joints whose axis is not the parent's z are not joint offsets; refit them. On the corrected TIAGo 2021-11-30 mocap data the fit now returns arm_5 −49.7 mrad and arm_2/arm_6 near zero, instead of +22.8 / −31.7 mrad.
- `export_urdf` writes joint offsets into the joint `<origin>` (about the joint's own axis), so the reloaded URDF reproduces `q + offset`. They were written to `<calibration rising>`, which Pinocchio and robot_state_publisher ignore, so exported joint offsets had no effect on reload (#101).

### [0.5.0] - 2026-10-03

#### Changed

- The blocking `Lint` job runs every pre-commit hook on the full tree instead of only on changed files, and the advisory `Lint backlog` job is removed: the legacy lint debt inventoried in #57 was cleared in #81-#85.
- Replace universal quality gates with scoped execution/prediction verification; preserve diagnostic metrics, record incomplete required evidence, reject nonfinite results, and require explicit per-output limits for prediction acceptance. (#70) **Behaviour change:** `verify()` without `scope` now means prediction acceptance, so callers that only checked `.passed` get `False` (not evaluated) unless they supply separately loaded validation data and limits; pass `scope="execution"` for numerical-execution checks. The default thresholds are now empty, and training-data fallback metrics are named `training_*`.
- Bound Pinocchio to `pin>=3.7,<5`, replace deprecated frame-parent access, and add explicit Pinocchio 3.7.0 / 4.1.0 CI profiles with compatible ndcurves versions. Constraints apply before environment creation; CI checks native imports, installed versions and dependency consistency.
- Require one focused PR per completed issue and explicit maintainer approval before agents merge, with the policy recorded in CONTRIBUTING, AGENTS and the PR review checklist.
- Reorganize the roadmap around delivery outcomes and exit gates, preserve its previous detailed planning snapshot in an archive, and replace the architecture inventory with a source-audited description of current contracts and limits.
- Document the `devel` integration / `main` release workflow in CONTRIBUTING, refresh agent guidance, and add issue/PR templates and a design-decision index.
- Add Python 3.12 core and MuJoCo 3.9/3.14 test jobs, critical whole-tree lint, changed-file pre-commit checks, and an advisory report of existing lint debt. Documentation builds now check PRs to both development and release branches; installation failures propagate, and deployment only runs on pushes to `main`. CI fetches the pinned examples mesh fixtures required by the full suite and fixes a platform-sensitive QR precision assertion to use absolute tolerance.

#### Fixed

- Optimal trajectory generation no longer exhausts memory or the time budget (figaroh-examples#50). Pivoted QR in `qrdecomposition` uses economic mode instead of building a full m×m Q (13.7 GB for UR10 at setup; TIAGo was killed). The trajectory problem uses forward-difference objective gradients instead of numdifftools, `calc_torque` calls RNEA once per sample instead of once per joint, and initial waypoint steps are clamped to the velocity limits so the feasibility search succeeds. Per-sample constraint-check messages are DEBUG, not WARNING.
- `RobotIPOPTSolver` no longer reports IPOPT status -1 (maximum iterations exceeded) as success. **Behaviour change:** a trajectory segment that stops at the iteration limit is kept only if its final iterate is feasible; it is then marked `converged: False` with a warning. The iteration cap is configurable as `problem.max_iterations` (default 200).
- Configuration variants that `extends` a section (e.g. `tasks.calibration`) now override that section. They were merged into the configuration root, so the selected task silently kept its base values (#76).
- `figaroh.utils.config_parser.get_param_from_yaml()` accepts whole legacy files with `calibration:` / `identification:` sections and dispatches the requested one; `task_type="auto"` selects a single present section and still requires an explicit task when both exist (#76).
- Configuration differentiation now estimates acceleration for every tangent coordinate, including the last fixed-base joint. Derivative widths use `nv` rather than `nq` or effort-selection flags, supporting quaternion/continuous joints through model/backend differences. Uneven intervals use velocity midpoint times; malformed/nonpositive/nonfinite intervals fail explicitly. The legacy trailing-two-sample trim and interval velocity alignment remain. Older UR10 dataset fits remain preprocessing-limited until torque-generation provenance and sampling/filter assumptions are audited separately. (#32)
- Physical-consistency fallback, SDP projection and SDP reconstruction now distinguish link-origin rotational inertia from the second-moment block of pseudo-inertia. Projection weights and returned p10 entries remain in dynamic-parameter coordinates; reconstruction enforces inertia triangle inequalities as well as positive mass. Previously SDP fits could accept physically invalid inertias. First-moment projection weights now penalize each component independently instead of only a weighted sum, preventing changes to feasible nonzero-CoM inputs. URDF reconstruction priors now use link-origin tensor entries via Pinocchio's dynamic-parameter conversion, including the parallel-axis contribution of a nonzero CoM. (#21)
- MuJoCo mass-matrix evaluation now supports both `mj_fullM` signatures: the pre-3.10 `(model, destination, qM)` form and the newer `(model, data, destination)` form. This fixes failures with recent MuJoCo versions that removed `data.qM`.
- `get_standard_parameters` read each joint's CAD inertia from the body before it (`model.inertias[i]` instead of `[i + 1]`), so `tau_ref`, the nominal validation torque/`improvement_pct` and the reconstruction prior were all off by one body. Base parameters were unaffected. (#11)
- Regressor extra columns (`fv`, `fs`, `Ia`, `off`) are now laid out from the enabled flags instead of fixed offsets. Previously any combination other than "friction only" or "all three" raised `IndexError`. `add_standard_additional_parameters` now only emits enabled blocks, so parameter keys always line up with regressor columns. With friction only, the parameter dict no longer carries unused `Ia_*`/`off_*` keys. (#12)
- Unified config: `signal_processing.sampling_frequency`, `cutoff_frequency` and `filter_order` now default the filter's `f_sample`/`f_butter`/`nbutter`, which were stuck at 100 Hz / 2 Hz / 4. Explicit `filter_params` still win. A cutoff at or above Nyquist raises `ValueError`. (#13)

#### Added

- Trajectory optimisation can retry a failed segment from a new initial guess: `problem.segment_attempts` (default 1, no retry). See figaroh-examples#60.
- `tasks.identification.problem.qr_relative_tolerance`: QR rank threshold as a fraction of the largest pivot. Default unset (unchanged behaviour). A warning suggests it when the base regressor's condition number exceeds 1e6. (#14)

### [0.4.8] - 2026-09-07

#### Added

- `skills`: New `skills/` directory shipping six agent skills that take a user from a fresh clone to a running task, so an AI coding agent can set each task up without first re-deriving the workspace layout. Mirrored in `figaroh-examples/skills/` (same content, paths resolved against `$FIGAROH_WS`, the directory holding both repos side by side):
- `figaroh-start` — the router. Maps the workspace (which of the sibling repos owns what), carries a robot x task x config matrix, states the two path rules every example script depends on (run from `examples/<robot>/`; `package_dirs="../../models"`), and dispatches to the skill below.
- `figaroh-setup-env` — environment bootstrap, with an executable `scripts/doctor.sh`: read-only, exits 0/1, locates the workspace root on its own, and prints a specific remedy per failure. It checks the failure that is otherwise invisible — whether `import figaroh` resolves to the local `src/` tree or to a shadowing site-packages copy.
- `figaroh-setup-calibration`, `figaroh-setup-identification`, `figaroh-setup-optimal` — one per task: required inputs, the exact config block, the real CLI flag surface, how to read the run archive, and a failure/cause/fix table. The data contracts are documented from `calibration/data_loader.py` and each robot's `load_trajectory_data()` rather than from prose, because both are derived rather than fixed: calibration CSV columns follow each marker's `measurable_dof`, and identification has no single schema at all.
- `figaroh-setup-new-robot` — onboarding a robot with no example folder, from description package to `validate.py --robot <robot>` exiting 0. The set is self-contained by design: it assumes only `figaroh` and `figaroh-examples`, references no other package or checkout, and points at repo directories rather than at skills outside the set.
- `docs`: README now points at `skills/` for agent-assisted setup.

#### Fixed

- `tools`: `build_total_regressor_wrench` raised `NameError` on every call. Its signature named the settings dict `param` while the body read `identif_config` three times -- an incomplete rename that made the exported function unusable. Renamed the parameter to `identif_config`, matching both the body and the sibling `build_total_regressor_current`. Nothing called it and nothing tested it, which is how a fully broken public function went unnoticed; `tests/unit/test_regressor_wrench.py` now covers it (7 tests, verified to fail if the rename is reverted).
- `optimal`: `BaseOptimalCalibration.save_results` reported `configuration_count` as `len(self.optimal_configurations)`, which counts the dict's keys (always 2) rather than the selected configurations. A UR10 run wrote `configuration_count: 2` to the summary CSV while printing "Selected 73" to stdout. Added `count_optimal_configurations()` and used it, so the saved summary now matches what the run reports.
- `optimal`: the D-optimality determinant root was saved under the key `condition_number`, which is a different quantity entirely and made the CSV actively misleading. Renamed to `d_optimality_detroot`. **Breaking** for anything reading that column out of a saved optimal-calibration CSV/YAML.
- `optimal`: `tasks.optimal_configuration.output.output_file` was silently ignored — `load_param` only ever loads the `calibration` task, so the optimal task's own `output` block never reached the object, and `save_results` hardcoded `results/`. The configured directory is now honoured (the filename stays managed/timestamped), falling back to `results/` when unset, so existing behaviour is unchanged for configs that do not set it.

#### Tests

- `tests/unit/test_optimal_results.py`: 17 new tests covering the three fixes above — configuration counting, output-directory resolution, and reading the optimal task's output block (including the YAML `None`-as-string trap, legacy configs, and missing files). `save_results` previously had no coverage.
- `tests/unit/test_regressor_wrench.py`: 7 new tests calling `build_total_regressor_wrench` for real — output shapes, finiteness, `mass_load` scaling, and `which_body_loaded` block selection. Verified to fail with the original `NameError` if the fix is reverted.

#### Changed

- `build`: Pinned the build backend to `hatchling<1.32`. 1.32.0 emits `Metadata-Version: 2.5`, which the current release toolchain rejects -- `twine check` fails with "'2.5' is not a valid metadata version" because `packaging` 25.0 recognises only up to 2.4. Every hatchling through 1.31 emits 2.4. `requires` was previously unpinned, so each build silently took whatever was newest; this also makes release builds reproducible.
- `docs`: Updated README's Examples Repository table with recent `figaroh-examples` additions: TIAGo Pro right-arm geometric calibration (contributed by Clement Pene), TALOS's single-plane table-contact whole-body calibration (no longer "to be released" — it shipped), and TIAGo's new experimental suspension-identification and empirical-backlash-surface examples.
- `docs`: Split `docs/decisions/tiago-suspension-backlash-and-modular-terms-plan.md` (an uncommitted working draft) into two focused documents reflecting their different implementation status: `tiago-suspension-backlash-examples.md` (**done** — the TIAGo suspension-identification and empirical-backlash-surface examples shipped in `figaroh-examples` PR #9, with an implementation summary, concrete real-data results, and known gaps against the original plan) and `modular-linear-residual-terms-plan.md` (still **proposed** — the generic `LinearRegressorTerm`/`ResidualTerm`/`WeightPolicy` architecture and the unbuilt physical/stateful backlash model). Also corrected `ROADMAP.md` §8.2's stale "not started" status for the suspension port (superseded by the new document) and added both new documents to `ROADMAP.md`'s References table and `docs/source/further_reading/decisions.md`'s index.
- `docs`: Consolidated `docs/decisions/` from 8 documents to 6 by merging related comparisons/analyses/reviews into single multi-part documents, each carrying its own verified implementation-status section rather than trusting the original "no implementation yet"/status-line claims:
- `external-tool-comparisons.md` — Part A (`robot_calibration` comparison
  - calibration-composability adaptation roadmap), Part B (MuJoCo `sysid` comparison), Part C (the full reporting/verification infrastructure build-out that Part B's comparison produced).
- `tiago-calibration-and-port-review.md` — Part A (TIAGo/TIAGo Pro structural & statistical calibration analysis, redistribution rationale), Part B (eye-hand + suspension identification port review).
- Remaining decision docs renamed to consistent kebab-case filenames (`figaroh-examples-improvement_plan.md`, `urdf_exporter.md`, `validation-quality-report.md`), with stale status markers corrected against the current codebase (e.g. `IMPROVEMENT_PLAN.md`'s Phase 7 was falsely marked all-complete; only 1 of 4 items actually was).
- `docs`: Rewrote `ROADMAP.md` (v2.0 → v2.1) as a single coherent document: added a table of contents, a References section linking every roadmap track to its source `docs/decisions/` document, and a consolidated Estimated Timeline spanning all tracks. Added three tracks that previously existed only as detailed decision docs with no roadmap-level rollup — **Track C** (Reporting, Verification & Quality Infrastructure), **Track D** (Calibration Layer Composability, `robot_calibration`-inspired), **Track E** (Example Ecosystem Parity & Robot Ports). Re-verified every Track A/B status marker against the codebase and test suite rather than carrying the prior draft forward; corrected one stale claim (`integration/api.py`/`RobotIdentificationSystem` is implemented, was marked "not started"). Removed the old per-version "Revision History" section, which had drifted into duplicating this changelog, in favor of a short roadmap-document-only history.
- `docs`: Added **Track F — Deployment & Sim-to-Real Integration** to `ROADMAP.md`, explicitly marked as ongoing research rather than a committed track. Sourced from a new `docs/decisions/sim2real-modelbased-deployment.md` positioning document (moved in from a working draft), which proposes a model-based deployment/control layer for RL/IL policies as FIGAROH's headline value proposition alongside its existing identification/calibration role. Cross-referenced its phase-level overlaps with existing tracks (backend cross-validation, online identification, URDF/friction export) so they supersede rather than duplicate the corresponding Track A/B/D/E items, and flagged ecosystem-integration questions (e.g. mjlab, motion-retargeting libraries) as open research, not yet scoped into any phase.

### [0.4.7] - 2026-08-07

#### Added

- `feat(calibration)`: `BaseCalibration.redistribute_parameters()` — minimum-norm redistribution of fitted base-parameter values (and their covariance) onto the full standard-parameter set, via `figaroh.tools.qrdecomposition.redistribute_min_norm`/ `propagate_covariance_min_norm` (Moore-Penrose pseudoinverse of the base-mapping matrix `M`, `phi_base = M @ theta_r`). Previously, `calculate_base_kinematics_regressor` already computed this combination internally but discarded it, deploying each fitted value under one arbitrary representative parameter name with the rest of its redundant group implicitly left at 0 — this is one choice among infinitely many that reproduce the same fit. Redistribution spreads the fitted value across the whole group instead (round-trips exactly to the same predictions), and the propagated covariance flags which directions are genuinely underdetermined rather than reporting false precision. Opt-in; no change to existing `solve()`/report behavior. See `docs/decisions/tiago-calibration-and-port-review.md` Part A §A.8 for the full motivation and literature context.
- `feat(report)`: the HTML calibration report now includes a "Redistributed standard parameters" section, calling the above `redistribute_parameters()` on the live calibrator and rendering it through the same `_param_uncertainty_section` table used for the base-only fit — degrades to a muted "not available" message rather than failing when redistribution isn't possible for a given run.
- `feat(tools)`: `figaroh.tools.geometric_calibration_export` — `build_geometric_calibration()`/`export_geometric_calibration_yaml()` produce the PAL Robotics `robot_state_publisher.geometric_calibration` runtime-correction YAML (the format hand-curated to date in `figaroh_tiagoPro/data/master_calibration_*.yaml`) directly from a solved `BaseCalibration` instance's redistributed parameters, with an optional `min_sigma` threshold reproducing the "conservative" (≥2σ) deploy variant. Reuses `urdf_exporter._parse_param_name` for joint/axis parsing; excludes the co-estimated-base-merged parameter block structurally (via `base_mapping_row_names`) rather than by name pattern, so it's robust to whatever display renaming a `BaseCalibration` subclass applies to `calib_config["param_name"]`.
- `feat(examples)`: `tiago`, `tiago_pro`, `ur10`, and `talos` example calibration scripts now export `master_calibration.yaml` (full) and `master_calibration_conservative.yaml` (≥2σ) to the run directory by default alongside `report.html`, via the new `geometric_calibration_export` module — new `--geometric-calibration-yaml` CLI flag (default on) to opt out, mirroring the existing `--html-report` flag.
- `feat(report)`: both the "Parameter uncertainty" table (base-only fit) and the new "Redistributed standard parameters" table now show each parameter's actual identified **value**, not just its standard error — `_param_uncertainty_section` gained an optional `values` column, and `BaseCalibration`'s stored evaluation metrics gained `param_values` (`result.x`) alongside the existing `param_stdev`/`param_stddev_percentage` so the report has a value to show. The identification report's base-parameter table picks up the same column for free (`phi_base` was already computed there, just not threaded through to the table). Backward compatible: a caller/older run without `param_values` renders "—" instead of failing.

### [0.4.6] - 2026-08-06

#### Fixed

- `fix(calibration)`: `calc_updated_fkm` now supports joint elasticity (`non_geom`) and camera/ref-frame base composition — capabilities previously implemented only in a second, dead-code function, `update_forward_kinematics` (zero callers in `figaroh`/`figaroh-examples`), which had regressed relative to its own predecessor and was deleted as part of this fix:
- The base/camera transform (`bMo`/`wMo`) was computed but never composed into the returned pose.
- Elasticity indexing (`xyz_rpy[elas_id + 3]`) went out of bounds for any rotary joint — i.e. every revolute joint.
- The elasticity parameter-match loop reused a stale `key` left over from an unrelated earlier loop instead of iterating its own.
- The per-sample pose write was gated on a parameter-count bookkeeping variable that accumulated across the whole sample loop instead of resetting per sample, silently zeroing out later samples.
- (Found while implementing the merge) the elasticity branch also read `data.oMf`/`get_rel_transform` without re-running forward kinematics after perturbing `model.jointPlacements` for the deflection, so the elastic offset never actually reached the output pose — fixed by re-running FK before computing `oMee`.
- `calc_updated_fkm` previously silently ignored `calib_config["non_geom"]` entirely (every real caller uses this function, not the dead one) — it now actually applies elasticity when the flag is set.
- `NbMarkers > 1` now raises `NotImplementedError` instead of silently logging a warning and falling back to an identity marker transform.
- Verified against the TIAGo Pro reference calibration dataset (`figaroh-examples/examples/tiago_pro`, 94 samples): the existing `full_params`/`known_baseframe=False` geometric path is numerically unchanged (bit-identical output for a fixed parameter set and data).
- `fix(config)`: `unified_to_legacy_config` read `parameters.free_flyer`, a key the unified schema never defines (the template puts it at `kinematics.free_flying_base`) — a unified config setting `free_flying_base: true` was silently ignored, always resolving to `free_flyer=False`. Extraction moved to `_extract_frame_info`, reading the correct key.
- `fix(config)`: eye-hand calibration (`base_to_ref_frame`/`ref_frame`) had no unified-format equivalent, even though the schema template already reserved `tasks.calibration.eye_hand.{camera_frame,reference_frame}` for it — `unified_to_legacy_config` just never read that section. Added `_extract_eye_hand_params`, wired in.
- `fix(calibration)`: `BaseCalibration` reported RMSE/MAE two different ways that could disagree by exactly `sqrt(n_dofs)` depending on which number you looked at — `_evaluate_solution`'s `evaluation_metrics["rmse"]`/`["mae"]` flattened every x/y/z/... residual component into one list (a "mean of squared components" convention), while `_compute_per_dof_stats`'s `pos_rmse_mm`/`orient_rmse_deg` and the validation table reduced each sample's residual vector to one Euclidean-norm distance first, then aggregated. For position-only calibration these differed by exactly `sqrt(3)`, always. Converged `_evaluate_solution`, `_detect_outliers`, and `_store_optimization_results`'s `_PEE_dist` onto the per-sample-distance convention — the physically meaningful one (an actual 3D positioning error in mm) and the one most of the report already used. Outlier detection is unaffected (scale-invariant comparison). Also added `pos_mae_mm`/`orient_mae_deg` to `_compute_per_dof_stats`'s "overall" block (RMSE already had a position/orientation split; MAE didn't), surfaced as "Position MAE"/"Orientation MAE" in both the terminal quality report and the HTML report (`report.py`). Re-verified against tiago, tiago_pro, ur10, and talos calibration runs.
- `fix(calibration)`: `calc_stddev()` ran *after* `_compute_parameter_correlation()` in `BaseCalibration`, but correlation reads `self._C_param`, which `calc_stddev()` is what sets — on a fresh instance's first `solve()`, correlation silently returned `[]` instead of the actual pairs. Reordered so `calc_stddev()` runs first.

#### Added

- `feat(config)`: Legacy flat config format (`calibration:`/`identification:` top-level sections) is now deprecated in favor of the unified format — loading one emits a `DeprecationWarning` (+ `logger.warning`) from `figaroh.calibration.config.get_param_from_yaml` / `figaroh.identification.config.get_param_from_yaml`, the single parser every legacy-format caller funnels through regardless of entry point.
- `feat(utils)`: `figaroh.utils.config_migration` — converts a legacy config to unified format automatically (`python -m figaroh.utils.config_migration --input legacy.yaml --output unified.yaml`), including the eye-hand and free-flyer fields fixed above. An optional `--urdf` flag runs a round-trip self-check (converts, then re-parses through the real `unified_to_legacy_*config` and diffs against the original) rather than trusting the conversion blindly. Fields with no unified-format consumer today are preserved under `custom:` instead of silently dropped. See "Migrating from legacy to unified format" in `docs/source/concepts/config_parameters.md`.

### [0.4.5] - 2026-07-13

#### Added

- `feat(identification)`: Weighted least squares (WLS) support — `BaseIdentification.solve(wls=True)` refines the OLS base-parameter estimate with iteratively-weighted least squares (Gautier, 1997) before computing quality metrics, so the terminal/HTML report and `verify()` verdict reflect the WLS-refined parameters rather than a stale OLS estimate. Config-driven via `identification.problem.wls` in the unified YAML config, following the existing `has_friction`/`has_joint_offset` pattern.
- `feat(tooling)`: `figaroh.tools.provenance` and `figaroh.tools.run_archive` — every calibration/identification run now carries a provenance record (git commit, config hash, timestamps, robot/asset identity), and `archive_run()` writes each run to a timestamped `results/runs/<robot>/<task>/<timestamp>/` directory (config snapshot, provenance JSON, report) instead of overwriting a single `results/` path.
- `feat(validation)`: When no separate validation dataset is configured, calibration and identification now fall back to evaluating validation metrics against the training data itself — rather than silently omitting the validation section — with an explicit warning surfaced in the terminal report, HTML report, and machine-readable insights.
- `feat(reporting)`: Self-contained HTML diagnostic reports for both calibration and identification.
- `figaroh.tools.report.generate_calibration_report()` / `BaseCalibration.export_html_report()` — summary, auto-generated insights, per-DOF residual table, parameter uncertainty bars, correlation section, validation section.
- `figaroh.tools.identification_report.generate_identification_report()` / `BaseIdentification.export_html_report()` — same shape, adapted to identification's per-joint torque residuals and base-parameter uncertainty.
- `figaroh.tools._report_common` — shared HTML/CSS kernel (styling, escaping, insight/uncertainty/correlation section builders) reused by both generators.
- Opt-in via `solve(html_report=True)` on both base classes, matching the existing `plotting`/`save_results` flag pattern.
- `feat(identification)`: `BaseIdentification.print_quality_report()` — terminal quality report (condition number, RMSE, per-joint residuals, base-parameter uncertainty, validation, physical-consistency/ reconstruction status), the identification analogue of calibration's existing terminal report.
- `feat(identification)`: Held-out validation support for identification, mirroring calibration's — a genuinely separate dataset via `identification.data.validation_data_file`, never a runtime split of training data.
- `feat(reporting)`: Machine-readable verification verdicts.
- `VerificationVerdict` / `ThresholdCheck` (`figaroh.tools._report_common`) — a structured pass/fail against per-metric thresholds, with provenance (git commit, config hash, timestamp).
- `verify(thresholds=None)` and `export_verification_report(output_path=None, output_dir="results", thresholds=None)` added to both `BaseCalibration` and `BaseIdentification`. Default thresholds cover condition number and, when validation data is configured, validation-set RMSE/correlation/ improvement — every threshold is overridable per call. A threshold whose metric isn't computable is skipped, not failed.
- Called explicitly (not from inside `solve()`) — verification is opt-in output computed from already-stored results.
- `feat(reporting)`: Interactive before/after panel embedded in both HTML reports — a zoomable, hoverable overlay of nominal vs. fitted vs. measured values, rendered as hand-rolled inline SVG (no new dependency).
- `feat(reporting)`: `figaroh.tools.compare_report.generate_compare_page()` — a static, self-contained, offline HTML page that loads two exported verification JSON files client-side and renders a per-metric diff table plus an overlaid before/after chart, gated by a mandatory compatibility check (domain, joint/DOF names, `decimate`, sample count) with an explicit "compare anyway" override on mismatch. No backend, no run history.
- `fix(results_manager)`: `plot_identification_results()` reshaped 1D torque arrays as `reshape(-1, 1)`, silently collapsing a multi-joint, joint-major-flattened array into a single mislabeled trace. Now accepts explicit `n_joints`/`joint_names` and reshapes joint-major (`reshape(n_joints, -1).T`) when provided.
- `refactor(results_manager)`: `plot_with_fallback()` helper deduplicates the four near-identical try/except plotting blocks across `BaseCalibration`, `BaseIdentification`, `BaseOptimalCalibration`, and `BaseOptimalTrajectory`.

#### Fixed

- `fix(calibration)`: `BaseCalibration.calc_stddev()` computed the calibrated-parameter count (`self.nvars`) once in `__init__`, before `initialize()` populated the actual parameter list — leaving it stuck at 0 and silently producing empty `std_dev`/`std_pctg` lists. The "Parameter uncertainty" section was therefore always empty in both the terminal and HTML calibration reports. Now derives the count from the solved parameter vector (`len(result.x)`) at computation time.
- `fix(identification)`: `_compute_validation_metrics()`'s before/after torque chart sliced the first `n_active` joint-blocks off the full-model regressor, which is only correct when the active/identified joints are exactly DOFs `0..n_active-1`. For robots identifying a strict subset of joints (e.g. TIAGo: torso+arm out of many more DOFs), this silently returned the wrong joints' nominal/identified torque predictions — visible as identical nominal/fitted curves and wildly wrong magnitudes in the before/after chart. Now indexes by the actual `act_idxv` DOF indices, mirroring the row-selection already used correctly in `_decimate_regressor_matrix`.
- `fix(identification)`: `_apply_weighted_least_squares()` sliced the stacked regressor into `model.nv` equal blocks to estimate per-joint residual variance, which is only correct when every model joint is identified. For subset-identified robots (TIAGo: `model.nv=24`, 8 joints identified), this misaligned the slicing entirely, producing an RMSE ~2000x worse than plain OLS. Now uses `len(identif_config["act_idxv"])`, the same quantity `_decimate_regressor_matrix` already uses.
- `fix(identification)`: `_prepare_undecimated_data()` flattened torque data sample-major while regressor rows are joint-major, corrupting `solve(decimate=False)`'s correlation/RMSE (0.44 vs 0.966 on identical data pre-fix).
- `fix(reporting)`: the before/after HTML panel's joint/DOF dropdown rendered its options twice — once server-side in the generated HTML, once again client-side via JS — so every entry appeared duplicated in the selector.
- `fix(reporting)`: the before/after HTML panel embedded the full validation-set series inline with no cap; for large validation sets (e.g. a fallback-to-full-training-data run with ~45k samples) this produced multi-MB HTML pages whose SVG path strings were too large to render reliably in the browser. Now uniformly subsampled to at most 3000 points, with the displayed sample count disclosed in the chart caption.
- `fix(export_validation)`: the interactive viser GUI's speed/pose/opacity sliders crashed with `AttributeError: 'GuiEvent' object has no attribute 'value'` on every drag — `GuiEvent` exposes the changed control via `.target`, not `.value` directly.

Full design history and rationale: [`docs/decisions/external-tool-comparisons.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/external-tool-comparisons.md) Part C.

### [0.4.4] - 2026-06-27

#### Added

- `feat(calibration)`: Add validation data support for ground-truth FK testing.
- `_load_validation_data(path)` — load a separate validation measurement CSV.
- `_compute_validation_metrics()` — compare nominal vs calibrated FK against held-out measurements; reports position/orientation RMSE, max error, and improvement percentage.
- Validation data path configurable via YAML (`validation_data_file`) or CLI (`--validation-data`) in example scripts.
- `feat(calibration)`: Add comprehensive statistical quality report.
- `print_quality_report()` — formatted terminal table with convergence status, per-DOF residuals (X/Y/Z/rx/ry/rz with R²), overall RMSE, validation metrics, parameter uncertainty (top 5), and strongly correlated pairs (|ρ| > 0.8).
- `_compute_per_dof_stats()` — mean, std, RMSE, max, R² per kinematic DOF.
- `_compute_condition_number()` — condition number of Jacobian with well/moderate/ill-conditioned label.
- `_compute_parameter_correlation()` — flags parameter pairs with |ρ| > 0.8 from the covariance matrix.
- Report printed automatically after `solve()` completes.
- `feat(calibration)`: Add `position_frame='body'|'world'` option to `_compute_logmap_residuals()` — choose body-frame or world-frame position error.
- `feat(urdf_exporter)`: New module `figaroh.tools.urdf_exporter` for applying calibrated joint parameters to URDF files with 12 joint-level categories.
- `feat(export_validation)`: New module `figaroh.tools.export_validation` with `URDFComparison` class — FK consistency checks, interactive viser visualization (trajectory animation, static overlay, error plots, replay controls).
- `feat(backends)`: Multi-simulator backend architecture.
- `PinocchioBackend` — 371 lines wrapping all Pinocchio dynamics/kinematics calls.
- `MuJoCoBackend` — 441 lines with URDF→MJCF auto-conversion.
- `DynamicsBackend` abstract interface with 9 abstract + 6 optional methods.
- `RobotIdentificationSystem` high-level integration API.
- `feat(tests)`: Cross-backend validation suite, backend unit tests (32 Pinocchio
- 13 MuJoCo), TIAGo integration test fixture, URDF exporter tests.
- `docs`: Combined ROADMAP.md (Track A + Track B), comprehensive architecture docs, ADR for calibration validation plan.

#### Fixed

- `fix(calibration)`: Replace element-wise RPY subtraction with proper SE3 log-map pose error in `_compute_logmap_residuals()` — geometrically correct orientation residuals without singularity issues.
- `fix(backends)`: MuJoCoBackend regressor (delegates to Pinocchio analytical), Coriolis matrix (finite-difference Jacobian/2), mass matrix (adds missing `mj_forward` call).

#### Changed

- `breaking(export_validation)`: Rename `trajectory_errors()` → `fk_consistency_check()` and `TrajectoryErrors` → `FkConsistencyResult` — clarifies this is a file-integrity check, NOT ground-truth FK validation.
- `chore`: Add black formatter to pre-commit hooks.
- `chore`: Modernize pyproject.toml with dev dependencies and tool configs.

#### Tests

- 293 passed, 20 skipped, 0 failed (from 208 passed with 4 pre-existing failures).
- New: `test_backends.py` (45 tests), `test_cross_backend.py` (integration), `test_tiago_fixture.py` (287 lines), `test_urdf_exporter.py` (296 lines), `test_integration.py` (250 lines).

### [0.4.3] - 2026-06-02

#### Added

- `feat(cad_constraints)`: New module `figaroh.identification.cad_constraints` with convex CAD-informed constraints for inertial parameter identification.
- `CADConstraints` dataclass — container for per-link mass bounds, first-moment (CoM) bounds, and symmetry equality constraints.
- `add_mass_bounds(cad, joint, *, m_min, m_max)` — add box constraint `m_min ≤ m_j ≤ m_max` for a link.
- `add_com_bounds(cad, joint, *, axis, h_min, h_max)` — add first-moment (linear) box constraint `h_min ≤ h_k ≤ h_max` for axis `k ∈ {x,y,z}`.
- `add_symmetry_constraints(cad, joint_a, joint_b, *, keys)` — add equality constraints between inertial parameters of two symmetric links.
- `bounds_from_urdf(model, ...)` — derive mass and CoM bounds automatically from Pinocchio model URDF inertials (CAD data source strategy).
- `build_cad_constraints_from_config(cfg, *, model)` — build `CADConstraints` from a YAML config sub-dict; returns `None` when empty (safe default-off).
- `apply_cad_constraints_to_problem(problem, theta, params_r, cad)` — inject CAD constraints into a picos `Problem` for the full-robot SDP.
- `feat(physical_consistency)`: `project_p10_lmi` gains optional `mass_bounds` and `com_bounds` kwargs — per-link box constraints on the SDP.
- `feat(physical_consistency)`: `project_robot_p10_lmi` gains optional `cad_constraints` kwarg — passes per-link bounds to `project_p10_lmi`.
- `feat(reconstruction)`: `_reconstruct_sdp` and `reconstruct_full_parameters` gain optional `cad_constraints` kwarg — injects CAD constraints after the per-joint LMI loop in the full-robot SDP.
- `feat(base_identification)`: Both `_apply_physical_consistency_if_enabled` and `_apply_reconstruction_if_enabled` hooks parse `cad_constraints` from config and pass the result to the underlying solvers.
- Example YAML comment block for `cad_constraints` in `figaroh-examples/examples/templates/manipulator_robot.yaml`.

#### Changed

- All new parameters are optional with safe defaults — fully backward-compatible.

### [0.4.2] - Unreleased

#### Added

- `feat(reconstruction)`: `ReconstructionResult.status` — outcome code (`"ok"`, `"solver_missing"`, `"error"`) on every result
- `feat(reconstruction)`: `ReconstructionResult.base_residual_norm` — L2 norm of the base constraint residual `M θ − φ_base` (scalar, always set)
- `feat(reconstruction)`: `ReconstructionResult.objective` — SDP objective value for Option B (`None` for nullspace)
- `feat(reconstruction)`: `BaseResult` frozen dataclass — structured container for `(M, phi_base, params_r)` as input to `reconstruct_full_parameters()`
- `feat(reconstruction)`: `reconstruct_full_parameters()` — unified entry point with `method="nullspace" | "sdp" | "auto"` dispatch
- `feat(reconstruction)`: Option B SDP (`method="sdp"`) — minimises `‖diag(w)(θ−θ₀)‖²` subject to `M θ = φ_base` and per-joint pseudo-inertia LMI `P_j ≽ 0` via Schur-complement epigraph (requires picos + cvxopt/mosek)
- `feat(reconstruction)`: `method="auto"` — silently falls back to nullspace when picos is not installed; `status="solver_missing"` when picos unavailable and `method="sdp"` was requested explicitly
- `feat(reconstruction)`: `_load_prior_from_urdf()` — builds flat prior dict from Pinocchio model inertias (`prior_source="urdf"`)
- `feat(reconstruction)`: `_load_prior_from_yaml()` — loads prior from a flat YAML file (`prior_source="yaml"`)
- `feat(identification)`: `_apply_reconstruction_if_enabled()` hook in `BaseIdentification._store_results()` — called after physical consistency; stores result under `self.result["reconstruction"]`
- `feat(identification)`: `_calculate_base_parameters()` now uses `QRDecomposer` directly to expose `self._M_matrix` / `self._params_r_for_recon` for downstream reconstruction; result dict includes `"M"` and `"params_r"` keys
- `feat(config)`: `reconstruction` block parsed from both legacy YAML (`get_param_from_yaml`) and unified config (`unified_to_legacy_identif_config`)
- `feat(identification/__init__)`: `BaseResult` and `reconstruct_full_parameters` added to public exports and `__all__`
- `tests`: 13 new unit tests in `tests/unit/test_reconstruction.py` covering new fields, BaseResult, prior helpers, `_p10_indices_for_joints`, nullspace end-to-end, auto fallback, YAML prior, and unsupported method error

#### Fixed

- `fix(reconstruction)`: alternation loop in `run_reconstruction` now correctly unpacks `project_robot_p10_lmi()` as `(projected_p10_dict, robot_report)`; removes `AttributeError: tuple has no attribute 'p10_by_joint'`
- `fix(identification)`: `"projected parameters"` key (space) renamed to `"projected_parameters"` (underscore) in `_apply_physical_consistency_if_enabled` result dict for consistency with `"raw_parameters"` and Python conventions
- `fix(tools/robotcollisions)`: `print_collision_pairs()` uses `print()` instead of `logger.info()` so output is visible in interactive use and captured by tests
- `fix(tools/solver)`: `LinearSolver._print_solution_info()` uses `print()` instead of `logger.info()` so verbose output is visible in interactive use and captured by tests

### [0.4.1] - Unreleased

#### Added

- `feat(physical-consistency)`: `is_feasible_link()` — public alias for `check_p10_feasibility()` matching roadmap spec naming
- `feat(physical-consistency)`: `project_link()` — public alias for `project_p10_lmi()` matching roadmap spec naming
- `feat(physical-consistency)`: `ProjectionReport.runtime` — per-link solve time (seconds) recorded via `time.perf_counter()` around `problem.solve()`
- `feat(physical-consistency)`: `weights` keyword argument to `project_robot_p10_lmi()` for passing a 10-element weight vector to all per-link projection calls
- `feat(config)`: `weights.mode: "auto" | "manual"` parsed from the `physical_consistency` YAML block in `_apply_physical_consistency_if_enabled`
- `feat(config)`: `weights.manual.{m, h, Sigma}` per-group manual weights built into a 10-element array and forwarded to `project_robot_p10_lmi`
- `feat(identification)`: `physical consistency` result dict now stores both `raw_parameters` (pre-projection) and `projected_parameters` (post-projection) as separate keys, preserving the original identified values for comparison
- `tests`: 28 unit tests in `tests/unit/test_physical_consistency.py` covering TC-1 through TC-12 (projection correctness, aliases, runtime, robot aggregation) plus 3 config-wiring tests for weights and raw/projected separation

#### Fixed

- `fix(physical-consistency)`: SDP formulation in `project_p10_lmi` no longer uses the picos 2.x-incompatible `pc.vstack`, `pc.multiply`, or `pc.sum_squares` functions; replaced with element-wise objective using `pc.SquaredNorm` and explicit sigma-entry loop
- `fix(physical-consistency)`: solver keyword `verbose=` replaced with `verbosity=` (picos 2.x API); eliminates `DeprecationWarning` on every call
- `fix(physical-consistency)`: feasibility check after projection uses a relaxed `psd_eig_tol=-1e-8` tolerance to absorb inevitable SDP solver numerical noise, preventing valid projections from being reported as `"infeasible"` due to tiny eigenvalue violations (~1e-9)

### [0.3.1] - 2026-04-01

#### Added

- `feat(physical-consistency)`: optional projection of inertial parameters onto a physically consistent set (`identification/physical_consistency.py`)
- `feat(reconstruction)`: reconstruction utilities — `run_reconstruction` and `run_option_a_reconstruction` entry points (`identification/reconstruction.py`)
- `feat(qrdecomposition)`: `get_M` / `get_M_labels` methods for retrieving the stored base mapping matrix after decomposition
- `feat(qrdecomposition)`: `QRResult` dataclass for structured, full-precision decomposition output (rank, base_indices, W_b, beta, M, phi_b, diag_R, cond_R1, method, …)
- `feat(qrdecomposition)`: `relative_tolerance` constructor parameter for scale-invariant rank detection (threshold relative to largest pivot)
- `feat(qrdecomposition)`: `get_diagnostics()` method returning rank, diag_R, cond_R1, and method after every decomposition
- `feat(qrdecomposition)`: `decompose()` unified entry point returning a `QRResult`; delegates to pivoting or double path based on `method` argument
- `feat(identification)`: enhanced parameter management with standard and custom parameter support in `BaseIdentification`

#### Fixed

- `fix(qrdecomposition)`: `_find_rank` now correctly returns 0 for zero/empty matrices (previously returned row count, causing phantom base parameters)
- `fix(reconstruction)`: add missing `run_option_a_reconstruction` alias that blocked package-level import

#### Changed

- `refactor(qrdecomposition)`: replace non-pivoted `numpy.linalg.qr` with `scipy.linalg.qr(..., pivoting=True)` in `_identify_base_parameters` — permutation-stable, deterministic basis selection
- `refactor(qrdecomposition)`: remove premature `np.around` from `beta` in all code paths; rounding deferred to display-only `_build_parameter_expressions`
- `refactor(qrdecomposition)`: remove fragile `assert np.allclose(W_base, W_b)`
- `refactor(qrdecomposition)`: enhanced docstrings and nominal-parameter handling
- `refactor(identification)`: patch filter configuration management in `BaseIdentification`
- `refactor(calibration)`: update deprecated `Frame.parent` → `Frame.parentJoint` for Pinocchio 3.x compatibility
- `refactor(logging)`: replace print statements with `logging` calls across calibration and identification modules
- `refactor(config)`: rename `load_from_yaml` → `load_param` for clarity

#### Tests

- `test(qrdecomposition)`: `TestNumericalImprovements` suite — 14 new tests, 23/23 total pass (rank-zero, relative tolerance, permutation stability ×20, mapping matrix property, column space, diagnostics, QRResult structure, full-precision beta)
- `test(reconstruction)`: unit tests for reconstruction utilities

______________________________________________________________________

### [0.3.0] - 2025-12-09

#### Added

- **Advanced Linear Solver (`figaroh.tools.solver`)**: Comprehensive multivariate linear solver for robot parameter identification
- Multiple solving methods: lstsq, QR, SVD, Ridge, Lasso, Elastic Net, Tikhonov, constrained, robust, weighted
- Regularization support: L1 (Lasso), L2 (Ridge), Elastic Net, custom Tikhonov
- Constraint handling: Box constraints (bounds), linear equality/inequality constraints
- Robust regression with iterative reweighting for outlier resistance
- Comprehensive solution quality metrics (RMSE, R², condition number, residuals)
- Optimized for dense, large, thin matrices typical in robot dynamics
- Full unit test coverage (18 tests) with robot identification scenarios
- **Module Reorganization**: Better code organization and separation of concerns
- **Calibration module restructuring**:
  - `calibration/config.py`: Configuration parsing and YAML handling (624 lines)
  - `calibration/parameter.py`: Parameter management utilities (240 lines)
  - `calibration/data_loader.py`: Data loading and I/O operations (160 lines)
- **Identification module restructuring**:
  - `identification/config.py`: Configuration parsing for identification (334 lines)
  - `identification/parameter.py`: Parameter management for identification (388 lines)
- Maintains 100% backward compatibility through re-exports
- **BaseIdentification Enhancement**:
- `solve_with_custom_solver()`: New method using advanced linear solver with regularization and constraints
- Flexible solving with multiple methods and custom constraints
- Support for physical parameter bounds (e.g., positive masses/inertias)

#### Improved

- **Parameter Ordering**: Changed to Pinocchio dynamic parameter ordering for consistency
- New order: [Ixx, Ixy, Ixz, Iyy, Iyz, Izz, mx, my, mz, m]
- Previous order: [m, mx, my, mz, Ixx, Ixy, Iyy, Ixz, Iyz, Izz]
- **Regressor Module**: Cleaned up build_basic_regressor methods
- Removed unused `tau` parameter for better API clarity
- Improved method signatures and documentation
- **Code Quality**: Significant reduction in code duplication
- `calibration_tools.py`: Reduced from ~1500 to ~630 lines (-58%)
- `identification_tools.py`: Reduced from ~900 to ~295 lines (-67%)
- Modular design with clear single responsibilities

#### Fixed

- Parameter naming: Changed from numbered indices to parent joint names for clarity
- Regressor handling: Better support for additional columns in regressor matrices
- Results manager imports and formatting issues

#### Technical Details

- **Files Changed**: 21 files
- **Lines Added**: +3,372
- **Lines Removed**: -1,604
- **Net Change**: +1,768 lines
- **Test Coverage**: All 18 new solver tests passing

### [0.2.4] - 2025-09-08

#### Changed

- **Optional Dependencies**: Removed `cyipopt` from required dependencies
- cyipopt is now truly optional and loaded only when IPOPT optimization is used
- Users can install without cyipopt and still use all other features
- Install cyipopt separately when needed: `pip install cyipopt` or via conda environment

#### Improved

- **Installation Flexibility**: Package now installs without requiring heavy optimization dependencies
- **Error Handling**: Better error messages when optional dependencies are missing

### [0.2.3] - 2025-09-08

#### Added

- **Streamlined Dependencies**: All core dependencies now available via PyPI with automatic installation
- **Lazy Loading**: Optional dependencies (cyipopt) now loaded only when needed to improve startup time
- **Enhanced Installation Notes**: Clear documentation of simplified dependency management

#### Improved

- **Dependency Management**: Complete cleanup and optimization of package dependencies
- Removed redundant `requirements.txt` and `setup.py` files
- Consolidated all dependencies in `pyproject.toml`
- Updated to use PyPI versions of robotics libraries (`pin` for Pinocchio)
- **Installation Process**: Significantly simplified installation with better cross-platform compatibility
- **Documentation**: Comprehensive README updates reflecting modern packaging standards
- Combined development installation methods for clarity
- Added official Pinocchio repository reference
- Updated dependency documentation with descriptions
- **Performance**: Faster module loading through localized imports
- **Environment Setup**: Streamlined conda environment with minimal dependencies

#### Enhanced

- **Import Strategy**: Localized cyipopt import to specific functions for better error handling
- **Error Messages**: More informative import error messages with installation instructions
- **Package Structure**: Modern Python packaging standards with pyproject.toml-only approach

#### Removed

- **Redundant Files**: Eliminated `requirements.txt` and `setup.py` in favor of modern `pyproject.toml`
- **Unnecessary Dependencies**: Cleaned up unused dependencies for leaner installation

#### Fixed

- **Package Name**: Corrected dependency references (e.g., proper use of `pin` for Pinocchio PyPI version)
- **Installation Conflicts**: Resolved potential conflicts between conda and pip installations

### [0.2.0] - 2025-09-05

#### Added

- **Unified Configuration System**: Complete overhaul of configuration management
- New `UnifiedConfigParser` with YAML template inheritance
- Automatic format detection for seamless legacy compatibility
- Advanced parameter mapping between configuration formats
- Comprehensive configuration validation with helpful error messages
- **Enhanced Base Classes**: Modern object-oriented workflow management
- `BaseCalibration`: Standardized calibration workflow with unified config support
- `BaseIdentification`: Standardized identification workflow with unified config support
- Automatic configuration format detection and conversion
- **Advanced Regressor Builder**: Complete redesign of regressor computation
- `RegressorBuilder`: Object-oriented, extensible regressor construction
- `RegressorConfig`: Configuration dataclass for regressor parameters
- Enhanced input validation and error handling
- Support for joint torque and external wrench modes
- **Configuration Format Mapping**: Seamless format conversion utilities
- `unified_to_legacy_config`: Calibration parameter mapping function
- `unified_to_legacy_identif_config`: Identification parameter mapping function
- Perfect compatibility with existing legacy configurations

#### Improved

- **Parameter Processing**: Enhanced parameter handling with better defaults
- **Error Messages**: More informative validation and error reporting
- **Documentation**: Comprehensive updates to README and module documentation
- **Code Organization**: Better structured modules with clear responsibilities
- **Type Safety**: Added type hints throughout the codebase

#### Enhanced

- **Cross-Platform Support**: Improved compatibility across operating systems
- **Input Validation**: Robust parameter validation and type checking
- **Template System**: Flexible configuration template inheritance
- **Backward Compatibility**: Full support for existing legacy configurations

#### Removed

- **quadprog dependency**: Removed unused quadprog dependency to reduce package size

#### Fixed

- **Configuration Parsing**: Resolved edge cases in YAML parsing
- **Parameter Mapping**: Accurate conversion between configuration formats
- **Validation Logic**: Improved configuration validation accuracy
- **Error Handling**: Better error recovery and user feedback

#### Technical Improvements

- Modern Python practices with dataclasses and type hints
- Enhanced error handling with custom exception classes
- Improved testing framework with comprehensive validation
- Better code documentation and examples

#### Documentation

- Updated README with modern API examples
- Enhanced configuration system documentation
- New API usage patterns and best practices
- Comprehensive module documentation updates

### [0.1.0] - 2025-01-25

#### Added

- Initial release of FIGAROH package
- Dynamic identification algorithms for rigid multi-body systems
- Geometric calibration algorithms for serial and tree-structure robots
- Support for URDF modeling convention
- Optimal trajectory generation for dynamic identification
- Optimal posture generation for geometric calibration
- Integration with Pinocchio for efficient computations
- Support for various optimization algorithms
- Data filtering and pre-processing utilities
- Model parameter update utilities

#### Features

- **Dynamic Identification**:
- Dynamic model including friction, actuator inertia, and joint torque offset
- Continuous optimal exciting trajectory generation
- Multiple parameter estimation algorithms
- Physically consistent standard inertial parameters calculation
- **Geometric Calibration**:
- Full kinematic parameter calibration
- Optimal calibration posture generation via combinatorial optimization
- Support for external sensors (cameras, motion capture)
- Non-external methods (planar constraints)

#### Dependencies

- Core scientific computing: numpy, scipy, matplotlib, pandas
- Robotics: pinocchio (via conda)
- Optimization: cyipopt (via conda)
- Visualization: meshcat
- Additional: numdifftools, ndcurves, rospkg

#### Documentation

- Comprehensive README with installation and usage instructions
- Examples moved to separate repository (figaroh-examples)
- API documentation structure prepared

#### Notes

- Examples and URDF models moved to separate repository for clean package distribution
- Package optimized for PyPI distribution
- Supports Python 3.8+

# Contributing

The contributor workflow is maintained alongside the code in [`CONTRIBUTING.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/CONTRIBUTING.md).

## Contributing to FIGAROH

Use this workflow for features, bug fixes, documentation and maintenance. Development commands run in the **`figaroh-dev` conda environment**. Start with [ARCHITECTURE.md](https://github.com/thanhndv212/figaroh-plus/blob/main/ARCHITECTURE.md), [ROADMAP.md](https://github.com/thanhndv212/figaroh-plus/blob/main/ROADMAP.md), and [validation.md](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/development/validation.md).

### One owner for each kind of information

| Information                                              | Authoritative location               |
| -------------------------------------------------------- | ------------------------------------ |
| Product outcomes, priorities and milestone exit criteria | `ROADMAP.md`                         |
| Current module boundaries, data flow and limitations     | `ARCHITECTURE.md`                    |
| Design choices, alternatives and research                | `docs/decisions/`                    |
| Task status, bugs, acceptance criteria and dependencies  | GitHub issues                        |
| What changed in a release                                | `CHANGELOG.md`                       |
| User instructions and API reference                      | `docs/source/` and source docstrings |
| Contribution and release procedure                       | This file                            |
| Agent environment, commands and implementation pitfalls  | `AGENTS.md`                          |

The site includes root roadmap/architecture documents directly. Update their canonical files rather than creating another copy. Keep detailed historical planning in the archive with an explicit historical banner.

### Planning discussion

The [identification/calibration delivery plan](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/index.md) is a **draft for discussion**, not an implementation mandate. Its proposed workflow amendments are reviewed alongside the roadmap before being adopted. Current issue/PR/merge rules below continue to apply. Keep new API contracts in focused decision records and avoid placing unreviewed proposals in the current architecture description.

### Issue → branch → PR

1. Select an existing issue or open one with the *Feature / work item* or *Bug report* template (the same forms are used in figaroh-examples). Write it for a reader without local context: link its delivery package, define testable acceptance criteria and list dependencies as `#N` (same repository) or `thanhndv212/figaroh-examples#N` (the other one), which GitHub links. See [how to read delivery issues](https://github.com/thanhndv212/figaroh-plus/blob/devel/docs/plans/README.md#how-to-read-delivery-issues). Bugs include a minimal reproducer, expected/actual results, environment and data provenance. Distinguish wrong numerical results, crashes, unsupported behavior and missing dependencies. For docs/CI work, a focused PR can carry the problem and acceptance criteria directly.
1. For a new public interface, schema, dependency or module boundary, add a [decision record](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/README.md) before or with the implementation. A design proposal is not evidence that the feature exists.
1. Branch from current `devel`: `feature/<issue>-<slug>`, `fix/<issue>-<slug>`, `docs/<slug>` or `chore/<slug>`. Keep a PR focused on one outcome.
1. Implement the acceptance criteria. Fixes include a regression test that exposes the original failure; features include meaningful behavioral tests. Preserve unrelated changes and avoid bundling numerical changes with broad formatting. For existing lint debt, clean the touched files or separate the necessary cleanup into a reviewable preliminary commit.
1. Run the [required validation](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/development/validation.md). Record commands, environment, pass/fail/skip counts and relevant domain metrics. Fix failures; do not hide them with test exclusions or new skip markers.
1. When one issue's implementation and local validation are complete, commit and push its focused branch and open a PR **against `devel`** using the template. Keep each completed issue in its own PR; independent work may continue on a separate branch. Include a changelog entry under `[Unreleased]` (including documentation reorganizations), user docs for changed behavior, and architecture/roadmap updates when their facts change.
1. The maintainer reviews each PR. Agents must wait for the maintainer's explicit approval before merging; passing checks or a request to implement/open a PR does not authorize a merge. Address review comments and rerun affected checks. After approval and required checks pass on the current head, merge the focused feature/fix PR with a **merge commit** (not squash or rebase), so the PR's individual commits stay in `devel`'s history. If the approved scope changes materially, obtain a renewed review before merging. Use a Conventional Commit title, e.g. `fix(identification): preserve joint ordering`.
1. Close the issue with the merged PR linked. Because `main` is the default branch, `Closes #N` may not close it when merging into `devel`. Mark roadmap outcomes complete only after their exit criteria pass.

For triage, use one type (`bug`, `feature`, `docs`, `maintenance`), an area (`identification`, `calibration`, `optimal`, `backends`, `examples`, `tooling`) and a priority. Put only actionable, sufficiently specified issues into `Ready`; then use `In progress` → `In review` → `Done`. These are recommended tracker fields, not a claim that a GitHub Project or branch protection is configured.

### Repository and branch boundaries

| Branch/repository       | Role                                                               |
| ----------------------- | ------------------------------------------------------------------ |
| `figaroh-plus/devel`    | Core integration branch; normal PR target                          |
| `figaroh-plus/main`     | Stable release branch; release and urgent hotfix PRs               |
| `figaroh-examples/main` | Separate examples integration branch; no `devel` branch is assumed |

Cross-repository work uses linked PRs and records the exact core/examples revisions validated together. Land a compatible core API before examples depend on it; document a minimum release or temporary commit dependency. Do not copy robot-specific code into core just to avoid a second PR.

### Merge and release gates

Required core checks are `Tests (core)`, `Tests (pinocchio-4.1)`, `Tests (mujoco-3.9)`, `Tests (mujoco-current)`, `Lint`, and `Docs`. `Lint` runs every pre-commit hook on the full tree (the legacy debt was cleared in #81-#85). These names describe workflow jobs; a maintainer must select them in GitHub branch protection/rulesets for server-enforced merge blocking.

Release sequence:

1. Confirm the selected roadmap exit criteria and linked validation evidence on `devel`. Run full tests, package build/metadata checks and the affected example workflows. Record unsupported or untested platforms explicitly.

1. Prepare the version and dated changelog on `devel` through a PR. Keep `pyproject.toml` and `src/figaroh/__init__.py` version strings aligned. In the same PR, revise `README.md` against the release: rewrite "What's new", rerun the Quickstart commands, check every claim (features, supported platforms, links) against the code and CI, and remove anything stale. Do the same for the figaroh-examples README (compatibility row, robot table, `requirements.txt` pin) in a paired examples PR. Also check that the maintainer's copyright notices, both the source file headers and `LICENSE`, end at the release year. In both repositories this command must print nothing:

   ```
   git grep -nE "Copyright (\[2021-[0-9]{4}\] Thanh Nguyen|[0-9-]+ Thanh D\. V\. Nguyen)" \
     | grep -v "$(date +%Y)"
   ```

   Third-party notices (CNRS/Toward SAS, PAL Robotics fixtures) keep their own years.

1. Open `devel` → `main` and merge with a **merge commit**, preserving shared history. Tag/release publishing is a separate maintainer action after the release checks; this repository's docs workflow does not publish to PyPI.

1. Merge `main` back into `devel` with a merge commit so it includes the release merge. This avoids repeated divergence at the next release.

For an urgent released-code bug, branch `fix/<issue>-<slug>` from `main`, target `main`, validate and make a patch release, then back-merge into `devel`. For normal fixes, use `devel` and the next planned release.

#### Release candidate procedure (1.0.0)

The 1.0.0 soak uses release candidates `v1.0.0rc1`, `v1.0.0rc2`, and so on.

> **Waived for 1.0.0.** On 2026-10-12 the maintainer released 1.0.0 directly through the release sequence above, without a candidate (#192). The evidence of step 3 was still recorded on the release pair. The procedure is kept for a future major release. Steps 1–4 of the release sequence above apply unchanged to each candidate; the steps below add only what is specific to the RC.

1. **Freeze `devel`.** Announce the freeze on the release tracker issue. From then on only fixes for soak blockers land on `devel`, each as a focused PR.

1. **Open the release PR into `main`.** Set the version to `1.0.0rc1` in `pyproject.toml` and `src/figaroh/__init__.py`, add the dated changelog section, and update the README as in release sequence step 2. Merge `devel` → `main` with a merge commit, then tag `v1.0.0rc1` on that merge commit.

1. **Collect evidence on one core/examples pair.** Record both exact commit SHAs in the release PR and tracker issue, then record command, environment, and pass/fail/skip counts for each item:

1. core `python -m pytest -q -rs`;

1. core hosted CI green on the tagged commit, and examples CI green on both Pinocchio profiles (3.7.0 and 4.1.0) against that core revision;

1. examples `MPLBACKEND=Agg python validate.py` against that core revision;

1. core `python -m build`, then `twine check dist/*`, then install of the wheel into a clean environment (not the editable `figaroh-dev` install) and an import check;

1. `python -m mkdocs build` from the repository root.

1. **Build once and publish back to back.** Build the sdist and wheel once from the `v1.0.0rc1` tag. The maintainer runs `twine upload` personally; upload the same files to PyPI and immediately attach them to the GitHub pre-release:

   ```
   gh release create v1.0.0rc1 --prerelease --target <full 40-character commit SHA> dist/*
   ```

   Use the full commit SHA for `--target`, not a branch or short SHA. Do not rebuild between the two uploads.

1. **Verify checksums.** Compare the sha256 of the PyPI files with the sha256 of the GitHub assets (`gh release download v1.0.0rc1`) and with the local `dist/` files. All three must match before the RC is announced.

1. **Pin examples to the RC.** During the RC, the figaroh-examples dependency pin (`requirements.txt`, README compatibility row) is `figaroh>=1.0.0rc1,<1.1`, merged to that repository's `main` through its own PR.

1. **Soak with fixes only.** Only soak-blocking fixes land on frozen `devel`. Each fix is released as the next candidate (`v1.0.0rc2`, …) and repeats step 3 in full and steps 4–5 for the new tag.

1. **End the soak** when both conditions hold: no new blocking issue has been opened for the period the maintainer sets for this RC (`<N days>`, recorded on the tracker issue), and the examples CI passes against the RC installed from PyPI.

1. **Release 1.0.0.** Repeat the release sequence for version `1.0.0` (steps 1–3, same build-once and checksum rules, without `--prerelease`) and tag `v1.0.0`. Tag the paired figaroh-examples revision `v1.0.0`. The existing `>=1.0.0rc1,<1.1` pin already admits 1.0.0, so no pin change is needed unless the maintainer decides otherwise. Finish with the back-merge in release sequence step 4.

### Definition of done

- Acceptance criteria are met with evidence, including regression coverage.
- Required checks pass on the final PR head; skips and limitations are explained.
- Changelog and affected docs agree with the implementation.
- Architecture decisions are recorded when applicable.
- PR is merged, linked issue is closed, and milestone status reflects its gate.

A locally prepared branch or a passing local suite alone is not a completed release or evidence of passing hosted CI.

# Design decisions

Contributor design records live in [docs/decisions/README.md](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/README.md). That index classifies historical analysis, implemented designs and proposals; older checklists in those records are not a live task tracker.

Use [Architecture](https://thanhndv212.github.io/figaroh-plus/concepts/architecture/index.md) for current implementation, [Roadmap](https://thanhndv212.github.io/figaroh-plus/further_reading/roadmap/index.md) for delivery priorities, and [Contributing](https://thanhndv212.github.io/figaroh-plus/further_reading/contributing/index.md) for the issue-to-release workflow. Proposals only become current architecture after implementation and validation.

# FAQ & Troubleshooting

## Installation

### `pip install figaroh` fails to build, or optimization scripts complain about missing solvers

Optimal configuration/trajectory generation uses `cyipopt` (a wrapper around the IPOPT solver), which needs system libraries that aren't pulled in by pip alone:

```
# Debian/Ubuntu
sudo apt-get install -y pkg-config coinor-libipopt-dev libblas-dev liblapack-dev
pip install cyipopt picos cvxpy
```

If you use conda, some robotics dependencies are easier to get from conda-forge:

```
conda install -c conda-forge pinocchio cyipopt
```

### Do I need MuJoCo, Genesis, or Isaac Sim installed?

No — Pinocchio is the only always-available backend and FIGAROH's core algorithms don't require the others. Install `mujoco`, `genesis-world`, or Isaac Sim only if you actually want that [backend](https://thanhndv212.github.io/figaroh-plus/concepts/backends/index.md); `figaroh.backends.list_backends()` reports what's currently available in your environment.

## Configuration

### What happened to the legacy flat format?

It was removed in 1.0; use the [unified format](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md) — it supports `extends:` template inheritance, so a robot config only states what's actually robot-specific. Loading an old flat config raises an error naming the converter, `python -m figaroh.utils.config_migration`. The examples in [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) all use the unified format.

### My robot config isn't picking up template defaults

Check that `extends:` is a path relative to the config file itself, not the working directory — e.g. `extends: "../../templates/manipulator_robot.yaml"` from `examples/my_robot/config/`. Also remember list-valued keys (like `markers`) are *replaced*, not merged, when both the template and the robot config define them — see [override rules](https://thanhndv212.github.io/figaroh-plus/concepts/configuration/index.md).

## Calibration & Identification

### Calibration/identification isn't converging

Check your initial parameter guesses and joint limits in the config first — most non-convergence traces back to a bad initial guess or an over-constrained problem, not the solver itself.

### My calibration fits the training postures well but predicts new postures badly

Usually too many parameters for what the postures excite: estimating weakly excited corrections freely fits noise. Check the number of measurements per estimated parameter, then try an estimation method that accounts for excitation (`excitation`, `map`, `map_cv`, `cv_subset`) and compare them on held-out postures. See [Choosing what to estimate](https://thanhndv212.github.io/figaroh-plus/tutorials/calibration_estimation_guide/index.md).

### The calibrated parameter set differs between my laptop and CI

With the default `structural` method, ties in the structural QR are broken by floating-point details, so the selected parameters can differ between platforms (figaroh-plus#113). The non-structural methods select on the measured postures in a fixed order; `map` and `map_cv` select nothing. See [Calibration estimation methods](https://thanhndv212.github.io/figaroh-plus/concepts/calibration_estimation/index.md).

### Identification results have high uncertainty or fail `verify()`

This is almost always a data-quality problem, not an algorithm problem:

- Was the trajectory actually "exciting"? See [Optimal Experiment Design](https://thanhndv212.github.io/figaroh-plus/tutorials/optimal_design/index.md) — an ill-conditioned trajectory will identify poorly no matter how good the solver is.
- Is the signal-to-noise ratio adequate? Try enabling `decimate=True` or tightening the Butterworth cutoff frequency in `signal_processing`.
- Do you have a genuinely held-out `validation_data_file` configured? A run with no validation data skips (not fails) the validation-based checks in `verify()` — see [thresholds](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md).

### Why did a metric get *skipped* instead of *failed* in my verification report?

A threshold whose metric can't be computed — typically because no `validation_data_file` is configured — is skipped rather than counted as a failure. An empty check list counts as passed since there's nothing to fail on. See [Reporting & Verification](https://thanhndv212.github.io/figaroh-plus/reporting_and_verification/index.md).

## Examples

### Where are the runnable robot examples?

In the separate [figaroh-examples](https://github.com/thanhndv212/figaroh-examples) repository, not in this one — see the [Examples Gallery](https://thanhndv212.github.io/figaroh-plus/examples/index.md). figaroh (this repo) is the library; figaroh-examples has the data, URDFs, and per-robot scripts.

### How do I add support for a new robot?

Scaffold it from the TIAGo example:

```
cd figaroh-examples/examples
./create_example.sh <robot_name>
```

Then implement a `BaseCalibration`/`BaseIdentification` subclass under `utils/`, and point its config at the matching [template](https://thanhndv212.github.io/figaroh-plus/examples/templates/index.md).

# Delivery plan review

The canonical proposal is maintained in `docs/plans/` and embedded below. It is a discussion draft; no new implementation or interface is accepted by its publication. The [roadmap](https://thanhndv212.github.io/figaroh-plus/further_reading/roadmap/index.md) owns priorities and [contributing](https://thanhndv212.github.io/figaroh-plus/further_reading/contributing/index.md) describes the current workflow.

## Identification and calibration delivery plan

**Status: Tracker setup approved; execution details under review — revision 4, 2026-10-02. Implementation status refreshed 2026-10-12 (after the M1 acceptance review)** (see [implementation status](#implementation-status)). Confirmed user preferences: advance calibration alongside dynamic identification; TIAGo mocap is the first calibration reference, with TALOS contact as the second consumer/regression target. The user authorized GitHub milestone/issue creation on 2026-10-02 and requested one-by-one delivery. Tracker setup does not accept unresolved public interfaces, change solver defaults, or approve automatic implementation/merges. Review issue readiness before selecting implementation work. Existing approved work and open PRs retain their own review gates.

### Purpose and scope

Deliver two trustworthy workflows within the existing package structure:

1. Measurements → dynamic identification → physical model → held-out effort verification → inertial URDF export/reload → archived report.
1. Measurements → geometric calibration → identifiable corrections → held-out pose/contact verification → geometric export/reload → archived report.

The workstreams proceed in parallel through focused PRs, not simultaneous edits to shared workflow classes. Their shared foundations are provenance, units/frames, explicit result stages, validation and reproducible artifacts. Dynamic and geometric solvers remain mathematically distinct.

The [roadmap](https://thanhndv212.github.io/figaroh-plus/further_reading/roadmap/index.md) owns priority and milestone outcomes. This document owns the proposed work breakdown, dependencies, alternatives and review sequence. Once approved, GitHub issues own changing task status and detailed acceptance criteria. [Architecture](https://thanhndv212.github.io/figaroh-plus/concepts/architecture/index.md) continues to describe existing code, not these proposed contracts. Existing design records remain authoritative for their scope; this plan does not supersede the modular-terms, Pinocchio-support or log-Cholesky decisions.

### Evidence motivating the plan

| Observation at 2026-10-02                                                          | Consequence                                                                                 |
| ---------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------- |
| Core #32 reproduces missing final-joint acceleration                               | Signal correctness precedes new dataset-based numerical claims                              |
| UR10/TIAGo sampling/filter assumptions disagree                                    | Add explicit timing and derivative provenance before refitting                              |
| Exact base-preserving SDP returns no accepted model on the three robot datasets    | Diagnose constraints/scaling; distinguish reconstruction from direct constrained estimation |
| All dataset log-Cholesky fits exhaust 200 evaluations                              | Keep #23–#25 conditional; investigate #30 rather than promote finite candidates             |
| Per-link projection succeeds on UR10/TX40 but changes the base fit                 | Report it as a separate method and select exported stages explicitly                        |
| Inertial URDF first-moment/tensor handlers are stubs                               | A file-write success cannot close physical-model delivery                                   |
| Examples CI fails on missing Hey5 geometry and TALOS 3.7 held-out regression       | Make assets reproducible and retain numerical regression gates                              |
| Current examples have robot-specific loaders and existing reporting/export helpers | Improve adapters and reuse primitives before proposing a new framework                      |

Sources: core issues #20/#22–#25/#30/#32, core PR #31, examples issues

## 11/#12/#14 and PR #13 (merged 2026-10-08). The examples dynamic report and supplemental

analysis are still local additions on the PR branch. Reports include failed solver statuses and preprocessing limitations; they are not scientific go approval. Current issue/PR status must be checked when work starts.

### Ownership within the current repositories

| Concern                                     | Existing owner                                                                                            | Proposed change boundary                                                                     |
| ------------------------------------------- | --------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------- |
| Dynamics preprocessing and regression       | `figaroh/identification/{identification_tools,base_identification,parameter}.py` and `tools/regressor.py` | Fix numerical contracts and expose diagnostics through existing workflow                     |
| Physical checks, projection, reconstruction | `identification/{physical_consistency,reconstruction}.py`                                                 | Keep distinct operations; add a research constrained-fit comparator before production design |
| Geometric preprocessing/solving             | `calibration/{base_calibration,calibration_tools,data_loader,config}.py`                                  | Fix proven problems; retain frame conventions and existing subclasses                        |
| Export and export verification              | `tools/{urdf_exporter,export_validation,geometric_calibration_export}.py`                                 | Reuse handlers/checks; complete missing inertial semantics                                   |
| Reports and archives                        | `tools/{identification_report,report,compare_report,run_archive}.py`, `utils/results_manager.py`          | Reuse output machinery; add explicit stage/provenance fields through an approved contract    |
| Robot I/O and conversions                   | `figaroh-examples/examples/<robot>/utils/`                                                                | CSV adapters, current-to-effort mapping and robot-specific assumptions stay here             |
| Robot models/config/data                    | `examples/<robot>/{urdf,config,data}` and shared `models/`                                                | Preserve originals; document sources, frames, sample rates and redistribution constraints    |
| Research comparisons                        | Core private `docs/development/spikes/`; examples `benchmarks/`                                           | Explicit source/commit dependency; never present as supported package API                    |
| Reference user workflows                    | Existing robot entry points under `examples/<robot>/`                                                     | Integrate validated primitives after research acceptance; avoid a competing CLI              |

No `TrajectoryData` class was found in the current source audit. A typed data contract is a proposal, not an existing API. First inventory existing trajectory dictionaries and calibration measurement structures. Extend those or introduce one narrow type only after an ADR demonstrates two consumers, conversion rules and backward-compatible adapters. Keep dynamic trajectory samples and geometric pose/contact observations as separate payloads.

### Dependency structure

```
flowchart TD
    R["Scope and contract review"]
    W1["<b>W1: Fixtures</b> · #33 ✅<br/>✓ ex#15 ex#16 ex#43 ex#59"]
    W2["<b>W2: TALOS regression</b> · #34 ✅<br/>✓ ex#14 ex#48"]
    W3["<b>W3: Data/result contracts</b> · #35 ✅<br/>✓ #54 #55 #125 #131<br/>✓ ex#17"]
    W4["<b>W4: Engineering process</b> · #50 ✅<br/>✓ #56 #57 #58 #73 #81–#85<br/>✓ ex#18 ex#40 ex#45 ex#50<br/>✓ ex#56 ex#60 ex#76"]
    D1["<b>D1: Acceleration correctness</b> · #36 ✅<br/>✓ #32"]
    D2["<b>D2: Signal audit</b> · #37 ✅<br/>✓ ex#19 ex#20 ex#51"]
    D3["<b>D3: UR10 truth fixture</b> · #38 ✅<br/>✓ #142 #143<br/>✓ ex#21"]
    D4["<b>D4: Physical comparison</b> · #39 ✅<br/>✓ #59 ex#12 ex#22"]
    D5["<b>D5: Convergence research</b> · #40 ✅<br/>✓ #22 (revise) #30 (no-go)"]
    D6["<b>D6: Inertial export</b> · #41 ✅<br/>✓ #60 (PR #138)"]
    D7["<b>D7: Dynamic reference</b> · #42 ✅<br/>#61✓ #116✓<br/>ex#23✓ ex#68✓ ex#69✓"]
    C1["<b>C1: Frames/data audit</b> · #43 ✅<br/>✓ ex#24 ex#25"]
    C2["<b>C2: Calibration truth/holdout</b> · #44 ✅<br/>✓ #101 #102 #105 #110 #113<br/>✓ ex#26 ex#27 ex#67"]
    C3["<b>C3: Geometric export</b> · #45 ✅<br/>✓ #62 #111 #114 #123<br/>✓ ex#28"]
    C4["<b>C4: Calibration reference</b> · #46 ✅<br/>✓ #97 #98 #99 #119 #120<br/>✓ ex#29"]
    C5["<b>C5: Calibration studies</b> · #167<br/>ex#96–#101 ex#70 ex#71"]
    D8["<b>D8: Identification studies</b> (research)<br/>#157–#161"]
    S1["<b>S1: Minimum reporting contract</b> · #47 ✅<br/>✓ #63 #70 #100 #103<br/>✓ ex#30 ex#36"]
    U1["<b>U1: Onboarding acceptance</b> · #51 ✅<br/>✓ #66<br/>✓ ex#33"]
    A["M1 acceptance review ✅"]
    S2["<b>S2: Sensitivity/uncertainty</b> · #48<br/>✓ #107<br/>open: #64 ex#31"]
    S3["<b>S3: Improved experiment design</b> · #49<br/>#65 #90<br/>ex#32"]
    S4["<b>S4: Selective composition</b> · #52<br/>#67"]
    B1["<b>B1: Backend parity</b> · #53<br/>#68<br/>ex#34"]
    LC1["<b>LC1: Log-Cholesky production</b> · #20<br/>#155 (go gate)<br/>#23 #24 #25<br/>ex#11"]

    R --> W1
    R --> W2
    R --> W3
    R --> W4
    W1 --> D2
    D1 --> D3
    D2 --> D3
    D3 --> D4
    D4 --> D5
    D6 --> D7
    D4 --> D7
    D5 -. optional nonlinear path .-> D7
    D5 --> LC1
    W1 --> C1
    W2 --> C1
    C1 --> C2
    C2 --> C3
    C3 --> C4
    C4 --> C5
    D4 --> D8
    C5 -. more worked studies .-> U1
    W3 --> S1
    S1 --> D7
    S1 --> C4
    D7 --> U1
    C4 --> U1
    D7 --> A
    C4 --> A
    U1 --> A
    A --> S2
    S2 --> S3
    A --> S4
    W3 --> S4
    A --> B1

    classDef cleared fill:#c8e6c9,stroke:#2e7d32,stroke-width:2px,color:#1b5e20
    classDef ready fill:#fff3cd,stroke:#b8860b,color:#5c4400
    class R,W1,W2,W3,W4,D1,D2,C1,C2,C3,S1,D6,C4,D3,D4,D5,D7,U1,A cleared
    class LC1 active
    classDef active fill:#fff3cd,stroke:#b8860b,stroke-width:3px,stroke-dasharray:6 3,color:#5c4400
```

Legend: **green ✅ = tracker cleared** (closure record confirmed); **amber = ready**, all prerequisites cleared (dashed amber border = work in progress or in review); uncoloured = blocked on an upstream package. Each box lists its tracker number, then the issues in its milestone (core first, then `ex#` = figaroh-examples). Issues marked ✓ are closed; unmarked issues are open. Two open examples issues have no package yet: ex#70 (TIAGo suspension real-data fit) and ex#71 (TIAGo backlash surface); both are research examples outside M1.

#### Implementation status

Status as of 2026-10-12, taken from the GitHub trackers. The trackers stay authoritative; refresh this table when a tracker closes.

| Package                          | Status                 | Closed     | Delivered by / remaining                                                                                                                                                                                                                                                                                                                                                         |
| -------------------------------- | ---------------------- | ---------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| **W1** fixtures                  | ✅ **Cleared**         | 2026-10-03 | examples #44, #57, #61, #62: hosted examples CI on both Pinocchio profiles; every robot loads from a clean checkout                                                                                                                                                                                                                                                              |
| **W2** TALOS regression          | ✅ **Cleared**         | 2026-10-03 | examples #46, #54: examples #14 explained; deterministic TALOS fixtures and noise-floor held-out checks                                                                                                                                                                                                                                                                          |
| **W4** engineering process       | ✅ **Cleared**         | 2026-10-03 | core #71, #74, #80, #86–#93; examples #37, #40, #47: templates, real-model regressor tests, lint debt removed, full pre-commit blocking                                                                                                                                                                                                                                          |
| **D1** acceleration              | ✅ **Cleared**         | 2026-10-02 | core #69 (fixes #32)                                                                                                                                                                                                                                                                                                                                                             |
| **D2** signal audit              | ✅ **Cleared**         | 2026-10-03 | examples #19/#20 UR10 and TIAGo audits, examples #35                                                                                                                                                                                                                                                                                                                             |
| **C1** frames/data audit         | ✅ **Cleared**         | 2026-10-03 | examples #24/#25 TIAGo and TALOS audits; its follow-ups #97–#99 were moved to C4 and closed there                                                                                                                                                                                                                                                                                |
| **C2** calibration truth/holdout | ✅ **Cleared**         | 2026-10-05 | accepted on core `a991bf3` + examples `c791ecd`; deferred items moved to #119/#120 (C4)                                                                                                                                                                                                                                                                                          |
| **C3** geometric export          | ✅ **Cleared**         | 2026-10-06 | accepted on core `efd9f69` + examples `2c75f50`                                                                                                                                                                                                                                                                                                                                  |
| **W3** data/result contracts     | ✅ **Cleared**         | 2026-10-06 | core #126 (ADR, #54), #128/#130 (contract, #55), #132 (#131), #127 (#125); examples #84 (#17); accepted on core `4ec06d4` + examples `9a9e6d7`                                                                                                                                                                                                                                   |
| **D3** UR10 truth fixture        | ✅ **Cleared**         | 2026-10-07 | examples #21 by examples #89/#91 (fixture, frozen protocol v1, UR10 example moved onto it); #142 by #144; #143 by #145 + examples #92 (both added 2026-10-07); accepted on core `2260786` + examples `b23b836` (core 846 passed; examples `validate.py` 16/16, pytest 255 passed)                                                                                                |
| **D5** convergence research      | ✅ **Cleared**         | 2026-10-10 | #22 by PR #31 (revise); #30 by PR #154 (no-go: 8/24 gated fits exhaust 2000 evaluations). The method change #155 moved to LC1 as its go gate                                                                                                                                                                                                                                     |
| **D6** inertial export           | ✅ **Cleared**         | 2026-10-06 | #60 by PR #138; accepted on core `0e6a78c` + examples `a3381bc` (core 817 passed; examples `validate.py` 16/16, pytest 237 passed); merged-body export left to D7 (#61)                                                                                                                                                                                                          |
| **S1** reporting contract        | ✅ **Cleared**         | 2026-10-06 | core #63 (PR #134), #70, #100, #103 (PR #135); examples #30 (examples PR #85), #36                                                                                                                                                                                                                                                                                               |
| **C4** calibration reference     | ✅ **Cleared**         | 2026-10-06 | examples #29 by examples #88 (`reference_run.py`); #119 by #141 (core support; TIAGo protocol keeps one marker); #120 by #140 + examples #87 (`map` replaces the coefficient); #97–#99 by #127, #137, #139 + examples #86; accepted on core `c80b5fc` + examples `88e2fc2`. Held-out marker RMSE 3.8–4.2 mm; export parity ≤ 2.1e-12 m                                           |
| S2 uncertainty                   | In progress (M2)       | —          | core #107 calibration standard errors fixed; #64 open                                                                                                                                                                                                                                                                                                                            |
| **U1** onboarding                | ✅ **Cleared**         | 2026-10-11 | core #66 (guideline); examples #33 by examples PR #123: worked briefs for the UR10 truth and TIAGo identification references, TIAGo mocap calibration and a TX40 no-validation-data case, rerun against the accepted D7/C4 evidence on core `779e830` + examples `b167298` (examples `validate.py` 18/18)                                                                        |
| **D4** physical comparison       | ✅ **Cleared**         | 2026-10-09 | #59 by PR #153 (2026-10-08); examples #12 by examples PR #13; examples #22 by examples PRs #94 and #95                                                                                                                                                                                                                                                                           |
| **D7** dynamic reference         | ✅ **Cleared**         | 2026-10-10 | #61 by PRs #165, #172; #116 by #169 + examples #117; examples #69, #68 by examples #118, #23 by examples #119 (`identification_reference.py`). Accepted on core `658fe99` + examples `663d642` (examples `validate.py` 18/18). UR10 truth and TIAGo select `physical_fit`; export parity ≤ 6.1e-10. Follow-ups #163 and #164 fixed by PR #174 (`projected` stays non-selectable) |
| LC1 log-Cholesky production      | In progress (optional) | —          | Go gate #155 (method change) in progress: exploration done on spent seeds, method choice pending, protocol not frozen. #23–#25 and examples #11 wait on its go decision                                                                                                                                                                                                          |
| S3, B1                           | Blocked                | —          | S3 and B1 wait on S2 and M1 acceptance                                                                                                                                                                                                                                                                                                                                           |
| S4 composition (M2)              | Blocked                | —          | W3 cleared; still waits on accepted M1 references                                                                                                                                                                                                                                                                                                                                |

**Gate summary:** M0's delivery roster (W1, W2, W4) and M1.1 (D1, D2, C1) are cleared. **The calibration workstream is complete through M1.4** (C2, C3, C4). **The dynamic workstream is complete through M1.4** (D3, D4, D6, D7), released in 0.7.0. U1 cleared on 2026-10-11.

**M1 accepted (2026-10-12; review tracker [#181](https://github.com/thanhndv212/figaroh-plus/issues/181)).** Evidence pair: core `779e830` + examples `b167298`. The maintainer ratified the closure records (R-1: W1, W2, W4, D1, D2, C1, D4, S1, D7) and accepted the #97 duplicate closure as completed via #125/#127 (C-1). Scope accepted: S-1 real-data references at execution scope (held-out error reported, not gated); S-2 TIAGo effort scale and arm_1 constant still open (examples #126); S-3 lumped TIAGo export; S-4 PAL not run on hardware; S-5 TALOS contact as a regression consumer. G-1 (a failed calibration fit could be exported) is fixed before the release candidate: core #182 by PR #187 (`9dba6da` on `devel`) and examples #124 by PR

## 127 (`20b6377` on `main`).

**M0 gate review accepted (2026-10-12, #181 §9).** Clauses: existing core gates retained (blocking `Lint`, Core CI and Documentation green); examples assets and dependencies reproducible (W1); numerical regressions explained (W2); review and merge rules applied (W4); exact tested pairs cited as core `fe7dc51` + examples `b8d9c75` (hosted runs 37134266859 / 37136143367) and core tag `v0.5.0` (`02f705a`). The pair is not clean for the M0 cycle: examples has no `v0.5.0` tag, because pairing started at 0.6.0.

Housekeeping (2026-10-12): empty milestones (core C4 and U1, examples U1 and the W1, D2, C1 and W4 ones) are closed and the stale D1 counter was refreshed (#183, H-2). The stale `status:blocked` labels on core #61 and tracker #42, and on examples #23, were resolved when D7 cleared.

Log-Cholesky #30 can use the independent analytic core fixture. D1 is fixed (#69, merged 2026-10-02) and the D2 audits are complete, so real-dataset work can start with D3. Inertial export can be tested with known feasible parameters independently of solver success. Calibration need not wait for dynamic optimization, but changes to shared export/report code require paired regression coverage and serial integration.

#### Path forward

Status as of 2026-10-12. The trackers stay authoritative.

**Critical path (M1 acceptance).** D7 cleared on 2026-10-10 (steps 1–6 below, kept as its record) and U1 on 2026-10-11 (step 7); the M1 acceptance review (step 8) was completed on 2026-10-12. Steps 9–12 are what remains before 1.0.0.

| Step | Issue                                                                                                                                              | State                                | What it needs                                                                                                                                                                           |
| ---- | -------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| 1    | examples #69 — ship the `calibration_slow` and `calibration_weight` runs, default `validation_data_file`, payload scale check                      | **done** (PR #103)                   | The original 2021-07 bags and the exporter that reproduces the shipped run byte for byte. Do this first: #68's acceptance bar is held-out RMSE on `calibration_slow` no worse than 1.21 |
| 2    | examples #68 — velocity filter model, torso/wrist/arm_1 effort handling, Hey5 URDF, constants in config                                            | **done** (examples PR #118)          | The same bags. Seven checklist items, each ships its derived data and analysis in the repository                                                                                        |
| 3    | core #116 — deterministic base-parameter selection                                                                                                 | **done** (PR #169)                   | Independent of 1–2; can run in parallel. Lets the golden tests drop the basis-dependent masks added in PR #165                                                                          |
| 4    | examples #23 — headless reference command (UR10 truth protocol with `physical_fit` and export; TIAGo rejected-stage and `arm_7` merged-body cases) | **done** (examples PR #119)          | Needs #61's library side (merged). Acceptance is V3: full `validate.py`                                                                                                                 |
| 5    | core #61 — close with the V2 run linked                                                                                                            | **done** (PRs #165, #172)            | Library side done in PR #165; its acceptance criteria are met, its validation level (V2) waits on step 4                                                                                |
| 6    | D7 tracker #42 — closing review                                                                                                                    | **done** (2026-10-10)                | Closes last, after steps 1–5 and the maintainer accepts the integration evidence                                                                                                        |
| 7    | U1 acceptance (examples #33)                                                                                                                       | **done** (examples PR #123)          | Worked briefs checked against the accepted evidence                                                                                                                                     |
| 8    | M1 acceptance review ([#181](https://github.com/thanhndv212/figaroh-plus/issues/181))                                                              | **done** (2026-10-12)                | Accepted on core `779e830` + examples `b167298`                                                                                                                                         |
| 9    | Calibration export refusal (G-1): core #182, examples #124                                                                                         | **done** (PR #187, examples PR #127) | A failed fit is no longer exported silently                                                                                                                                             |
| 10   | Public API and stability: core #184                                                                                                                | open                                 | Declare the 1.0 public API                                                                                                                                                              |
| 11   | Deprecation removals: core #185 + examples #125                                                                                                    | open                                 | Remove deprecated features batched into 1.0                                                                                                                                             |
| 12   | RC procedure: core #186                                                                                                                            | open                                 | Release candidate, soak, then `1.0.0`                                                                                                                                                   |

**After M1 acceptance (M2/M3):** S2 sensitivity (core #64, examples #31), then S3 experiment design (core #65, #90, examples #32), S4 composition (core #67) and B1 backend parity (core #68, examples #34).

**Found along the way (unscheduled, `delivery` label, no package).** These did not block D7. Both were fixed by PR #174 (merged 2026-10-11) and released in 0.7.0.

| Issue     | Defect                                                                                                | Fix                                                                                                                                              |
| --------- | ----------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------ |
| core #163 | The physical-consistency projection projected the nominal model, not the fit                          | ✅ Projects the reconstruction; without `reconstruction` it is `skipped` and the `physical` stage is `not_run`. `projected` stays non-selectable |
| core #164 | The SDP reconstruction and LMI projection passed `max_seconds` to picos, which only knows `timelimit` | ✅ Passed as `timelimit` in both                                                                                                                 |

**Identification study expansion (package D8, research, `status:planned`, outside baseline M1).** D8 groups core #157–#161 and examples #115 under milestone D8 (no tracker yet). Each extends the D4 physical comparison and none changes the baseline:

| Issue             | Study                                                                                                                                                   |
| ----------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- |
| core #155         | Change the log-Cholesky fitting method for reliable termination (belongs to LC1 as its go gate, not D8; `delivery`)                                     |
| core #157         | Geometric (log-det) regularization for the physical direct fit                                                                                          |
| core #158         | Geometry-derived physical constraints (mass, CoM, bounding ellipsoid)                                                                                   |
| core #159         | Manifold-optimization alternative to log-Cholesky                                                                                                       |
| core #160         | Noise-bias estimators (IDIM-IV, DIDIM, TLS) in the physical comparison                                                                                  |
| core #161         | Bayesian physically consistent identification                                                                                                           |
| examples #115     | TX40: trace the identification CSVs to the original recording and ship the author's 2021 reference identification (data for #160 and the D4 comparison) |
| examples #70, #71 | TIAGo suspension and backlash research examples; now Phase 3 of C5 (below)                                                                              |

**Calibration study expansion (package C5, tracker core #167, `status:planned`).** The [calibration studies plan](https://github.com/thanhndv212/figaroh-plus/blob/devel/docs/plans/calibration-studies-plan.md) (proposed 2026-10-09) brings further studies to the TIAGo motion-capture standard. Phase 0 first, because every study uses its tools:

| Phase | Issue             | Study                                                                                          |
| ----- | ----------------- | ---------------------------------------------------------------------------------------------- |
| 0     | examples #96      | Shared simulation and held-out harness (confirms the identifiable-set fixes #99 and #113 hold) |
| 1     | examples #97      | TIAGo Pro motion capture                                                                       |
| 1     | examples #98      | TALOS table contact                                                                            |
| 1     | examples #99      | UR10 hand-eye (k-fold within one session)                                                      |
| 2     | examples #100     | TIAGo motion capture across hardware (audit first)                                             |
| 2     | examples #101     | TIAGo head camera, chessboard on hand (audit first)                                            |
| 3     | examples #71, #70 | TIAGo backlash surface and suspension base (existing issues, to be attached to C5)             |

C5 is separate from M1: no study depends on D7, and the TIAGo motion-capture reference numbers are its regression check. Phase 2 needs the original 2023 recordings from the maintainer.

### Work-package milestones and PR boundaries

#### Work-package milestone rules

Each work package is a **delivery milestone** beneath a roadmap outcome M0–M3. M1.1–M1.4 are acceptance gates, not additional issue containers. Package IDs are stable package identifiers, distinct from GitHub issue numbers. GitHub milestones use these IDs; existing issues are reused and missing tasks have explicit owning packages. The package table below defines their roadmap mapping.

A package can be marked **Cleared** only when:

1. Its reviewed issue roster is complete, including linked core and examples issues. Assign each issue one owning package; link dependencies elsewhere.
1. **Every included issue is resolved with its acceptance criteria met**, its required PRs reviewed/merged, and evidence linked. Closing an issue as a duplicate, deferred or “not planned” does not satisfy that obligation.
1. Package-level integration/acceptance evidence also passes on the declared revision pair. All issue closures alone are insufficient if the combined workflow still fails its gate.
1. A maintainer reviews the closure record and confirms the accepted outcome.

Before Ready, record owner, roadmap gate, issue roster, dependencies, acceptance criteria and evidence location. At closure, retain issue/PR links, tested revisions, commands, report and any limitations. Status is Draft → Ready → In progress → In review → Cleared; blocked work retains its unmet dependencies. An empty draft roster cannot be treated as a completed milestone.

Deferral requires an explicit reviewed scope change: preserve the original issue and reason, move it to a named future package, and update the roadmap mapping if its exit criteria change. Do not silently remove unresolved issues to clear a package. Cross-repository milestones need one canonical roster (parent issue/project record) linking both repositories, since their individual GitHub milestone views do not provide a combined closure record.

D5 is a separate research milestone, cleared on 2026-10-10 with #22 (revise) and

## 30 (no-go); it was never required for the supported baseline M1 scope. The

method-change revision #155 moved to LC1 as its go gate. Promoting the nonlinear path adds #155 and #23–#25 to that path's reviewed roster; none are silently treated as completed by D7. Basic acquisition/design guidance is part of U1; S3 is the later algorithmic improvement milestone.

| ID / canonical tracker                                      | Roadmap outcome / gate              | Outcome / justification                               | Repository and dependencies                                                | Acceptance evidence                                                                                                                                                                           |
| ----------------------------------------------------------- | ----------------------------------- | ----------------------------------------------------- | -------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| [W1](https://github.com/thanhndv212/figaroh-plus/issues/33) | M0; enables M1.1                    | Reproducible examples environment and fixtures        | Examples; existing PR #13 plus focused asset follow-up                     | Clean checkout resolves required geometry; pinned sources/licenses; explicit dependency failures; no reliance on a developer ROS search path                                                  |
| [W2](https://github.com/thanhndv212/figaroh-plus/issues/34) | M0; enables calibration M1.1        | Explain TALOS Linux/3.7 regression                    | Examples #14, core companion only if cause is generic                      | Capture solver status, seeds, native versions and both chains; reproduce before/after; retain current improvement gate until independently justified                                          |
| [W3](https://github.com/thanhndv212/figaroh-plus/issues/35) | M1 shared contract; M2 prerequisite | Working data/result contracts                         | Proposed core ADR after inventory of both workflows                        | Joint/order/frame/unit/timestamp provenance, observed vs reconstructed effort, masks, split IDs and optional fields; documented adapters preserve old users                                   |
| [D1](https://github.com/thanhndv212/figaroh-plus/issues/36) | M1.1 dynamic                        | Correct every tangent acceleration coordinate         | Core #32                                                                   | Analytic final-joint regression, constant/variable dt, invalid timestamps and documented `nq != nv` policy; affected examples rechecked                                                       |
| [D2](https://github.com/thanhndv212/figaroh-plus/issues/37) | M1.1 dynamic                        | Audit robot signal processing                         | Separate examples issues for UR10 and TIAGo                                | Recorded vs inferred timestamps; explicit filter rates/cutoffs/order; torque/current units/signs; trim/decimation index provenance; immutable raw files                                       |
| [D3](https://github.com/thanhndv212/figaroh-plus/issues/38) | M1.2 dynamic                        | Fresh UR10 truth fixture and benchmark protocol       | Examples; D1/D2                                                            | Save verified true inertias, analytic q/dq/ddq, independent train/validation trajectories and noise seeds; no ground-truth parameter claims from old CSVs                                     |
| [D4](https://github.com/thanhndv212/figaroh-plus/issues/39) | M1.2 dynamic                        | Fair physical-estimation comparator                   | Private core spike + linked examples experiment; D3                        | Base OLS, exact reconstruction, direct LMI-constrained effort fit, per-link projection and log-Cholesky; common inputs; comparable objectives and extras policy; separate failures            |
| [D5](https://github.com/thanhndv212/figaroh-plus/issues/40) | M1.2 optional nonlinear research    | Convergence/scaling revision                          | Core #22 (revise), #30 (no-go); cleared 2026-10-10, #155 moved to LC1      | Objective/gradient histories, scaling/bounds/prior ablations, multiple starts and justified budget; preserve earlier protocol; the #30 go gates, carried unchanged into #155, govern go       |
| [D6](https://github.com/thanhndv212/figaroh-plus/issues/41) | M1.3 dynamic                        | Complete inertial export                              | Core; prior mapping review and known-parameter fixture                     | Mass/first moments/CoM, origin vs CoM tensor and inertial rotation handled; export/reload matches intended RNEA and physical verdict; unsupported targets fail explicitly                     |
| [D7](https://github.com/thanhndv212/figaroh-plus/issues/42) | M1.4 dynamic                        | Accepted dynamic reference workflow                   | Core integration + examples; D4/D6/S1; D5 only for optional nonlinear path | Explicit selected stage, fit and genuine validation splits, per-joint units, physical/solver verdict, exported model and archive; successful supported baseline can ship without nonlinear go |
| [C1](https://github.com/thanhndv212/figaroh-plus/issues/43) | M1.1 calibration                    | Audit geometric data/frames and reproduction          | Examples with focused core fixes; W1/W2                                    | Named observation frames, translation/rotation units, timestamps, measurement source and split policy; TIAGo and TALOS current behavior captured                                              |
| [C2](https://github.com/thanhndv212/figaroh-plus/issues/44) | M1.2 calibration                    | Calibration synthetic truth + real held-out reference | Examples; C1                                                               | Known synthetic corrections recovered in identifiable coordinates; held-out pose/contact errors on unused postures; diagnose gauge, priors and solver failures                                |
| [C3](https://github.com/thanhndv212/figaroh-plus/issues/45) | M1.3 calibration                    | Validate redistribution and geometric export          | Existing core QR/export helpers + examples; C2                             | Calibrated-model FK matches exported/reloaded model; parameter/frame semantics retained; PAL runtime YAML checked where relevant; metrology frames are not silently embedded                  |
| [C4](https://github.com/thanhndv212/figaroh-plus/issues/46) | M1.4 calibration                    | One accepted calibration reference workflow           | Examples TIAGo mocap selected; C2/C3/S1                                    | One command, training/held-out report, identifiable vs redistributed corrections, export and archive; contact-only evidence labeled separately from full pose metrology                       |
| [S1](https://github.com/thanhndv212/figaroh-plus/issues/47) | M1.4 shared reporting prerequisite  | Minimum reporting contract                            | Existing core report/archive modules; W3; enables D7/C4                    | Separate solver/data/fit/physical/export verdicts, explicit failed/fallback stages, paired revisions/hashes, old result compatibility                                                         |
| [S2](https://github.com/thanhndv212/figaroh-plus/issues/48) | M2 follow-up diagnostics            | Sensitivity and uncertainty                           | Core diagnostics + examples; stable D7/C4                                  | Noise/initialization trials, singular spectrum and parameter correlations; observable combinations reported; no unjustified full-parameter certainty                                          |
| [S3](https://github.com/thanhndv212/figaroh-plus/issues/49) | M2 follow-up experiment design      | Improved experiment design                            | Existing `optimal/` + examples; S2                                         | Compare excitation/posture selection with a frozen baseline under declared constraints; improve independent validation evidence rather than only matrix condition                             |
| [W4](https://github.com/thanhndv212/figaroh-plus/issues/50) | M0                                  | Engineering process and regression coverage           | Both repositories; existing validation/contribution rules                  | Reviewed workflow changes, focused legacy lint/mock coverage issues, reproducible checks; claimed CI automation verified on hosted runs                                                       |
| [U1](https://github.com/thanhndv212/figaroh-plus/issues/51) | M1.4 shared onboarding              | General robot/dataset guide and worked briefs         | Core docs + examples; draft now, acceptance uses D7/C4                     | Fresh dynamic and TIAGo mocap walkthroughs plus missing-data case; supported commands, methods and limitations verified                                                                       |
| [S4](https://github.com/thanhndv212/figaroh-plus/issues/52) | M2 composition                      | Selective reusable regressor/residual composition     | Core + two examples consumers; W3 and accepted M1 references               | Narrow ADR, two consumers, legacy numerical parity and additive compatibility; no broad refactor prerequisite                                                                                 |
| [B1](https://github.com/thanhndv212/figaroh-plus/issues/53) | M3                                  | Backend capability/parity review                      | Core `backends/`/`integration/`; accepted reference workflow               | Actual backend selection or explicit rejection, supported operations matrix, same model/state comparisons and export/validation parity                                                        |

Potential new work must not duplicate #20/#22–#25/#30/#32 or examples

## 11/#12/#14. If one package contains independent defects, split them into

individual issues/PRs rather than closing a broad issue with partial evidence.

### GitHub delivery register

Milestones and their canonical rosters were created on 2026-10-02. Open each tracker for its complete issue roster, dependency links and acceptance evidence. Core trackers combine both repositories; GitHub milestone percentages are local to a repository and do not supersede the closure review. No due dates or automatic merging are configured by this setup.

| Package                                                      | Core milestone                                                                       | Examples milestone                                                             |
| ------------------------------------------------------------ | ------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------ |
| [W1](https://github.com/thanhndv212/figaroh-plus/issues/33)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/1)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/1)  |
| [W2](https://github.com/thanhndv212/figaroh-plus/issues/34)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/2)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/2)  |
| [W3](https://github.com/thanhndv212/figaroh-plus/issues/35)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/3)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/3)  |
| [D1](https://github.com/thanhndv212/figaroh-plus/issues/36)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/4)            | No examples-owned issues in this roster                                        |
| [D2](https://github.com/thanhndv212/figaroh-plus/issues/37)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/5)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/4)  |
| [D3](https://github.com/thanhndv212/figaroh-plus/issues/38)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/6)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/5)  |
| [D4](https://github.com/thanhndv212/figaroh-plus/issues/39)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/7)            | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/6)  |
| [D5](https://github.com/thanhndv212/figaroh-plus/issues/40)  | [Closed milestone](https://github.com/thanhndv212/figaroh-plus/milestone/8?closed=1) | No examples-owned issues in this roster                                        |
| [D6](https://github.com/thanhndv212/figaroh-plus/issues/41)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/9)            | No examples-owned issues in this roster                                        |
| [D7](https://github.com/thanhndv212/figaroh-plus/issues/42)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/10)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/7)  |
| [C1](https://github.com/thanhndv212/figaroh-plus/issues/43)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/11)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/8)  |
| [C2](https://github.com/thanhndv212/figaroh-plus/issues/44)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/12)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/9)  |
| [C3](https://github.com/thanhndv212/figaroh-plus/issues/45)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/13)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/10) |
| [C4](https://github.com/thanhndv212/figaroh-plus/issues/46)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/14)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/11) |
| [S1](https://github.com/thanhndv212/figaroh-plus/issues/47)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/15)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/12) |
| [S2](https://github.com/thanhndv212/figaroh-plus/issues/48)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/16)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/13) |
| [S3](https://github.com/thanhndv212/figaroh-plus/issues/49)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/17)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/14) |
| [W4](https://github.com/thanhndv212/figaroh-plus/issues/50)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/18)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/15) |
| [U1](https://github.com/thanhndv212/figaroh-plus/issues/51)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/19)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/16) |
| [S4](https://github.com/thanhndv212/figaroh-plus/issues/52)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/20)           | No examples-owned issues in this roster                                        |
| [B1](https://github.com/thanhndv212/figaroh-plus/issues/53)  | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/21)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/17) |
| [C5](https://github.com/thanhndv212/figaroh-plus/issues/167) | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/23)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/19) |
| D8 (no tracker yet)                                          | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/24)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/20) |
| [LC1](https://github.com/thanhndv212/figaroh-plus/issues/20) | [Open milestone](https://github.com/thanhndv212/figaroh-plus/milestone/22)           | [Open milestone](https://github.com/thanhndv212/figaroh-examples/milestone/18) |

**C5** (calibration studies, tracker core #167) and **D8** (identification studies, core #157–#161) were added on 2026-10-09 beneath M2 and are outside baseline M1 closure. Their milestones were created on 2026-10-10. Each data transfer is its own examples issue: C5 examples #104–#114 (sub-issues of their studies), D8 examples #115 (sub-issue of core #160).

**LC1** uses existing core #20 as the canonical conditional-production tracker for core #23–#25 and examples #11. Its go gate is research issue #155 (moved from D5 on 2026-10-10); a go decision there is a dependency, not completion of those production issues. LC1 is outside baseline M1 closure and introduces no additional production scope.

Initial queue (2026-10-02): D1 / core #32 was the first ready correctness fix, delivered by #69. W1, W2, W3, W4, D1, D2, D3, D6, C1, C2, C3, C4 and S1 have since cleared (see [implementation status](#implementation-status)). Current queue (2026-10-12): calibration and dynamic references are both done through M1.4 (C4, D7; released in 0.6.0 and 0.7.0), U1 cleared on 2026-10-11 and M1 was accepted on 2026-10-12. The critical path is the 1.0 work (see the [path forward](#path-forward)). On the optional path, D5 cleared (#30 no-go) and LC1's go gate #155 (method change) is in progress. Select one issue, validate its focused change, open its PR, then await review/merge approval.

### Experimental design decisions to review before coding

#### Dynamic identification

Use two comparison levels. The first holds identical training-estimated extras fixed to isolate inertial estimation; the second jointly estimates physical extras where supported. Do not rank a joint solver against a frozen-extra solver as though only parameterization changed. Define per-joint residual scaling from training noise/units if whitening is proposed, and first retain an unweighted compatibility baseline. TIAGo force and torque cannot become one unlabeled physical RMSE.

Full SDP equality reconstruction preserves an estimated base vector; direct constrained effort fitting may change it; per-link projection may change both base fit and effort. Each needs its own objective, success definition and stage label. Boundary solutions and massless fixed links need explicit policy. No API is chosen just because one research script already contains a function.

Freeze ranks/tolerances, bounds, noise, initialization, iteration/evaluation budget, timeout and quality metrics before measuring. Preserve the old protocol. The existing #30 criteria (carried into #155) are not relaxed by this plan; any revised criterion requires a reasoned decision before new measurements.

#### Calibration

TIAGo mocap is the user-selected first real reference because existing redistribution and runtime-export support can be reused. TALOS table-contact remains a second consumer and a regression target, not a substitute for full 6D ground truth. Use held-out postures/recordings with no fit-stage leakage. Address gauge/frame ambiguity before interpreting individual geometric corrections or covariance. Assess translated and rotational residuals separately unless a justified measurement covariance defines a common whitened residual.

New camera/eye-hand, multi-marker or suspension features are candidates for a separate reviewed scope after the reference baseline; they are not implicitly included in this iteration. Historical port documents must be re-audited.

### General onboarding pipeline alongside the references

The two reference workflows are demonstrations, not the only supported route for users. Add a reusable decision pipeline for any new robot/dataset without creating a new runtime orchestration API in this planning phase:

1. Inventory available models, sensors, timestamps, measured/derived channels and independent validation; record missing information.
1. Define the application, observation model, parameter blocks, fixed quantities, gauge and limits on what the data can identify.
1. Select supported baseline/candidate methods by objective, constraints and noise/prior assumptions; label research paths separately.
1. Design acquisition and final validation together, reviewing excitation, observation coverage, feasibility and collection constraints.
1. Preserve raw measurements; audit clocks, units, frames, synchronization, derivatives/filter rates and raw-to-processed indices.
1. Fit under a frozen protocol; interpret training fit, observability, physical feasibility and solver termination separately.
1. Evaluate unused data, inspect computed/skipped verification checks, validate exported-model parity and archive reproducible evidence.

Core `docs/source/example_workflow.md` owns this scientific decision guide. Existing task tutorials link to it and retain task-specific runnable material. Examples `docs/new-example-guide.md` owns repository integration and `docs/experiment-brief-template.md` provides a human-readable record copied into `examples/<robot>/EXPERIMENT.md`. This is not a new YAML schema or an enforced runtime contract. Robot README/data notes own actual measurements and commands; CONTRIBUTING owns contributor/review rules.

**Work-package milestone U1 — general user onboarding** runs alongside D7/C4. Draft the guide now; validate it later by walking through one fresh dynamic example and TIAGo mocap, plus a missing-data case. Acceptance means the brief leads to an explicit model/method choice, reproducible processing and applicable validation without relying on undocumented reference assumptions. Check that supported commands/links match the accepted core/examples revisions; label unimplemented methods and missing evidence. Do not add an interactive wizard, new config keys or generated acquisition code without separate review.

Round 2 reviews the brief's questions and evidence requirements alongside the data/result contracts. Reference delivery PRs provide worked briefs and expose where the generic guide needs correction. U1 does not wait for backend expansion or a successful research-only log-Cholesky integration.

### Repository and documentation organization

Preserve existing directories and user entry points. Do not move datasets, robot helpers or broad packages as a prerequisite to fixing known defects.

- Core `ROADMAP.md`: milestone order, priorities and exit criteria; one canonical file embedded by the docs site. Stable M0–M3 IDs remain.
- Core `docs/plans/`: this active execution proposal; `archive/` keeps old plans. Add an index with explicit Draft/Accepted/Historical status.
- Core `docs/decisions/`: narrow ADRs for new schemas/interfaces/objectives. Link to existing decisions rather than duplicating their reasoning.
- Core `docs/development/`: dated audits and validation evidence; private spikes remain marked experimental. Architecture changes only after facts change.
- Examples `benchmarks/`: research protocol, comparisons, curated small result records and dated reports. Preserve old protocols and source hashes.
- Examples `examples/<robot>/`: runnable supported recipes and robot data/config notes. Large generated runs remain outside source control or in CI artifacts.
- Each repository `CONTRIBUTING.md`: its execution/PR instructions. Core owns the shared rules; examples documents exact commands, dependency pairing, fixtures and its `main` PR target rather than copying the whole core workflow.

Existing examples `.github/` ignore rules and local-only CI material need a focused tracked-file audit before adding more automation. A local file or passing local test is not proof that it is tracked or executed remotely.

### Proposed development workflow improvements

These amendments are for review; CONTRIBUTING/AGENTS are not silently replaced by this draft. Retain the established one-issue/one-PR rule and explicit merge approval. Suggested additions:

1. **Discussion before implementation.** Agree on scope and acceptance evidence; approve public contracts through an ADR. Separate “approve plan,” “implement,” and “merge.” A research revise decision cannot be interpreted as API approval.
1. **Issue readiness.** Classify numerical bug, asset/dependency problem, research, API feature or maintenance. State core/examples owner, dependencies, current reproducer, immutable inputs, proposed tests and out-of-scope work. Avoid creating all speculative issues at once.
1. **Focused branches.** Core branches from current `devel`; examples from `main`. Keep discussion/report changes out of unrelated numerical PRs. Record tested core/examples commit pairs. Prefer isolated worktrees when an open PR is already using the main checkout.
1. **Before/after evidence.** Run a reproducer on baseline and changed revisions. Report command/exit code and classify data validity, solver convergence, physical consistency, held-out quality and export fidelity separately.
1. **Validation tiers.** V0 hooks/docs for documentation; V1 core tests for code; V2 affected examples for data/numerics/API; V3 both reference workflows and export invariants for milestone/release. Examples retains full `validate.py` phase checks and known-failure evidence. A failure cannot be hidden by a skip.
1. **CI organization.** Fast core checks plus Pinocchio 3.7/4.1 profiles; examples deterministic fixtures/smokes and dataset jobs with reproducible assets. Slow IPOPT/full workflow checks can be separately scheduled/manual, retaining their failure/timeout statuses. Collect research artifacts on failure without marking a failed regression green. This is a proposed CI split, not deployed.
1. **Review and merge.** Open each completed issue's focused PR. Maintainer reviews; merge only after explicit approval and required checks on the current head. Material scope changes require renewed review. No automatic merging.
1. **Post-merge cleanup.** Synchronize base branches; remove that PR's merged remote/local feature branch and worktree after checking for uncommitted or unique commits. Keep main/devel/releases and unrelated ancient branches.
1. **Release evidence.** An accepted method and passing CI are insufficient: verify supported examples, exported model reloads, archived reports, version metadata and dependency pair. Version/tag/publishing remains separate review.

Recommended issue metadata: type, area, priority, milestone, dependency links, research/production designation and acceptance evidence. Recommended states: Draft → Ready → In progress → In review → Done, with Blocked carrying its actual dependency. Readiness labels and milestone/issue rosters are established by the approved tracker setup. They are maintained manually; GitHub Project automation and server-enforced branch rules are not implied.

### Review rounds and bounded implementation batches

#### Round 1 — this proposal

Review the two parallel outcomes, package ownership, calibration reference, method comparison boundaries and proposed sequence. Revise roadmap/plan locally. The subsequent user instruction explicitly authorized tracker creation. No code changes, commits/pushes or merges follow merely from accepting the plan.

#### Round 2 — contracts and frozen protocol

Inventory current data/results and propose the smallest compatible extension. Review each numerical objective, extras/weighting policy, exports and failure semantics. Decide what metrics and datasets can actually establish. Update narrow ADRs and the benchmark protocol before approving implementation.

#### Round 3 — first delivery batch

Select only ready work: existing core #32 (delivered by #69); reproducible example asset fixes; existing examples #14; separate robot sampling audits. Calibration frame audit can proceed in parallel. **Done (2026-10-03):** this batch cleared as D1, W1, W2, D2 and C1, together with W4. Open focused issues for the approved missing work. Each completed issue produces a PR for review; dependent work waits or uses an explicit tested commit pair. Review evidence before expanding the batch.

#### Later batches

Fresh UR10 truth fixture and calibration truth/held-out fixture; physical solver comparison and #30; independent inertial/geometric export gates; then dynamic and calibration reference integration. Each batch ends with an evidence review, not a guessed calendar deadline. Estimates follow a scoped issue audit; no release date or package version is promised in this draft. Progress at 2026-10-06: the calibration batches (C2, C3, C4) and inertial export (D6) have cleared; the UR10 truth fixture (D3) cleared on 2026-10-07; the physical comparison (D4) cleared on 2026-10-09.

### Decisions still open

Calibration order is settled for this draft: TIAGo mocap first, TALOS contact second. The following choices still need review:

1. Should the first joint physical estimator include bounded/positive friction and actuator parameters, or limit the initial comparison to fixed extras?
1. Which independent real recordings are available for final acceptance?
1. What is the minimum supported result/data extension after adapter inventory?
1. What measured physical boundary policy and export failure behavior should the public API expose?

Proposed remaining defaults: fixed-extra comparison followed by a separate joint-estimation comparison; no claims of independent real validation until recordings exist; additive compatibility adapters; failed/nonphysical stages cannot silently become the selected exported model.

# Roadmap

The canonical roadmap lives at the repository root ([`ROADMAP.md`](https://github.com/thanhndv212/figaroh-plus/blob/main/ROADMAP.md)) so it stays next to the code it tracks. This page embeds it directly so the same content is browsable as part of the docs site, without a second copy to keep in sync.

## FIGAROH Roadmap

**Evidence refresh: 2026-10-12; delivery status refreshed 2026-10-12. Priority changes below are a tracker scope approved; execution details under review, revision 4.**

Review the [detailed delivery plan](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/index.md) before implementation. The user requested calibration improvements in parallel with dynamic identification, with TIAGo mocap as the first calibration reference and TALOS contact as the second consumer/regression target. Proposed work is not an accepted API, completed feature or authorization to implement/merge.

FIGAROH turns robot measurements into calibrated, identified models with explicit validation and export evidence. The proposed next delivery cycle advances two parallel reference workflows in `figaroh-examples`: dynamic identification → physical model → held-out effort verification → inertial export, and geometric calibration → identifiable corrections → held-out pose/contact verification → geometric export. Both must archive reproducible evidence and validate reloaded models.

This file owns priorities and milestone exit criteria. [GitHub issues](https://github.com/thanhndv212/figaroh-plus/issues) own individual work items; [CHANGELOG.md](https://github.com/thanhndv212/figaroh-plus/blob/main/CHANGELOG.md) owns release history; [ARCHITECTURE.md](https://github.com/thanhndv212/figaroh-plus/blob/main/ARCHITECTURE.md) describes current implementation; and [design decisions](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/README.md) explain proposals and trade-offs. The [contribution workflow](https://github.com/thanhndv212/figaroh-plus/blob/main/CONTRIBUTING.md) defines how work reaches `devel` and then `main`. The docs site embeds this file directly.

### Current evidence and unresolved gates

- Pinocchio and MuJoCo backend modules exist. The high-level identification workflow still requires a URDF-based robot; `from_mjcf()` raises `NotImplementedError`. See the [backend boundary](https://github.com/thanhndv212/figaroh-plus/blob/main/ARCHITECTURE.md#backend-boundary).
- Projection and reconstruction utilities exist. Projection results already retain both `raw_parameters` and `projected_parameters`; their config dictionaries, including `max_seconds`, are passed through by the parser. The old roadmap's claims that raw parameters are overwritten and that this field is never forwarded are obsolete.
- General URDF export now writes identified first moments and inertia tensors and verifies them by reload ([#60](https://github.com/thanhndv212/figaroh-plus/issues/60), PR #138, merged 2026-10-06). D6 cleared on 2026-10-06, so the dynamic side of the M1.3 export gate is met; the whole gate closes at review.
- The September identification fixes (#11–#14: CAD inertia lookup, optional regressor blocks, filtering configuration, relative QR threshold) shipped in 0.5.0. See the changelog for their exact scope.
- The local baseline before this workflow change was **499 passed, 20 skipped** in `figaroh-dev` (Python 3.12). This is dated evidence, not a permanent test count or a claim about hosted CI. Details and skipped coverage are in the [audit record](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/development/workflow-audit-2026-09-28.md).

#### October findings

- The reproduced final-joint acceleration defect ([core #32](https://github.com/thanhndv212/figaroh-plus/issues/32), package D1) is fixed by [PR #69](https://github.com/thanhndv212/figaroh-plus/pull/69), merged into `devel` on 2026-10-02. UR10/TIAGo signal-rate assumptions still require separate examples audits (D2) before new scientific benchmark claims.
- Log-Cholesky research (D5) is optional and still open. The feasibility spike ([core PR #31](https://github.com/thanhndv212/figaroh-plus/pull/31), merged) recorded **revise**; the budget/scaling revision ([#30](https://github.com/thanhndv212/figaroh-plus/issues/30)) recorded **no-go** because 8 of 24 gated fits still exhaust 2000 evaluations. [#155](https://github.com/thanhndv212/figaroh-plus/issues/155) changes the fitting method next. The private experiment is not a production solver, and #23–#25 stay blocked until a go decision.
- [Examples PR #13](https://github.com/thanhndv212/figaroh-examples/pull/13) was merged on 2026-10-08. The Hey5 geometry failure was fixed by W1 (every robot loads from a clean checkout) and the 3.7 TALOS regression ([examples #14](https://github.com/thanhndv212/figaroh-examples/issues/14)) was explained and fixed by W2. Completion of a dataset runner is not a passing physical-model workflow.
- Local supplemental comparisons distinguish exact base-preserving SDP reconstruction from per-link projection; exact reconstruction yields no accepted model and nonlinear dataset candidates exhaust 200 evaluations. A direct constrained SDP effort-fit comparator remains unmeasured.
- Existing calibration redistribution, report/archive and geometric export helpers provide a starting point for a parallel calibration reference. New data/result types or composition interfaces require an ADR first.

#### Delivery progress (2026-10-12)

**Cleared:** W1, W2, W4 (M0 delivery roster); D1, D2, C1 (M1.1 inputs); C2 (calibration side of M1.2); C3 and D6 (M1.3 export); W3 data/result contracts and S1 minimum reporting (M1.4 groundwork); **C4 calibration reference** (M1.4); D4 physical-estimation comparison (M1.2) and **D7 dynamic reference** (M1.4). The calibration workstream is complete through M1.4: one command (`examples/tiago/reference_run.py`) fits TIAGo mocap, reports held-out marker RMSE of 3.8–4.2 mm on three unused sessions, and exports a URDF and PAL file that reload within 2.1e-12 m. The dynamic workstream is complete through M1.4: one command (`examples/identification_reference.py --robot ur10|tiago`) selects the direct LMI effort fit, verifies it on held-out splits and exports a URDF that reloads within 1e-9. U1 onboarding cleared on 2026-10-11. **M1 was accepted on 2026-10-12** ([review tracker #181](https://github.com/thanhndv212/figaroh-plus/issues/181)) on core `779e830` + examples `b167298`: closure records ratified (R-1: W1, W2, W4, D1, D2, C1, D4, S1, D7), the #97 duplicate closure accepted as completed (C-1), and scope accepted (S-1 real-data references at execution scope, with held-out error reported, not gated; S-2 TIAGo effort scale and arm_1 constant still open; S-3 lumped TIAGo export; S-4 PAL not run on hardware; S-5 TALOS contact as a regression consumer). The one code gap, G-1 (a failed calibration fit could be exported), is fixed before 1.0.0: core #182 by PR

## 187 (`9dba6da` on `devel`) and examples #124 by PR #127 (`20b6377` on `main`).

Per-package evidence is in the [implementation status](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/#implementation-status).

**M0 gate review (accepted 2026-10-12, [tracker #181](https://github.com/thanhndv212/figaroh-plus/issues/181) §9).** Existing core gates retained (blocking `Lint`, Core CI and Documentation green); examples assets and dependencies reproducible (W1); numerical regressions explained (W2); review/merge rules and cleanup applied (W4); exact tested commit pairs cited as core `fe7dc51` + examples `b8d9c75` (hosted runs 37134266859 / 37136143367) and core tag `v0.5.0` (`02f705a`). The pair is not clean for the M0 cycle: examples has no `v0.5.0` tag, because pairing started at 0.6.0.

These are dated observations; verify heads and status before beginning work. The [delivery plan](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/index.md) maps concrete work to existing modules and both repositories. Numerical limitations must be reproduced and classified rather than waived to obtain a green check.

### Delivery order

Milestone IDs below are stable references for issues. `Now` means the next integration priority; `Next` means queued behind its dependencies; `Later` and `Research` are unscheduled. Completion requires the exit criteria and linked validation evidence, not just the presence of a module.

| Milestone                                      | Priority                          | Outcome                                                                                  | Exit criteria                                                                                                                                                                                      |
| ---------------------------------------------- | --------------------------------- | ---------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| M0 — Engineering baseline                      | Now, continuous                   | Reproducible review and validation in both repositories                                  | Existing core gates retained; examples assets/dependencies reproducible; numerical regressions explained; review/merge rules and cleanup applied; exact tested commit pairs                        |
| M1 — Trustworthy model workflows               | ✅ Accepted 2026-10-12 (#181)     | Dynamic and geometric reference workflows with held-out verification and faithful export | Correct signals/frames/units; explicit fit and validation splits; identifiable/physical parameter diagnostics; selected output stage; exported model reload parity; archived report and provenance |
| M2 — Composable calibration and identification | Next, after reference contracts   | Reusable data/diagnostic primitives and selective regressor/residual composition         | Two concrete consumers; accepted narrow ADR; legacy/default numerical parity; additive result compatibility; no premature broad workflow rewrite                                                   |
| M3 — Backend parity and broader examples       | Later, after reference acceptance | Supported backend operations have measured parity                                        | Actual backend selection or rejection; capability matrix; same-input accuracy/runtime comparisons; supported export/validation path; unsupported capabilities explicit                             |

M0's delivery roster for this cycle (W1, W2, W4) has cleared: examples assets and hosted CI are reproducible on both Pinocchio profiles, the TALOS regression is explained, legacy lint debt is removed and the full pre-commit suite is blocking. The M0 gate review was accepted on 2026-10-12 (see below). M1 was accepted the same day; its record is under Delivery progress below. Calibration reliability improvements run alongside dynamic identification; M2 refers to the broader composition architecture, not a requirement to postpone all calibration work until dynamics finishes. Versions follow the [release plan](#release-plan): each minor release ships one accepted outcome; no release date is promised.

#### Delivery milestones beneath roadmap outcomes

M0–M3 describe strategic outcomes. Work packages are the delivery milestones that achieve them; issues are their implementation units. Their canonical scope, dependencies and closure policy are in the [work-package milestone rules](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/#work-package-milestone-rules).

| Roadmap outcome / gate      | Required delivery milestones in this draft                                                                       | Status at 2026-10-12 (✅ = cleared)                        |
| --------------------------- | ---------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------- |
| M0 reviewed delivery scope  | W1 fixtures, W2 regression diagnosis, W4 engineering process                                                     | ✅ **W1, W2, W4 all cleared**                              |
| M1.1 inputs                 | D1–D2 dynamic signals; C1 calibration frames/data                                                                | ✅ **D1, D2, C1 all cleared**                              |
| M1.2 estimation             | D3–D4 dynamic truth/comparison; C2 calibration truth/holdout                                                     | ✅ **C2, D3, D4** all cleared                              |
| M1.3 export                 | D6 inertial export; C3 geometric export                                                                          | ✅ **C3, D6** both cleared                                 |
| M1.4 integrated workflows   | W3 minimum contracts, S1 minimum reports, D7 dynamic reference, C4 calibration reference, U1 verified onboarding | ✅ **W3, S1, C4, D7, U1**                                  |
| M2 follow-up                | S2 uncertainty, S3 experiment-design improvements, S4 selective composition; W3 is prerequisite groundwork       | ✅ W3 groundwork done; S2 in progress; S3, S4 blocked      |
| M3 backend parity           | B1 capabilities and measured parity; broader ports need separately reviewed packages                             | Blocked on M1                                              |
| Optional nonlinear research | D5 and linked production #23–#25 when that path is approved; excluded from baseline M1 closure                   | D5 open: #22 revise, #30 no-go; #155 (method change) ready |

A delivery milestone is cleared only when **all included issues meet their acceptance criteria**, required changes are reviewed/merged, and milestone-level integration evidence is accepted. Deferred or administratively closed issues are not completed work; scope changes require review and preserve the original roster/history. Each roadmap gate closes only when all required delivery milestones and its own exit criteria pass. M0's delivery roster is scoped per cycle; ongoing maintenance does not prevent acceptance of that reviewed cycle.

GitHub milestones and canonical issue rosters are available in the [delivery register](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/#github-delivery-register). The optional LC1 tracker reuses Feature 3 #20 and remains outside baseline M1.

Track the chain in both directions: roadmap gate → delivery milestone → owning issues → reviewed PRs → acceptance report. Issue/project records own current status and evidence links; this roadmap does not duplicate live checklists.

#### M1 review gates

| Gate                                | Dynamic workstream                                                                                               | Calibration workstream                                                                                                      |
| ----------------------------------- | ---------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------- |
| M1.1 — Inputs and reproducibility   | Correct tangent derivatives; audited rates/conversions; synchronized channels; independent preprocessing         | Audited observation frames, units and timestamps; reproducible TIAGo/TALOS baseline; explicit gauge/measurement assumptions |
| M1.2 — Verified estimation protocol | Fresh UR10 truth fixture; distinct base/SDP/projection/nonlinear objectives; frozen comparison and solver status | Synthetic correction truth in identifiable coordinates; genuine held-out postures; solver/gauge/regularization diagnostics  |
| M1.3 — Faithful model export        | Complete first-moment/tensor semantics; physical verdict and reloaded-model RNEA parity                          | Redistribution semantics; exported/reloaded FK parity; runtime correction output where relevant                             |
| M1.4 — Accepted reference workflow  | Supported physical baseline → held-out effort report → export/archive; nonlinear integration only after go       | Geometric fit → held-out pose/contact report → export/archive; contact-only evidence explicitly labeled                     |

The calibration and dynamic gates are reviewed independently. M1 closes when all required delivery milestones above and both reference outcomes pass. A supported physical dynamic baseline can ship without waiting for log-Cholesky go; the nonlinear method remains opt-in and conditional. A failed/fallback model cannot silently become an exportable production result. Future uncertainty and experiment-design work follows stable reference baselines, with backend expansion queued behind measured parity.

### Release plan

Versions follow roadmap outcomes, not commit counts. A release ships what has been accepted: its delivery packages are cleared, each with a closure record and a core/examples revision pair, and those records are its evidence.

| Bump                     | When                                                                                                                                                                         |
| ------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| **0.MINOR** (before 1.0) | One roadmap outcome or gate side is accepted                                                                                                                                 |
| **0.x.PATCH**            | Wrong-result or crash fixes with no behaviour change, from a `main` hotfix branch, then merged back into `devel`                                                             |
| **1.0.0**                | M1 accepted (both reference workflows), public API declared, deprecated features removed; a release candidate first (waived by the maintainer for 1.0.0 on 2026-10-12, #192) |
| **After 1.0**            | M2 additions are minor releases; removing a deprecated feature is a major release                                                                                            |

A deprecated feature warns for at least one minor release before it is removed; removals are batched into 1.0. Optional research (D5/LC1) ships as an opt-in feature in the minor release after its go decision and never holds a release.

| Version    | Outcome                                                                             | Delivery packages                                                                                                                                                                                                                                                                                                                                                                                                                                   | Status                                                                                                                                                                                          |
| ---------- | ----------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| 0.5.0      | M0 engineering baseline                                                             | W1, W2, W4, D1                                                                                                                                                                                                                                                                                                                                                                                                                                      | Released 2026-10-03                                                                                                                                                                             |
| 0.6.0      | Calibration reference workflow (M1.1–M1.4, calibration side) and shared foundations | D2, C1–C4, W3, S1; D6 inertial export and D3 fixture fixes, both accepted                                                                                                                                                                                                                                                                                                                                                                           | Released 2026-10-07 ([release](https://github.com/thanhndv212/figaroh-plus/releases/tag/v0.6.0), [tracker #146](https://github.com/thanhndv212/figaroh-plus/issues/146)); examples tag `v0.6.0` |
| 0.7.0      | Dynamic reference workflow (M1.2–M1.4, dynamic side)                                | D4, D7 (D5 no-go; LC1 #155 not included)                                                                                                                                                                                                                                                                                                                                                                                                            | Released 2026-10-11 ([release](https://github.com/thanhndv212/figaroh-plus/releases/tag/v0.7.0), [tracker #177](https://github.com/thanhndv212/figaroh-plus/issues/177)); examples tag `v0.7.0` |
| 1.0.0      | M1 accepted                                                                         | M1 review ✅ (2026-10-12, [#181](https://github.com/thanhndv212/figaroh-plus/issues/181)); public API (#184), deprecation removals (#185, examples #125), G-1 (#182) and RC procedure (#186) done. Released 2026-10-12 without a release candidate (maintainer decision; [release](https://github.com/thanhndv212/figaroh-plus/releases/tag/v1.0.0), [tracker #192](https://github.com/thanhndv212/figaroh-plus/issues/192)); examples tag `v1.0.0` |                                                                                                                                                                                                 |
| 1.1, 1.2   | M2 composable calibration and identification                                        | S2, S3, S4                                                                                                                                                                                                                                                                                                                                                                                                                                          | Later                                                                                                                                                                                           |
| 1.x or 2.0 | M3 backend parity                                                                   | B1                                                                                                                                                                                                                                                                                                                                                                                                                                                  | Later; 2.0 only if the backend API breaks                                                                                                                                                       |

Each release is tracked by a core release issue. It lists the packages in scope and what is excluded, freezes `devel`, and records the release-candidate evidence on one core/examples pair: core tests, hosted CI on both Pinocchio profiles, examples `validate.py`, package build with `twine check` and a clean wheel install, and the docs build. The release PR bumps both version strings and dates the changelog with a scope paragraph, behaviour changes, migration notes and limitations. The [release sequence](https://github.com/thanhndv212/figaroh-plus/blob/devel/CONTRIBUTING.md#merge-and-release-gates) then merges `devel` into `main` with a merge commit (branch deletion off); one build from the `vX.Y.Z` tag goes to PyPI and the GitHub release, with matching sha256. The examples repository pins `figaroh>=X.Y,<X.(Y+1)` and is tagged `vX.Y.Z` as the paired revision. Finally `main` is merged back into `devel`.

### Work to split into issues

Create focused issues from these candidates as they enter `Ready`; keep their acceptance criteria and changing status in the tracker. For work spanning both repositories, use a parent issue in `figaroh-plus` and linked implementation issues/PRs in `figaroh-examples`, with the tested commit pair recorded.

| Candidate                                                  | Milestone                 | Evidence or acceptance gate                                                                                                                                                                                                                                                            |
| ---------------------------------------------------------- | ------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| ✅ Legacy lint and formatting cleanup                      | M0 follow-up              | **Done in W4** (#81–#85 via #87–#92); full pre-commit is a blocking gate (#93)                                                                                                                                                                                                         |
| ✅ Regressor tests that skip on incompatible mocks         | M0 follow-up              | **Done in W4** (#58 via #80): obsolete mocks replaced with real-model coverage                                                                                                                                                                                                         |
| ✅ Signal-rate/provenance audits and fresh UR10 truth data | M1.1/M1.2                 | ✅ Audits **done in D2**; fresh UR10 truth data **done in D3** (examples #89, frozen protocol v1). Original gate: independent analytic truth and validation trajectories                                                                                                               |
| ✅ Physical estimator comparison and convergence revision  | M1.2                      | **Done in D4** (#39; examples #22 report): direct LMI effort fit selected for D7. Original gate: reuse #30; distinguish exact reconstruction, direct constrained effort fitting, per-link projection and log-Cholesky; freeze metrics/extra-column policies before measurement         |
| ✅ Calibration held-out reference and export parity        | Parallel M1.1–M1.4        | ✅ TALOS #14 diagnosed (W2), held-out reference (C2) and reloaded-model FK parity (C3) **done**; integrated TIAGo reference **done in C4** (examples #29 via examples #88)                                                                                                             |
| ✅ General robot/dataset onboarding guide                  | Parallel M1 / proposed U1 | **Done in U1** (examples #33 via examples PR #123). Original gate: Data/model inventory → method choice → acquisition/processing → fit interpretation → held-out validation/export; reusable experiment brief; worked dynamic and TIAGo mocap cases; missing-data limitations explicit |
| ✅ Narrow data/result contract                             | M1/M2                     | **Done in W3** (core #126/#128/#130, examples #84). Original gate: Inventory current adapters; ADR; two consumers; legacy dictionaries/configs remain compatible; proposed types not presented as existing API                                                                         |
| ✅ Full inertial URDF export                               | M1 correctness            | **Merged in D6** (#60, PR #138). Original gate: first moments and inertia tensors survive export/reload; mass/CoM conventions and inertia reference frames are tested                                                                                                                  |
| ✅ End-to-end reference example                            | M1                        | Calibration side ✅ done in C4; dynamic side ✅ **done in D7** (#42). Original gate: connect the existing projection, reconstruction, verification, export and archive APIs; demonstrate improvement on held-out data                                                                  |
| Backend selection in `RobotIdentificationSystem`           | M3 correctness            | `from_urdf(backend=...)` currently stores a name while loading the usual Robot; selection must change the executing backend or reject unsupported choices                                                                                                                              |
| Optional backend/solver coverage                           | M0/M3                     | Record tested dependency versions and skips; broaden OS/Python coverage after the initial Python 3.12 gate                                                                                                                                                                             |

Bug severity takes precedence over milestone order. A reproducible wrong-result bug can be fixed immediately without starting the rest of its milestone.

### Existing tracks and design material

The active [delivery plan](https://thanhndv212.github.io/figaroh-plus/further_reading/plans/index.md) contains work packages, dependency order and proposed workflow amendments. Milestone/issue setup is authorized and linked from the plan; implementation proceeds one ready issue at a time after its unresolved decisions are reviewed. Track letters from the previous roadmap remain useful labels for issues. Detailed historical tasks are retained in the [archived v2 roadmap](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/plans/archive/roadmap-v2.md); its old dates and completion claims are not current commitments.

| Track                          | Retained scope                                                                                                                                                 | Delivery home / design reference                                                                                                                                                                                                                             |
| ------------------------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
| A — Algorithmic core           | Physical consistency, reconstruction, CAD constraints, modular pipeline, model visualization; later online ID, friction models and FIM-based experiment design | M1/M2; [linear terms proposal](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/modular-linear-residual-terms-plan.md)                                                                                                                   |
| B — Backends                   | Pinocchio/MuJoCo parity, explicit API capabilities, benchmarks and CLI; later Genesis/IsaacSim                                                                 | M3; [current backend limits](https://github.com/thanhndv212/figaroh-plus/blob/main/ARCHITECTURE.md#backend-boundary)                                                                                                                                         |
| C — Reporting and quality      | Verification, reports/archives; remaining optimal-task reporting and report-schema decisions                                                                   | M0/M1; [comparison and reporting rationale](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/external-tool-comparisons.md)                                                                                                               |
| D — Calibration composability  | Residuals, staged solving, priors, camera intrinsics, sensor input and camera-YAML export                                                                      | M2; [calibration design material](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/external-tool-comparisons.md)                                                                                                                         |
| E — Examples and robot ports   | TALOS/TX40 script parity, TIAGo eye-hand work, suspension/backlash examples and export integration                                                             | M1/M3; [example audit](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/figaroh-examples-improvement_plan.md), [TIAGo review](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/tiago-calibration-and-port-review.md) |
| F — Deployment and sim-to-real | Data adapters, model-based control, rollout refinement and retargeting                                                                                         | Research; [deployment proposal](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/sim2real-modelbased-deployment.md)                                                                                                                      |

The SO-101 example added to `figaroh-examples` is useful simulation evidence for gravity/friction identification. It does not establish complete inertial export or physical-hardware validation for M1. Existing TIAGo suspension/backlash work remains a research example; generic core promotion has separate gates in the [composition proposal](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/decisions/modular-linear-residual-terms-plan.md).

### Review and completion

At milestone review, record the core/examples revisions, environment, commands, pass/fail/skip counts and domain metrics using the [validation guide](https://github.com/thanhndv212/figaroh-plus/blob/main/docs/development/validation.md). A numerical improvement needs a comparable dataset and metric; a successful URDF write needs a reload/round-trip check; a simulated result is labeled as such.

Update this roadmap when a milestone's outcome or priority changes. Update the issue for task progress, the architecture for changed contracts, and the changelog for shipped behavior. Research enters the delivery queue only after its scope, dependencies and measurable exit criteria are accepted.