Debug a collision

The arm reaches the destination at low speed but clips an obstacle at high speed. The planner found a collision-free path both times — so the problem is not planning, it is tracking.

Confirm it is a tracking problem

move_arm runs a trajectory collision monitor for the duration of every go_destination and verify_trajectories call. It samples /check_robot_collision at 30 Hz and logs on the edges — when a collision starts and when it clears — naming both bodies:

collision started: arm_link_3 <-> shelf_01
collision cleared after 0.43 s

If the monitor reports collisions during execution but the plan validated clean, the arm is not following the trajectory it was given.

Compare commanded against actual

The signature of a tracking failure is actual joint acceleration far above what was commanded. The plugin drives joints through a P-controller, and when the profile demands more than it can deliver the arm swings wide of the planned path — through whatever the planner had routed around.

Look for:

  • actual joint acceleration exceeding the commanded value by a wide margin

  • overshoot concentrated in the joints that move furthest

  • the deviation appearing only at the higher speed profile

Fixes, in order of preference

Lower the speed profile. Velocity and acceleration limits live in parameterization_config.json. If the profile asks for more than the arm can deliver, nothing downstream can rescue it.

Use kinematic drive in simulation. ArmPlugin accepts a drive_mode in its SDF block:

<plugin filename="libArmPlugin.so" name="ArmPlugin">
  <drive_mode>kinematic</drive_mode>
</plugin>

velocity (the default) commands joint velocities and lets the physics solver integrate them, so tracking error is real and speed-dependent. kinematic applies the commanded joint positions directly, so the simulated arm follows the trajectory exactly. Use kinematic when you are testing application logic and the arm’s dynamics are not what you are studying; use velocity when tracking fidelity is the point.

Note

kinematic makes the simulated arm better than the real one. A trajectory that only works kinematically will still fail on hardware — so validate the final speed profile in velocity mode.

Re-check the collision geometry. If the monitor names a pair you did not expect, the collision model may not match the visual model.