Payload-dependent arm sag during manipulation: URDF-based gravity compensation does not account for grasped objects

[b-is-wash-k]

I am running VLA-based manipulation tasks on OpenArm hardware. The VLA predicts desired arm joint positions and gripper positions, while gravity compensation is calculated using KDL ChainDynParam from the robot’s URDF model.

This works correctly when the robot is unloaded or handling very lightweight objects. However, after the robot grasps a heavier object, one or more arm joints sag below their commanded positions.

Tasks tested

I observed the behavior across three VLA manipulation tasks.

1. Lightweight cube pick-and-place

The robot picks up a lightweight cube and places it at the target location.
For this task, the existing Pinocchio gravity compensation based on the URDF works reasonably well. There is no significant joint sag because the cube payload is small.

2. Bimanual rectangular-box transfer

The robot transfers a rectangular box from one arm to the other.
During this task, noticeable sagging occurs while one arm is supporting the box. The behavior can also become more noticeable during the transfer because the payload distribution between the two arms changes.
The commanded joint positions remain approximately the same, but the measured joint positions drop under the additional load.

3. Steel-wrench pick-and-place

The robot picks up different steel wrenches weighing approximately 300–700 g and places them in a tray.
After grasping and lifting the wrench, the arm consistently sags. The amount of sag generally becomes more noticeable with the heavier payloads.

Setup / context

I'm running VLA-based (GR00T) manipulation on a bimanual OpenArm v10 hardware setup, on Ubuntu 24.04 with ROS 2 Jazzy, built on the OpenArm ROS 2 stack (openarm_bringup, openarm_hardware, openarm_can, openarm_description).

Control flow:

GR00T policy (workstation, 15 fps)
    ↓ predicted arm joint positions (7 per arm) + gripper positions
deploy client
    ↓ /joint_trajectory
joint_trajectory_relay
    ↓ /left_joint_trajectory_controller/joint_trajectory
    ↓ /right_joint_trajectory_controller/joint_trajectory
ros2_control (openarm_hardware)
    ↓ MIT PD mode per joint:  kp(q_cmd − q) + kd(v_cmd − v) + τ_ff
    │   kp = [90, 90, 70, 60, 10, 10, 10]
    │   kd = [3.1, 2.8, 2.0, 2.0, 0.7, 0.6, 0.5]
CAN bus → Damiao motors

Gravity compensation (separate node, runs in parallel):
/joint_states
    ↓ KDL ChainDynParam gravity torques from URDF (v10_bimanual.urdf)
    ↓ global scale (0.5) × per-joint scale_joints [1.0,1.0,0.8,1.1,1.0,1.0,1.0]
    ↓ low-pass filter (alpha) + soft-start ramp
    ↓ /left|right_joint_group_effort_controller/commands   (feed-forward τ_ff)

Gravity comp launch:

ros2 run controller_manager spawner \
    right_joint_group_effort_controller left_joint_group_effort_controller \
    -c /controller_manager
ros2 run openarm_gravity_comp bimanual_gravity_comp_node --ros-args \
    -p urdf_path:=$HOME/OPEN_ARM/urdf/v10_bimanual.urdf \
    -p publish_effort:=True -p scale:=0.5 -p alpha:=1.0 -p ramp_time:=2.0 \
    -p scale_joints:="[1.0,1.0,0.8,1.1,1.0,1.0,1.0]"

Training data is collected by XR teleoperation (head + both wrist cameras) into a LeRobot-format bimanual dataset; the policy is trained on and deployed with --fps 15 --action-horizon 0.15 --exec-steps 16 --rtc.

The issue

This works correctly when the robot is unloaded or handling very lightweight objects. However, after the robot grasps a heavier object, one or more arm joints sag below their commanded positions. The commanded joint positions remain approximately the same, but the measured joint positions drop under the additional load. Since the gravity feed-forward is computed from the URDF alone, it knows nothing about the payload, and the PD position loop (especially the wrist joints with kp = 10) is too soft to absorb the extra torque.

Questions

