Tools¶

Reporting & Verification¶

The reporting & verification suite'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)

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

    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>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">{_param_uncertainty_section(
        param_names,
        eval_.get("param_stdev", []),
        eval_.get("param_stddev_percentage", []),
    )}</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")

    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>Insights</h2>
    {_insights_section(insights)}
  </section>

  <section>
    <h2>Per-joint torque residuals</h2>
    <div class="card">{_per_joint_section(per_joint)}</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 [],
    )}</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

_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)` `dataclass` ¶

One metric checked against one threshold.

`VerificationVerdict(passed, checks, metrics, insights=list(), metadata=dict(), series=dict(), compat=dict())` `dataclass` ¶

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)` ¶

Check each metric named in thresholds against its threshold.

A threshold whose metric is missing from metrics (or is NaN) — e.g. a validation-set threshold when no validation data was provided — is silently skipped rather than counted as a failure; verify() callers can note the gap via insights instead. An empty check list (nothing was evaluable) counts as passed: there is nothing to fail on.

Source code in src/figaroh/tools/_report_common.py

def evaluate_thresholds(
    metrics: Dict[str, float], thresholds: Dict[str, Dict[str, Any]]
) -> VerificationVerdict:
    """Check each metric named in ``thresholds`` against its threshold.

    A threshold whose metric is missing from ``metrics`` (or is NaN) —
    e.g. a validation-set threshold when no validation data was
    provided — is silently skipped rather than counted as a failure;
    ``verify()`` callers can note the gap via ``insights`` instead. An
    empty check list (nothing was evaluable) counts as passed: there is
    nothing to fail on.
    """
    checks: List[ThresholdCheck] = []
    for name, spec in thresholds.items():
        value = metrics.get(name)
        if value is None or (isinstance(value, float) and math.isnan(value)):
            continue

        threshold = spec["threshold"]
        comparison = spec["comparison"]
        if comparison == "max":
            passed = value <= threshold
        elif comparison == "min":
            passed = value >= threshold
        else:
            raise ValueError(
                f"Unknown comparison {comparison!r} for threshold {name!r} "
                "(expected 'max' or 'min')"
            )
        checks.append(
            ThresholdCheck(
                name=name,
                value=float(value),
                threshold=float(threshold),
                comparison=comparison,
                passed=bool(passed),
            )
        )

    overall_passed = all(c.passed for c in checks) if checks else True
    return VerificationVerdict(
        passed=overall_passed, checks=checks, metrics=dict(metrics)
    )

`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_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(),
            "git_commit": git_commit,
            "git_dirty": _git_dirty(),
            "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
            )

    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))

    # 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` `property` ¶

Number of actuated joints (excluding universe and free-flyer).

`actuated_joint_names` `property` ¶

Names of actuated joints only.

`freeflyer_limits` `property` ¶

Current free-flyer position limits.

`backend` `property` ¶

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)` `dataclass` ¶

Configuration for regressor computation.

`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()

`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	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	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, param)` ¶

Build regressor for total least squares with external wrench measurements.

Parameters:

Name	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
`param`	Dictionary of settings	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, param
):
    """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
        param: Dictionary of settings

    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)` `dataclass` ¶

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].parent - 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].parent - 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."""
    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."""
    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."""
    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."""
    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."""
    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."""
    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."""
    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 for backward compatibility.

Source code in src/figaroh/tools/robotcollisions.py

