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?