Skip to content

fix: reset the CartesianController state on activation and keep the realtime path free of allocations - #78

Open
destogl wants to merge 4 commits into
learnsyslab:mainfrom
b-robotized-forks:fix/cartesian-controller-realtime-switch
Open

destogl wants to merge 4 commits into
learnsyslab:mainfrom
b-robotized-forks:fix/cartesian-controller-realtime-switch

Conversation

@destogl

@destogl destogl commented Nov 19, 2026 •

Copy link
Copy Markdown

Fixes for CartesianController when it is switched often, found on a ctrlX CORE X3 (arm64) with a 2 ms realtime cycle and a UR10e.

Depends on #74 (first commit f772ffb, try_update_params() in update()). The commits of this PR are on top of it.

Problems

  • on_activate() used the state of the previous activation:
    • Target messages received while the controller was inactive set new_target_*. The first update() after activation applied them, so the robot moved to an old target.
    • target_wrench_, dq_ref, tau_previous and the topic stiffness kept their old values.
    • A missing or non-finite joint state gave a wrong target pose and NaN torques.
  • on_deactivate() did nothing: the last torque stayed on the command interfaces.
  • The new_target_* flags were plain bool, written by the subscription callbacks and read by update().
  • parse_target_joint_() copied the velocities with the size of the position array, without a size check.
  • updateCurrentState() (every cycle) copied each joint name into a new std::string and looked up the joint by name. setStiffnessAndDamping() allocated a vector.

Changes

  • on_activate(): refuse activation with a missing or non-finite joint state. Drop pending targets. Reset target_wrench_, dq_ref, tau_previous and the topic stiffness. Apply pending parameter changes.
  • on_deactivate(): set the effort commands to zero. Drop pending targets.
  • new_target_* are std::atomic<bool>.
  • parse_target_joint_(): copy at most as many positions and velocities as the controller has joints.
  • on_configure() stores the q_pin index and the continuous flag of each joint. updateNullspaceWeights() looks up the nullspace weights by name only after a parameter change. No log messages in on_activate()/on_deactivate().

The overruns with enable_introspection: true at each switch are fixed separately in #79.

#69 changes the nullspace weights code too. I can rebase on it if it is merged first.

Tests

  • Build and the 14 tests of the package pass on Jazzy. These tests do not load the controller.
  • Hardware: ctrlX CORE X3, UR10e, ros2_control Jazzy, 2 ms cycle, together with fix: publish the CartesianController debug values from an own registry #79. 5 activations and 5 deactivations of cartesian_impedance_controller, mixed with switches to gravity_compensation, the joint trajectory controller and the UR passthrough/force mode controllers: no cycle overrun, and no error or NaN message in the log.

Nibanovic and others added 4 commits October 2, 2026 12:27
on_activate:
- Refuse to activate when a joint state is missing or not finite.
- Drop target pose, joint, wrench, and stiffness messages received while inactive.
- Reset target_wrench_, dq_ref, tau_previous, and the topic stiffness.
- Apply pending parameter changes.

on_deactivate:
- Set the effort commands to zero.
- Drop pending target messages.

Make the new_target_* flags atomic. The subscription callbacks write them
and update() reads them.

Co-Authored-By: Frank <frank@b-robotized.ai>
parse_target_joint_() copied velocities with the size of the position
array and copied positions without a size check. Copy at most as many
entries as the controller has joints, for positions and velocities.

Co-Authored-By: Frank <frank@b-robotized.ai>
- on_configure(): store the q_pin index and the continuous flag of each
  joint. updateCurrentState() uses them instead of a string copy and a
  name lookup per joint and cycle.
- updateNullspaceWeights(): looks up the nullspace weights by joint name
  after a parameter change only. setStiffnessAndDamping() uses the stored
  weights and does not allocate.
- on_activate(): computes the weights again only if try_update_params()
  returns new parameters. No log messages in on_activate() and
  on_deactivate().

Co-Authored-By: Frank <frank@b-robotized.ai>
Comment thread src/cartesian_controller.yaml Resolved
@destogl
destogl force-pushed the fix/cartesian-controller-realtime-switch branch from 389f1b8 to bf4f9f5 Compare November 19, 2026 19:30
@destogl destogl changed the title fix: realtime-safe activation and deactivation of the CartesianController fix: reset the CartesianController state on activation and keep the realtime path free of allocations Nov 19, 2026

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants