Graph View

Changelog

gravity-inertial-compensation

9 min read

Gravity + inertial compensation — where we actually stand

Status: investigation only, nothing changed. Deliberately not folded into the RFC yet.

Goal: know what compensation the stack really performs before deciding whether it deserves a phase of its own.

  • RFC: Force-Based Movements — mine, DRAFT, approved by Lou Graniou 2026-08-06. The RFC currently says, under Scope:

    Gravity and inertia. We do not model these forces. We remove them by measurement.

  • The feedback is an unresolved inline comment by Michelle ([email protected]) on that exact line, 2026-08-05:

    this is required to do actually good force sensing and one of the most serious drawbacks of our system right now. Direct link: https://app.notion.com/p/37b1b2bda0bb81b0b2c0ce4148694f62?d=3b31b2bda0bb80b7b57c001ce0559e53&pvs=42#656d2183034648d7b603d5dc3b9bd268

  • Phase 0 in the RFC’s Implementation Roadmap is currently just “basic F/T collection scripts merged”. The open question is whether to widen it to include gravity/inertia characterization.
  • [WIP] RFC: Hand-Guided Calibration — Aarjav’s admittance-mode doc, branch aarjavjain/handguid-experiment. Read this before writing anything. Its Step 0 is already built and sim-validated and measures precisely the residual in question (scripts/data_collection/ft_orientation_sweep.py + _ft_capture.py, built on my merged saahas/ft-experiments / PR #10460). Two other unresolved comments on my RFC, both from Lou: deck-wide time sync is out of scope but quantifying clock skew is worth doing, and “are we accounting for all 6 DOF of the EEF or a subset?” on the retry-policy section.

TL;DR

The RC8 compensates the arm well and our URDFs have nothing to do with it. The tool is compensated through a two-entry table selected by an integer, and a grasped plate/tip-rack/bottle is not compensated at all. The knob we want (per-payload mass + COG + auto-measured MOI) already exists on the controller; we use two of its slots.

1. What the robot does

Arm self-weight and inertia: handled by the RC8’s own model. Our URDFs are not in that loop. Evidence it does inertia and not merely gravity: the controller exposes a payload moment of inertia setting distinct from mass and COG. A gravity-only controller needs mass + COG and nothing more — an MOI field only earns its place in an inertia feedforward term. (Inference, not read from an official Denso manual.)

Tool: one integer wide.

// medra_bcap/src/DensoController.cpp:413
BCAP_HRESULT DensoReadWriteDriver::SetTcpLoad(const int32_t tool_value)
    // tool_value = 1; // ROBOTIQ_2F85_GRIPPER_PAYLOAD
    // tool_value = 2; // ROBOTIQ_2F140_GRIPPER_PAYLOAD
    bCap_RobotGetVariable(iSockFD, lhRobot, "@CURRENT_TOOL", "", &lhVar);

TCPPayload(mass=1.025, com=(0,0,73)) at medra_robotics/arm_control/robot_constants.py:154 is never transmitted. It records what somebody typed into a pendant once — per robot, unverified across the fleet, unverifiable from code. Two slots, both gripper-only.

Grasped objects: not compensated. Nothing re-registers a payload on grasp. The cost is already in the tree:

# medra_robotics/task_execution/behaviors/bottle_behaviors.py:252
# The grasped 45deg tool + bottle weight is not gravity-compensated (the bottle
# Grasp passes no TCP payload), so that uncompensated hanging load shows up as a
# standing Z force. A 10 N limit trips "contact" from gravity alone...

10 N → 30 N to absorb a standing load, i.e. a 3× loss of contact sensitivity. Against the sigma ≈ 0.2–0.35 N noise floor that is ~100 sigma of headroom given away to a computable term.

F/T is model-compensated and re-tared — and there is no load cell. Confirmed by hardware inspection (hand-guide RFC, 2026-06-27): there is no external wrist F/T puck. forceValue mode 13 is the RC9’s estimate of the external TCP wrench, computed from drive/joint torque plus the controller’s own dynamic model, already gravity-compensated through the registered payload. On top of that, ForceSensor("0") re-tares before each force-limited servo trajectory (DensoController.cpp:610), which zeroes the remaining bias in the tare pose only — as the tool reorients, the residual rotates in the sensor frame and the cancellation decays like |g| m (1 - cos dtheta). Fine for a straight tool-z approach, wrong for the 45 deg bottle.

Two consequences that sharpen everything below:

  • Do not subtract gravity in software. The controller already does it; a second subtraction double-compensates. What is worth measuring is the residual of the controller’s compensation.
  • The grasped-object gap is not a missing layer — it is a wrong number inside an existing one. The RC8/RC9 is compensating for whichever of two gripper-only payloads is registered while the arm carries a plate. That is a model error we feed the controller, not an absent model.

A motor/model-based estimate is also noisier and more friction- and hysteresis-prone than a load cell, which is consistent with the measured sigma ~ 0.2–0.4 N and the 3–5x per-deck spread. So the sensor we would be “fixing” with a better payload model is itself the weak link — worth sizing before promising much.

The knob already exists. RC8 [Easy Setting] -> Payload stores mass (integer grams), COG vector, and moment of inertia, and MOI has an [Automatic measure] routine that identifies it by moving the arm. The table is per-tool-number, so “gripper + plate” and “gripper + tip rack” can each be their own entry, selected on grasp. That is config plus a set_tcp_load call — not a new dynamics layer.

2. What the URDFs contain (and one thing we shipped wrong)

Every moving link has <inertial> with mass, COM, and an inertia tensor. Three gaps:

  1. Diagonal-only inertias on all six Cobotta links — ixy = ixz = iyz = 0. That asserts each link’s COM frame is its principal frame, true only under two planes of mirror symmetry through the COM. A cast link with an offset motor and cable routing doesn’t have them. xArm6’s URDF does carry off-diagonals, so this isn’t our house convention — the Cobotta numbers were transcribed from a datasheet table, not exported from CAD. Harmless for gravity torque (needs only m, COM); wrong for anything with omega_dot or omega x I omega.
  2. link_base is a placeholder — m = 1, I = identity (assets/urdf/cobotta_pro/cobotta_pro.urdf:26). Contributes zero generalized torque (fixed to world), but shows nobody has audited the file.
  3. Nothing past link6, no payload anywhere. end_effector and tool_frame have no <inertial>. No tip rack, plate, lid, bottle, or pipette appears in any URDF.

The shipped error: assets/curobo/cobotta_pro/cobotta_pro_2f140.urdf’s gripper chain is MuJoCo Menagerie’s 2F-85 numbers, rounded:

linkoursMenagerie 2f85.xml
gripper_base_link0.7770.777441
driver0.0090.00899563
coupler0.0140.0140974
spring_link0.0220.0221642
follower0.0130.0125222
finger0.01, I = 1e-6 * identitypad 0.0035

Menagerie stores inertia as (principal diaginertia + quat to the principal frame). The conversion kept the diaginertia and dropped the quat — which is the claim that the COM frame is principal-aligned. That is where the off-diagonals went. Sum: 0.913 kg against the 2F-140 datasheet 1.025 kg, and it is the wrong gripper’s linkage.

Nothing consumes these for dynamics: cuRobo reads the URDF for kinematics + collision only, and MuJoCo scene objects get a hardcoded 0.2 kg / fixed diaginertia (simulation/urdf_tools/mjcf_helpers.py:241).

3. Starting point for the gripper mass matrix

Two sources, each incomplete in a different way:

  • ROS-Industrial robotiq_2f_140_gripper_visualization — correct 140 linkage, full off-diagonal tensors. Sums to 0.364 kg because its base_link (0.2265) is the shell with no drive unit. Take its finger chain: per side outer_knuckle 0.00853, outer_finger 0.02261, inner_finger 0.01040, inner_knuckle 0.02712.
  • Menagerie 2f85 — sums to 0.900 against a 0.925 datasheet, so its base (0.777) is a complete drive unit. The 85 and 140 share that body; the ~0.1 kg delta is the longer linkage.

Composite: 0.777 + 2(0.0687) = 0.914 kg, 0.111 short of datasheet. Scale all masses by 1.025/0.914 = 1.12 — uniform scaling leaves the COM exactly where it is (the only thing gravity comp reads) and scales I linearly. Alternative is dumping the residual in the base if you believe it is fasteners and cable at the mount. It is ±0.1 kg on a 1 kg tool, and the RC8 auto-measure moots the argument for the MOI half anyway.

Custom fingers

assets/curobo/cobotta_pro/meshes/gripper_finger.STL is watertight, so trimesh (already a dep) gives it up directly. Per unit mass, about its own COM (at y = -5.80 mm, z = 18.52 mm, mesh frame):

I/m = diag(1.148e-4, 1.541e-4, 3.978e-5)  m^2      V = 1.303 cm^3

Mass is then a single scalar knob — weigh a finger and multiply. At 6061 (2700 kg/m^3): 3.5 g, giving I = diag(4.0e-7, 5.4e-7, 1.4e-7) kg m^2.

Reproduce with:

import trimesh
m = trimesh.load('medra_robotics/assets/curobo/cobotta_pro/meshes/gripper_finger.STL')
m.density = 1.0                       # unit density -> per-kg quantities
print(m.center_mass, m.moment_inertia / m.mass)

Do not trust that 3.5 g. 22 x 1.6 x 37.5 mm is a flat blade, i.e. a collision proxy rather than a machined finger — its computed diagonals match the uniform-plate (b^2 + c^2)/12 formula to three digits, and 3.5 g is suspiciously exactly Menagerie’s pad mass. Off-diagonals came out at 1e-16 relative, so for this geometry diagonal-only is honest; that will not survive the real part.

Next steps (in this order)

  1. Talk to Aarjav before writing any characterization code. The hand-guide RFC’s Step 0 sweep (orientation-diverse static poses, dwell-read over a second read-only b-CAP connection, Welford mean+std, gravity direction from FK) already measures the residual, is sim-validated, and is built on my own ft-experiments tooling. If Phase 0 grows to cover gravity/inertia, it should adopt that script rather than grow a parallel one. Their pending item is the fit/analysis plus a real-deck run — that is the piece to pick up.
  2. Weigh the gripper with the real custom fingers mounted. One number settles the 0.111 kg that no URDF will.
  3. Enter mass + COG in an RC8 payload slot, run [Automatic measure] for MOI. Controller-identified beats any CAD tensor, and it is the only path that buys inertia comp rather than gravity comp. Needs a human at the pendant with the deadman switch.
  4. Register per-grasp payload entries (gripper+plate, gripper+tip rack, gripper+bottle) and call set_tcp_load on grasp. This is the actual fix for the bottle 30 N workaround.
  5. Fix the URDFs last, as documentation of what is in the controller. Nothing reads them for dynamics, so starting there buys nothing.

Open question worth answering before step 3: whether we want a write path from TCPPayload to the controller (b-CAP Payload command takes grams + a COG vector) so the Python constants stop being fiction, or whether the pendant stays the source of truth and Python just selects indices.

Sources

Recent Notes

Graph View