def __init__(self, robot, geom_model=None, geom_data=None, viz=None):
    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)

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)` `dataclass` ¶

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 (`P` from `scipy.linalg.qr`); `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)` ¶

Enhanced QR decomposition handler for robot parameter identification.

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,
):
    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"}:
        _, R, P = linalg.qr(W_e, pivoting=True)
        rank = self._find_rank(R)
        params_sorted = [params_r[P[i]] for i in range(P.shape[0])]
        R1, Q1, R2 = self._extract_base_components(
            R, np.linalg.qr(W_e[:, P])[0], rank
        )
        beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
        phi_b = np.round(np.linalg.solve(R1, Q1.T @ tau), 6)
        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.round(np.linalg.solve(R1, Q1.T @ tau), 6)
        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 = linalg.qr(W_e, pivoting=True)

    # Reorder parameters according to pivoting
    params_sorted = [params_r[P[i]] for i in range(P.shape[0])]

    # Find effective rank
    rank = self._find_rank(R)

    # Extract base components
    R1, Q1, R2 = self._extract_base_components(R, Q, rank)

    # Compute base parameters — full precision; round only for display
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
    phi_b = np.round(np.linalg.solve(R1, Q1.T @ tau), 6)
    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. 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.

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
    beta = np.linalg.solve(R1, R2) if R2.size else np.empty((rank, 0))
    phi_b = np.round(np.linalg.solve(R1, Q1.T @ tau), 6)
    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.around(phi_std, 5)

`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 = linalg.qr(W_e, pivoting=True)
    rank = self._find_rank(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)` `staticmethod` ¶

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

`QR_pivoting(tau, W_e, params_r, tol_qr=TOL_QR)` ¶

Legacy QR pivoting function for backward compatibility.

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 for backward compatibility."""
    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 for backward compatibility.

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 for backward compatibility."""
    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)

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())` `dataclass` ¶

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()` `classmethod` ¶

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()` `classmethod` ¶

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()` `abstractmethod` ¶

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()` `abstractmethod` ¶

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()` `abstractmethod` ¶

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)` `abstractmethod` ¶

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)` `abstractmethod` ¶

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: -1 (solved to acceptable level), 0 (solved), 1 (solved to acceptable level). All other codes indicate various types of failures or early termination.

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: -1 (solved to acceptable level),
        0 (solved), 1 (solved to acceptable level). All other codes
        indicate various types of failures or early termination.
    """
    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
        # Acceptable IPOPT exit codes
        success = info["status"] in [-1, 0, 1]

        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	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	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	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 (additive): ``d_px_{joint}``, ``d_py_{joint}``,
  ``d_pz_{joint}``, ``d_phix_{joint}``, ``d_phiy_{joint}``,
  ``d_phiz_{joint}``
- Joint offset / calibration (additive): ``offsetPX_{joint}``,
  ``offsetPY_{joint}``, ``offsetPZ_{joint}``, ``offsetRX_{joint}``,
  ``offsetRY_{joint}``, ``offsetRZ_{joint}``
- Legacy offset (absolute): ``off_{joint}``
- Mass (absolute): ``m_{link}``
- First moments (absolute): ``mx_{link}``, ``my_{link}``, ``mz_{link}``
- Inertia tensor (absolute): ``Ixx_{link}``, ``Ixy_{link}``, ...,
  ``Izz_{link}``
- Viscous/static friction (absolute): ``fv_{joint}``, ``fs_{joint}``
- Armature (absolute): ``Ia_{joint}``
- Joint elasticity (additive): ``k_PX_{joint}``, ..., ``k_RZ_{joint}``

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

`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. %%s = {...} to export_urdf() "
            "and configure your\n"
            "  controller or calibration pipeline accordingly.",
            note,
        )

    return defaults

