robotiq_description: sim goals abort as stalled when the simulator publishes no joint velocities

[ebarnett3]

What happened

With sim_isaac:=true, every gripper_cmd goal that takes longer than 0.5 s is ABORTED with stalled: true / reached_goal: false when the simulator's JointState carries no velocity array, even though the fingers are moving and reach the commanded position.

Regression from #52 (912cd2f): config/robotiq_controllers.sim.yaml and the Humble variant set allow_stalling: false so that a genuine stall on an object aborts instead of succeeding. But parallel_gripper_action_controller decides "stalled" purely from the velocity state: outside goal_tolerance, it resets its timer only while |velocity| > stall_velocity_threshold (0.05) and marks the goal stalled once stall_timeout (0.5 s) elapses without that. topic_based_ros2_control leaves the velocity state at 0 when the incoming JointState.velocity is empty, so on such a simulator the timer never resets and the goal aborts at 0.5 s regardless of the fingers' progress. Before 912cd2f the same goals were reported SUCCEEDED (mislabelled, as in #29); now they fail outright.

Reproduced on Jazzy with a stand-in simulator that tracks /isaac_joint_commands at 0.5 rad/s and publishes /isaac_joint_states (2F-85, closed → 0.1 rad, a 1.4 s move):

simulator publishes	result
positions only	ABORTED, stalled: true, reached_goal: false after ~0.5 s
positions + velocities	SUCCEEDED, reached_goal: true

Isaac Sim's joint-state publisher includes velocities by default, which is why #52's live test passed; any simulator, bridge or recording that omits them hits this. The velocity state interface is mandatory in the controller (on_activate errors without it), so it cannot be dropped from state_interfaces to sidestep the detector.

Related: #29 (driver reports velocity 0 → same detector misfires on hardware; the gOBJ route proposed there does not exist on the sim path).

ROS distro

Jazzy

Hardware

sim_isaac:=true with topic_based_ros2_control 0.3.0, no gripper

Setup

source build on Ubuntu, ~/robotiq/devel/ros at 84c0ffc (main after #52)

Exact command

ros2 launch robotiq_description robotiq_control.launch.py sim_isaac:=true gripper_model:=2f_85
ros2 action send_goal /robotiq_gripper_controller/gripper_cmd control_msgs/action/ParallelGripperCommand "{command: {name: ['robotiq_85_left_knuckle_joint'], position: [0.1]}}"

Console output

stalled: true
reached_goal: false
Goal finished with status: ABORTED

[jlongvalRobotiq]

Suggested fix

Why it happens. The gripper controller detects a stall from joint velocity only: if velocity stays under stall_velocity_threshold for stall_timeout while the goal is not reached, the goal is marked stalled. topic_based_ros2_control writes 0 into the velocity state when the incoming JointState has no velocity array, so on such a simulator the controller sees "not moving" from the first cycle and aborts every goal longer than 0.5 s. Before #52 the same goals came back as a fake success (#29); now they come back as a fake failure. Isaac Sim publishes velocities by default, which is why the live test passed.

Why not a config change. allow_stalling: true restores the fake success. A longer timeout only delays the abort. The velocity interface can't be dropped from the controller, it refuses to activate without it. Marking the missing velocity as NaN (the plugin keeps the URDF initial_value when no velocity arrives) doesn't help either: the controller's fabs(velocity) > threshold is false for NaN, so NaN counts as "not moving" exactly like 0. Making the controller skip stall detection on NaN would be an upstream ros2_controllers change.

Proposal. A small relay node, started by the launch only under sim_isaac:=true, between the simulator's JointState topic and the plugin. Messages that already carry velocities pass through unchanged. Messages without them get velocities estimated by finite difference on position over the message timestamps. The plugin's joint_states_topic points at the relay output. Real hardware and the mock are untouched. A launch argument turns the relay off for simulators known to publish velocities.

Verification would be the stand-in from this issue: positions only → SUCCEEDED with reached_goal: true, plus unit tests on the estimator and the launch wiring.

@ebarnett3 what do you think?

[jlongvalRobotiq]

One refinement after re-reading #29, which argues against differentiating position.

#29's objection is about the driver: gPO is an 8-bit register (~0.003 rad per count) read at 100 Hz, so most cycles show no change and the occasional count flip turns into a velocity spike. A simulator publishes floating-point positions from its integrator, so at 60 Hz and ~0.5 rad/s every message moves ~0.008 rad and a finger stopped on an object drops to solver jitter, far under 0.05 rad/s. The derivative is smooth where #29's is stepwise, and the gOBJ route #29 prefers has no equivalent on the topic-based path.

It still bites for a source that relays quantized positions (a twin mirroring the real driver's joint states) or publishes at a low/irregular rate. So the relay should not be a raw two-sample difference: it would average the estimate over a short window (5 messages at 60 Hz, under 100 ms of lag), so a single count flip cannot clear the threshold on its own. That is the "filter the derivative and accept the lag" option #29 lists.

[ebarnett3]

One refinement after re-reading #29, which argues against differentiating position.

#29's objection is about the driver: gPO is an 8-bit register (~0.003 rad per count) read at 100 Hz, so most cycles show no change and the occasional count flip turns into a velocity spike. A simulator publishes floating-point positions from its integrator, so at 60 Hz and ~0.5 rad/s every message moves ~0.008 rad and a finger stopped on an object drops to solver jitter, far under 0.05 rad/s. The derivative is smooth where #29's is stepwise, and the gOBJ route #29 prefers has no equivalent on the topic-based path.

It still bites for a source that relays quantized positions (a twin mirroring the real driver's joint states) or publishes at a low/irregular rate. So the relay should not be a raw two-sample difference: it would average the estimate over a short window (5 messages at 60 Hz, under 100 ms of lag), so a single count flip cannot clear the threshold on its own. That is the "filter the derivative and accept the lag" option #29 lists.

Yes I agree that we should have a velocity estimation method that supports all of the use cases. The proposed method of smoothing a position difference makes sense. We will need to test it on a real gripper at max speed and min speed.

[ebarnett3]

Not fixed by #100: the topic-based path has no object status, so the velocity stall check still decides there. With the real gripper decided by the gripper itself, this path is the only one left on the velocity check, and it drops to low priority. If it gets fixed, the velocity belongs to the simulator publishing the joint states, not to a relay in ROS.