(Hacky version do not use) Add PVAT streaming DoCommand#218
Draft
Esha Maharishi (EshaMaharishi) wants to merge 6 commits into
Draft
(Hacky version do not use) Add PVAT streaming DoCommand#218Esha Maharishi (EshaMaharishi) wants to merge 6 commits into
Esha Maharishi (EshaMaharishi) wants to merge 6 commits into
Conversation
Externally-generated trajex PVATs can now be streamed to the arm without the controller re-planning: open one long-lived trajectory and feed spline points into it incrementally, terminating with a CANCEL. - streaming_trajectory: a third move_command_data alternative holding the declared point budget and a pending deque of spline points. - state_ open/push/close_pvat_stream: open grabs the move slot with a streaming command; push appends points (do_command thread); close requests the terminating CANCEL. pending is pushed/drained under mutex_. - state_controlled_::handle_move_request streaming_trajectory branch: write TRAJECTORY_START once, then writeTrajectorySplinePoint per pending point each worker tick WITHOUT re-issuing START (a second START would kill and restart the robot-side trajectory thread), then CANCEL on close or Stop(). - do_command keys trajex_stream_open / _points / _close, with parse_pva_points decoding [p0..5, v0..5, a0..5, dt] points (radians; dt = per-segment seconds). - is_moving reports true while a stream is open. Builds clean via conan on macOS arm64 (bin/setup.sh + bin/build.sh). Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
The terminating CANCEL is async: close set the flag and returned, but the move slot stays occupied until the robot replies CANCELED and trajectory_done_callback_ clears it. So a stream reopen at a coverage-line boundary, or the executor's post-work retreat (a normal MoveThroughJointPositions), raced the still-in-progress actuation and failed with "an actuation is already in progress". Keep the move request's completion future at open and block on it in close (up to 1.5s, under no lock so the worker can issue the CANCEL and the done-callback can run) until the slot is freed. Swallow the expected cancellation exception. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
trajex is time-optimal (TOTG), so its acceleration profile is bang-bang: it slams between +a_max and -a_max and saturates. Feeding that pt.a as a quintic spline boundary condition forces the robot to realize a discontinuous acceleration at each segment boundary, tripping a joint-torque jump fault (e.g. C174A1) ~1s into a work stroke. Velocity is continuous, so the cubic overload (positions + velocities) tracks the path smoothly and lets the robot pick its own bounded acceleration. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
The trajex/PVAT stream was opened with a null RealtimeTrajectoryLogger and the streaming branch never called write_realtime_sample, so streaming faults (e.g. C306A3 "Joint: Acceleration failed to pass sanity check") left no telemetry to diagnose — only regular planned moves were logged. Attach a logger to the stream (when telemetry_output_path is set) and record the robot's actual-vs-target joint state every control tick while streaming. The realtime samples include target_accelerations_rad_per_sec2, which is exactly the "received Joint acceleration target" the C306A3 sanity check rejects, so the log nearest the fault pins the offending joint and magnitude. Writes a per-stream "<ts>_<resource>_trajex_realtime_trajectory.json"; null logger (telemetry off) preserves prior behavior. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Add streamed_points[] to the telemetry log: each knot exactly as handed to writeTrajectorySplinePoint (positions, velocities, trajex's intended accelerations, and the segment timestep). The realtime_samples only show the robot's cubic-derived target_accelerations, which surfaced a ~40 rad/s² start-of-stream spike that trips protective slowdowns (and at the limit, the C306A3 acceleration sanity fault) — but not whether the cause is the dropped zero-velocity start knot or a too-small first timestep. Logging the raw knots makes that diagnosable directly: compare the sent (p, v, dt) against the derived target_accel. No-ops when no logger is attached (telemetry off). Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
The streaming branch dropped trajex's acceleration and sent cubic (P+V) spline points, letting the robot derive its own acceleration. That cubic reconstruction is hypersensitive to the first-knot position gap (derived accel ~ 6*dp/dt^2), which produced the start-of-stream spikes. Pass pt.a through via the quintic overload instead. CAUTION: this re-introduces the C174A1 risk the cubic choice originally avoided — trajex (TOTG) has a bang-bang acceleration profile, and feeding it as a quintic boundary condition makes the robot realize discontinuous acceleration at segment joins, which previously tripped a joint-torque-jump fault mid-stream. Only safe if trajex's acceleration is smooth/jerk-limited enough to be realizable; revert to the cubic overload if C174A1 returns. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
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.
No description provided.