`export_urdf(nominal_urdf_path, params, *, output_path=None, verbose=False)` ¶

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`

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 an unknown parameter name is encountered.

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,
) -> 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.

    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 an unknown parameter name is encountered.
    """
    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 = {}

    for name, value in params.items():
        parsed = _parse_param_name(name)
        if parsed is not None:
            category, target, idx, is_additive = parsed
            handler = _HANDLERS.get(category)
            if handler is None:
                logger.warning("No handler for category '%s' (param='%s')",
                               category, name)
                continue
            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_*), inertia (Ixx_*).  "
            f"Metrology frame params: base_*, pEE*, phiEE*."
        )

    # 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())

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)` `dataclass` ¶

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)` `dataclass` ¶

Aggregated FK consistency check — compares nominal vs. exported URDF FK.

`PoseError(q, position_error_mm, orientation_error_deg, pose_delta, label='')` `dataclass` ¶

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

    import trimesh
    import viser
    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
    traj_parent = 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) ---
    static_parent = 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

Tools¶

Reporting & Verification¶

report¶

generate_calibration_report(calibrator, output_path=None, title=None) ¶

identification_report¶

generate_identification_report(identifier, output_path=None, title=None) ¶

compare_report¶

generate_compare_page(output_path=None, title=None) ¶

_report_common¶

ThresholdCheck(name, value, threshold, comparison, passed) dataclass ¶

VerificationVerdict(passed, checks, metrics, insights=list(), metadata=dict(), series=dict(), compat=dict()) dataclass ¶

evaluate_thresholds(metrics, thresholds) ¶

build_provenance_metadata(config_file_path, robot_name) ¶

provenance¶

collect_run_provenance(obj, task) ¶

run_archive¶

compute_run_dir(obj, root='results/runs') ¶

archive_run(obj, run_dir) ¶

Linear Solver¶

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) ¶

solve(A, b) ¶

solve_linear_system(A, b, method='lstsq', **kwargs) ¶

Robot Management¶

Robot(robot_urdf, package_dirs, isFext=False, freeflyer_ori=None, freeflyer_limits=None) ¶

num_joints property ¶

actuated_joint_names property ¶

freeflyer_limits property ¶

backend property ¶

update_freeflyer_limits(limits) ¶

display_q0(visualizer_type=None, q=None) ¶

get_configuration_bounds() ¶

validate_configuration(q) ¶

__repr__() ¶

create_robot(robot_urdf, package_dirs=None, loader='figaroh', enhanced_features=True, **kwargs) ¶

load_robot(robot_urdf, package_dirs=None, isFext=False, load_by_urdf=True, robot_pkg=None, loader='figaroh', **kwargs) ¶

get_available_loaders() ¶

list_available_robots(loader='robot_description') ¶

Regressor Builder¶

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) dataclass ¶

RegressorBuilder(robot, config=None) ¶

build_basic_regressor(q, v, a, identif_config=None) ¶

build_regressor_basic(robot, q, v, a, identif_config, tau=None) ¶

eliminate_non_dynaffect(W, params_std, tol_e=1e-06) ¶

get_index_eliminate(W, params_std, tol_e=1e-06) ¶

build_regressor_reduced(W, idx_e) ¶

build_total_regressor_current(W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, identif_config) ¶

build_total_regressor_wrench(W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, param) ¶

Robot Visualization¶

VisualizationConfig(com_color=None, axes_color=None, force_color=None, bbox_color=None, scale_factor=1.0) dataclass ¶

RobotVisualizer(model, data, viz, config=None) ¶

update_kinematics(q) ¶

display_com(q, frame_indices) ¶

display_axes(q, axis_length=0.15, axis_radius=0.01) ¶

display_force(force, placement, scale=0.001, name='force_vector') ¶

display_bounding_boxes(q, com_min, com_max, frame_indices) ¶

display_joints(q) ¶

place(viz, name, M) ¶

display_COM(model, data, viz, q, IDX) ¶

display_axes(model, data, viz, q) ¶

rotation_matrix_from_vectors(vec1, vec2) ¶

display_force(viz, phi, M_se3) ¶

display_bounding_boxes(viz, model, data, q, COM_min, COM_max, IDX) ¶

display_joints(viz, model, data, q) ¶

Robot Collisions¶

CollisionManager(robot, geom_model=None, geom_data=None, viz=None) ¶

setup_collision_pairs(srdf_model_path=None) ¶

check_collisions(q, update_geometry=True) ¶

get_collision_details() ¶

get_collision_distances(collision_details=None) ¶

get_all_distances() ¶

print_collision_pairs() ¶

visualize_collisions(collision_details=None) ¶

CollisionWrapper(robot, geom_model=None, geom_data=None, viz=None) ¶

add_collisions() ¶

remove_collisions(srdf_model_path) ¶

computeCollisions(q, geom_data=None) ¶

getCollisionList() ¶

getCollisionDistances(collisions=None) ¶

getDistances() ¶

getAllpairs() ¶

`generate_calibration_report(calibrator, output_path=None, title=None)` ¶

`generate_identification_report(identifier, output_path=None, title=None)` ¶

`generate_compare_page(output_path=None, title=None)` ¶

`ThresholdCheck(name, value, threshold, comparison, passed)` `dataclass` ¶

`VerificationVerdict(passed, checks, metrics, insights=list(), metadata=dict(), series=dict(), compat=dict())` `dataclass` ¶

`evaluate_thresholds(metrics, thresholds)` ¶

`build_provenance_metadata(config_file_path, robot_name)` ¶

`collect_run_provenance(obj, task)` ¶

`compute_run_dir(obj, root='results/runs')` ¶

`archive_run(obj, run_dir)` ¶

`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)` ¶

`solve(A, b)` ¶

`solve_linear_system(A, b, method='lstsq', **kwargs)` ¶

`Robot(robot_urdf, package_dirs, isFext=False, freeflyer_ori=None, freeflyer_limits=None)` ¶

`num_joints` `property` ¶

`actuated_joint_names` `property` ¶

`freeflyer_limits` `property` ¶

`backend` `property` ¶

`update_freeflyer_limits(limits)` ¶

`display_q0(visualizer_type=None, q=None)` ¶

`get_configuration_bounds()` ¶

`validate_configuration(q)` ¶

`repr()` ¶

`create_robot(robot_urdf, package_dirs=None, loader='figaroh', enhanced_features=True, **kwargs)` ¶

`load_robot(robot_urdf, package_dirs=None, isFext=False, load_by_urdf=True, robot_pkg=None, loader='figaroh', **kwargs)` ¶

`get_available_loaders()` ¶

`list_available_robots(loader='robot_description')` ¶

`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)` `dataclass` ¶

`RegressorBuilder(robot, config=None)` ¶

`build_basic_regressor(q, v, a, identif_config=None)` ¶

`build_regressor_basic(robot, q, v, a, identif_config, tau=None)` ¶

`eliminate_non_dynaffect(W, params_std, tol_e=1e-06)` ¶

`get_index_eliminate(W, params_std, tol_e=1e-06)` ¶

`build_regressor_reduced(W, idx_e)` ¶

`build_total_regressor_current(W_b_u, W_b_l, W_l, I_u, I_l, param_standard_l, identif_config)` ¶

`build_total_regressor_wrench(W_b_u, W_b_l, W_l, tau_u, tau_l, param_standard_l, param)` ¶

`VisualizationConfig(com_color=None, axes_color=None, force_color=None, bbox_color=None, scale_factor=1.0)` `dataclass` ¶

`RobotVisualizer(model, data, viz, config=None)` ¶

`update_kinematics(q)` ¶

`display_com(q, frame_indices)` ¶

`display_axes(q, axis_length=0.15, axis_radius=0.01)` ¶

`display_force(force, placement, scale=0.001, name='force_vector')` ¶

`display_bounding_boxes(q, com_min, com_max, frame_indices)` ¶

`display_joints(q)` ¶

`place(viz, name, M)` ¶

`display_COM(model, data, viz, q, IDX)` ¶

`display_axes(model, data, viz, q)` ¶

`rotation_matrix_from_vectors(vec1, vec2)` ¶

`display_force(viz, phi, M_se3)` ¶

`display_bounding_boxes(viz, model, data, q, COM_min, COM_max, IDX)` ¶

`display_joints(viz, model, data, q)` ¶

`CollisionManager(robot, geom_model=None, geom_data=None, viz=None)` ¶

`setup_collision_pairs(srdf_model_path=None)` ¶

`check_collisions(q, update_geometry=True)` ¶

`get_collision_details()` ¶

`get_collision_distances(collision_details=None)` ¶

`get_all_distances()` ¶

`print_collision_pairs()` ¶

`visualize_collisions(collision_details=None)` ¶

`CollisionWrapper(robot, geom_model=None, geom_data=None, viz=None)` ¶

`add_collisions()` ¶

`remove_collisions(srdf_model_path)` ¶

`computeCollisions(q, geom_data=None)` ¶

`getCollisionList()` ¶

`getCollisionDistances(collisions=None)` ¶

`getDistances()` ¶

`getAllpairs()` ¶

`check_collision(q)` ¶

`displayCollisions(collisions=None)` ¶

`QRResult(rank, base_indices, pivot_order, W_b, beta, M, base_param_expressions, phi_b, phi_b_nom, method, diag_R, cond_R1)` `dataclass` ¶

`QRDecomposer(tolerance=TOL_QR, beta_tolerance=TOL_BETA, relative_tolerance=None)` ¶

`get_M()` ¶

`get_M_labels()` ¶

`get_base_indices()` ¶

`get_diagnostics()` ¶

`decompose(W_e, params_r, tau=None, params_std=None, method='double')` ¶

`decompose_with_pivoting(tau, W_e, params_r)` ¶

`double_decomposition(tau, W_e, params_r, params_std=None)` ¶

`compute_nominal_base_parameters(base_params, regroup_params, beta, params_std)` ¶

`get_base_mapping_matrix_pivoting(W_e, params_r)` ¶