Bracket ServoJ stream with ServoMoveStart/ServoMoveEnd (~2.1s → ~0.2s latency) - #1
Open
jonathan-fries wants to merge 1 commit into
Open
Bracket ServoJ stream with ServoMoveStart/ServoMoveEnd (~2.1s → ~0.2s latency)#1jonathan-fries wants to merge 1 commit into
jonathan-fries wants to merge 1 commit into
Conversation
The hardware interface streamed ServoJ() without first calling ServoMoveStart() (and never called ServoMoveEnd() on shutdown). The SDK documents ServoJ as requiring ServoMoveStart/ServoMoveEnd bracketing (robot.h: "Start/End servo motion, used with ServoJ and ServoCart commands"). Without the bracket the controller treats each ServoJ as a discrete buffered motion, adding ~2.1s command->motion latency, which makes streamed trajectory execution abort on path tolerance (the commanded position runs ~2s ahead of the arm). Measured on an FR5 (FW 3.98, libfairino 2.3.8, aarch64): adding the bracket cut command->motion latency from 2.113s to 0.158-0.278s (~90%) and planned moves complete at the stock 0.2 rad path tolerance. Refs FAIR-INNOVATION#39
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
1 participant
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.
What
Bracket the
ServoJcommand stream with the SDK'sServoMoveStart()/ServoMoveEnd()calls in the ros2_control hardware interface.on_activate(): callServoMoveStart()after the RPC connect + initial joint sync, before returningSUCCESS(logs the return code; 0 = accepted).on_deactivate(): callServoMoveEnd()beforeStopMotion().Why
write()streamsServoJ(&cmd,&extcmd,0,0,0.008,0,0)at the controller update rate, but the stream was never announced withServoMoveStart().robot.hdocuments these as "Start/End servo motion, used with ServoJ and ServoCart commands." Un-bracketed, the controller appears to treat eachServoJas a discrete buffered motion, adding a large command→motion latency (~2.1 s).Downstream (MoveIt Pro / ros2_control
joint_trajectoryexecution) this makes streamed trajectories abort on path tolerance at the start of the move — the commanded position runs ~2 s ahead of the stationary arm, tripping the tolerance (e.g.deviation 0.2007 > tolerance 0.2000) before the arm has moved; the arm then begins moving ~2 s later and stops short. Continuous velocity jogging "works" only because it has no path-tolerance check (it just trails the operator by ~2 s).Measured result (controlled A/B, same move, same host)
0.2007 > 0.2000ServoMoveStart()returns 0 (accepted); noerror-14. Latency measured from JTACcontroller_state.referenceonset vs/joint_statesactual-position onset.Notes
ServoJ'sacc/vel/filterT/gainare documented "temporarily not available" inrobot.h, so they are not viable latency levers — the bracketing is the effective fix.Refs FAIR-INNOVATION#39