Repository navigation
fix: reset the CartesianController state on activation and keep the realtime path free of allocations - #78
Open
destogl wants to merge 4 commits into
Conversation
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>
This was referenced Nov 19, 2026
destogl
commented
Nov 19, 2026
destogl
force-pushed
the
fix/cartesian-controller-realtime-switch
branch
from
November 19, 2026 19:30
389f1b8 to
bf4f9f5
Compare
This branch has not been deployed
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Fixes for
CartesianControllerwhen 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()inupdate()). The commits of this PR are on top of it.Problems
on_activate()used the state of the previous activation:new_target_*. The firstupdate()after activation applied them, so the robot moved to an old target.target_wrench_,dq_ref,tau_previousand the topic stiffness kept their old values.on_deactivate()did nothing: the last torque stayed on the command interfaces.new_target_*flags were plainbool, written by the subscription callbacks and read byupdate().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 newstd::stringand 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. Resettarget_wrench_,dq_ref,tau_previousand the topic stiffness. Apply pending parameter changes.on_deactivate(): set the effort commands to zero. Drop pending targets.new_target_*arestd::atomic<bool>.parse_target_joint_(): copy at most as many positions and velocities as the controller has joints.on_configure()stores theq_pinindex and the continuous flag of each joint.updateNullspaceWeights()looks up the nullspace weights by name only after a parameter change. No log messages inon_activate()/on_deactivate().The overruns with
enable_introspection: trueat 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
cartesian_impedance_controller, mixed with switches togravity_compensation, the joint trajectory controller and the UR passthrough/force mode controllers: no cycle overrun, and no error or NaN message in the log.