# Force Reconstruction (Wrench Estimator)

The Wrench Estimator reconstructs the external wrench (force + torque) applied at
each foot **from the measured actuator forces alone** — no foot force/torque
sensor required. It is provided by the `kangaroo_constrained_dynamics_ros`
package, built on top of the [Kangaroo Full Model](../robot_models/full.md):
the same constrained-dynamics formulation that "opens" every closed linkage and
re-imposes it as a mechanical constraint is used here to reconstruct the full
76-DOF leg configuration from the 6 actuated joints, and then to run inverse and
forward dynamics on it.

It is mainly used to visualize and validate the estimated foot wrench against a
real FT sensor (RViz overlay) and, from that comparison, to calibrate the
actuators' internal force sensors.

## Overview

| Parameter | Value |
|---|---|
| Package | `kangaroo_constrained_dynamics_ros` |
| Core library | [`kangaroo_constrained_dynamics`](../robot_models/full.md) (constrained forward/inverse dynamics on the full model) |
| Executable / node | `wrench_estimator` |
| Model used | [Kangaroo Full Model](../robot_models/full.md) (per leg, fixed base) |
| Input | Actuated-joint positions and forces, from `pal_statistics` |
| Output | Estimated external wrench at each foot |

## How the Estimate is Computed

For each enabled leg, on every `/actuators_statistics/values` message the node:

1. reads the 6 actuated-joint positions and forces by key and maps them into the
   model's convention;
2. **reconstructs the full leg configuration `q`** (all 76 DOFs) from the 6
   actuated joints, by solving the closed-loop constraints of the full model;
3. runs the constrained forward dynamics, `f_ext = forward(q, expand_tau(tau_act))`,
   to recover the external wrench consistent with the measured actuator forces;
4. rotates the result into the leg's output frame and publishes it.

`````{admonition} Failure policy
:class: note
On missing/non-finite statistics, statistics names not yet received, a
non-converged kinematics solve, or any NaN/Inf in the result, the node logs a
throttled warning and skips that leg's cycle (nothing is published for it that
cycle).
`````

## Topics

| Topic | Type | Description |
|---|---|---|
| `/actuators_statistics/names` | `pal_statistics_msgs/StatisticsNames` | Subscribed. Actuator statistics key names. |
| `/actuators_statistics/values` | `pal_statistics_msgs/StatisticsValues` | Subscribed. Actuator statistics values (positions, forces). |
| `/wrench_estimator/left/wrench` | `geometry_msgs/WrenchStamped` | Published. Estimated external wrench at the left foot. |
| `/wrench_estimator/right/wrench` | `geometry_msgs/WrenchStamped` | Published. Estimated external wrench at the right foot. |

## Frames

The wrench is first computed in a **world-aligned** frame
(`ee_reference_frame: LOCAL_WORLD_ALIGNED`) — i.e. in the model's world axes,
which for the fixed-base leg model coincide with `base_link`
(`reference_frame`) — and then rotated via `tf2` into each leg's
`output_frame` (default `ankle_<leg>_ft_sensor_link`). Since the FT frame is
co-located with the foot, this projection is a plain rotation, so the estimate
lands in the same frame the real FT sensor uses and overlays directly on it.
Set `left_output_frame` / `right_output_frame` to `""` to instead publish the
world-aligned wrench in `reference_frame`.

## Launching

```bash
ros2 launch kangaroo_constrained_dynamics_ros wrench_estimator.launch.py
```

Both legs run from one shared model by default. Estimates start flowing as soon
as `/actuators_statistics/*` is available.

## Configuration

| Parameter | Default | Meaning |
|---|---|---|
| `legs` | `["left", "right"]` | Which legs to estimate; e.g. `["left"]` for one. |
| `gravity` | `true` | Include gravity in the dynamics. |
| `left_output_frame` / `right_output_frame` | `ankle_<side>_ft_sensor_link` | TF frame the wrench is published in (`""` keeps it world-aligned in `base_link`). |
| `stats_names_topic` / `stats_values_topic` | `/actuators_statistics/{names,values}` | PAL statistics topic pair. |
| `left_model_config` / `right_model_config` | `""` | Core leg-config path; empty uses the package default. |
| `reference_frame` | `base_link` | TF frame whose axes the world-aligned wrench corresponds to. |
| `ee_reference_frame` | `LOCAL_WORLD_ALIGNED` | Pinocchio reference frame the wrench is computed in. |

Defaults live in `config/wrench_nodes.yaml`. Point the launch at a different file
with `config:=/path/to/params.yaml`.

## Visualizing in RViz

```bash
ros2 launch kangaroo_constrained_dynamics_ros wrench_estimator.launch.py rviz:=true
```

This opens RViz preloaded with four `WrenchStamped` displays on a `base_link`
fixed frame — the estimated wrench per leg (green) and the real FT-sensor wrench
per leg (blue, from `/ankle_ft_{left,right}_controller/wrench`, shown only when
published) — so the estimate and the ground-truth measurement overlay for direct
comparison:

<div style="display: flex; gap: 10px; margin-bottom: 20px;">
  <img src="../../_static/images/wrench_estimator_rviz_overview.png" alt="Estimated vs FT-sensor wrench in RViz — overview" height="400"/>
  <img src="../../_static/images/wrench_estimator_rviz_detail.png" alt="Estimated vs FT-sensor wrench in RViz — single foot detail" height="400"/>
</div>

Standalone, against an already-running estimator:

```bash
rviz2 -d $(ros2 pkg prefix kangaroo_constrained_dynamics_ros)/share/kangaroo_constrained_dynamics_ros/config/wrench_estimator.rviz
```

`````{admonition} FT sensor as ground truth
:class: important
The FT-sensor wrench is only a meaningful comparison once the FT sensor itself
has a trustworthy zero (warmed up, calibrated unloaded) — see
[FT Sensor Calibration](../robot_usage_guide/ft_sensor_calibration.md).
`````