1. What is the recommended way to handle a grasped payload with URDF-based gravity compensation — e.g., dynamically adding a payload mass/CoM at the end-effector link in the dynamics model when the gripper closes? Is there an existing OpenArm mechanism or example for this?
2. Alternatively, should I simply raise the MIT PD stiffness (kp/kd), particularly on the wrist joints (currently kp = 10), so the position loop absorbs the payload disturbance instead of trying to make the feed-forward exact? Are there recommended safe gain ranges for the Damiao motors under payload?
3. Has anyone done online payload estimation on OpenArm (estimating mass/CoM from joint-torque residuals after grasping), or is there a simpler standard practice?
4. For the bimanual transfer case, where the load shifts between the two arms mid-task, is there a recommended approach beyond per-arm payload estimation?

Please let me know if you need more details about my setup (URDF, controller configs, recordings). Thanks in advance!

Can anyone guide me on my approach ? How should i do it ? what's my mistake here and how to solve it?

[abetomo]

Discord: https://discord.com/channels/1339899404546740275/1527729227854516425/1527729227854516425

[abetomo]

Would this be helpful as a reference: enactic/openarm_ros2#72 ?

[Maaz-qureshi98]

I run VLA policies on OpenArm too, and this pattern is expected with your setup. Here's what I'd do, in order.

1. Fix the unloaded feed-forward first (scale: 0.5)

With a global scale of 0.5, the PD loop is already holding half of the arm's own weight before any payload is added. Each joint then settles at an offset of roughly Δq ≈ τ_missing / kp. Rough numbers with your gains:

• Wrist (kp = 10): 0.5 kg at ~0.10 m → 0.49 N·m → ~0.05 rad (≈ 3°)
• Shoulder (kp = 90): 0.5 kg at ~0.45 m → 2.2 N·m → ~0.025 rad, which is about 1 cm at the TCP

So the payload sits on top of an error that's already there. Rather than tuning scale / scale_joints by hand, hold the arm in 10–20 static poses, log the Damiao torque estimate against the KDL gravity torque, and fit the per-joint scale by least squares. If the fit lands far from 1.0, the URDF inertials or friction don't match your hardware, and the payload term won't line up either.

2. Add the payload to the feed-forward when the gripper closes (Q1)

The sign-safe way is a Jacobian term rather than editing link inertias:

τ_payload(q) = J_v(q)ᵀ · F,   F = (0, 0, m·g)

Here J_v is the linear Jacobian at the grasp point (the TCP is fine). F is the upward force the arm must supply, expressed in your KDL chain's root frame. If the chain is rooted at world / openarm_body_link0, then F = (0, 0, m·g). If it's rooted at openarm_left_link0, rotate it first, because the arm mounts are rolled ±90°.

In KDL, append a fixed TCP segment to the chain, use ChainJntToJacSolver, take the top three rows, and add the result to your gravity torques. Ramp m in over ~0.3 s after the gripper closes and back out when it opens, so you don't step the motors.

3. Estimate m online instead of hard-coding it (Q3)

After the lift, while the arm is near-static (|q̇| small), average the residual over ~0.3–0.5 s:

r = τ_measured − τ_gravity(q)
m̂ = (bᵀ r) / (bᵀ b),   b = J_v(q)ᵀ · (0, 0, g)

Use the shoulder and elbow joints for this. They have the lever arm and the best signal-to-noise; the wrist DM4310 torque estimates are noisy. Clamp m̂ to a sane range, and reset it to 0 when the gripper opens. A point mass at the TCP is good enough for the 300–700 g wrenches. CoM offset matters much less than mass at these scales.

4. Gains (Q2)

Raising wrist kp helps, but it fights a modelling error instead of removing it. With 15 Hz inference and 16-step chunks, stiffer wrists also make chunk boundaries more visible. I'd fix 1–3 first, then raise wrist kp moderately and re-tune kd toward critical damping (kd ≈ 2√(kp·I_eff)). Step it up while watching motor temperature and any oscillation at chunk switches. I wouldn't trust a single "safe" number across units.

5. Bimanual handover (Q4)

Run the estimator independently per arm, and keep it running with a slow low-pass (~0.5 s) while both grippers are closed. Each arm's residual then tracks its own share of the load as it shifts, without assuming a fixed split.

One more thing worth checking: if your XR teleop demos were recorded on a follower that also sagged, the actions in the dataset are leader poses. That means the policy has learned around the sag to some extent. After you add payload compensation, a short re-collection or fine-tune on a few heavy-object episodes may help.