control: per-episode arm staging to a home pose and hold-pose on teleop stop by shantanuparab-tr · Pull Request #246 · TrossenRobotics/trossen_sdk

shantanuparab-tr · 2026-06-02T17:02:09Z

Summary

Rebased onto current main (the observer work this branch was originally stacked on has since landed via #238–#245). Three changes remain, all surfacing through the three AI example apps: an opt-in per-episode arm-staging flow that moves each arm to a configured home pose at the start of every episode, a teleop-stop fix so the arm holds its pose on Ctrl+C instead of dropping under gravity, and removal of stale scratch demos.

Changes

In include/trossen_sdk/configuration/types/hardware/arm_config.hpp + src/...: carry staged_position and teleop_moving_time_s through from_json/to_json. to_json() was dropping both, so the home pose declared in config.json never reached TrossenArmComponent and staging silently no-op'd. staged_position is omitted when empty so the no-staging case stays valid.
In include/trossen_sdk/configuration/sdk_config.hpp + src/configuration/sdk_config.cpp: add an opt-in, default-off SdkConfig.stage_each_episode. When set, each example stages every arm with a configured staged_position at the start of every episode; when unset, no staging happens. The shipped example configs set it false.
In include/trossen_sdk/hw/teleop/teleop_controller.hpp + src/hw/teleop/teleop_controller.cpp: add pause_mirror(), which stops the control thread without tearing down the drivers, so staging can cooperate with the continuous mirror loop instead of fighting it. Also drops the one-shot stage() that ran in the constructor — staging now lives entirely in the application episode loop.
In src/hw/arm/trossen_arm_component.cpp: on teleop stop, switch straight into position mode and command the current measured pose first (entering position mode locks the arm where it is, matching the trossen_arm teleop demo's safe-stop sequence), then move to rest over the configured trajectory time. Previously end_teleop() switched to idle mode first, dropping motor torque and letting the arm fall under gravity before position control re-engaged.
In examples/trossen_{solo,stationary,mobile}_ai/{trossen_*_ai.cpp,config.json,README.md}: episode loop now pauses the mirror, stages the arms (blocking move to home), then prepare_teleop() re-arms teleop modes before teleop() restarts the loop. Shipped configs set stage_each_episode false; READMEs document the opt-in flag.
In examples/ + examples/CMakeLists.txt: remove the callback_demo, realsense_rgb config-only sample, and the backend/hardware/producer registry demos along with their CMake targets. These were scratch demos; the three AI example apps remain the supported entry points.

Test Plan

Builds cleanly (make build)
Tests pass (make test) — test_config, test_teleop_controller
Lint passes (pre-commit run --all-files)
Tested on hardware — with stage_each_episode enabled, each arm moves to its configured home pose at the start of every episode; on Ctrl+C the arm holds its pose (no gravity drop) before returning to rest

Breaking Changes

No API or config breaks: the new config keys (stage_each_episode, staged_position, teleop_moving_time_s) are optional and default off, so existing configs load unchanged. The teleop-stop sequence is a deliberate runtime behavior change — the arm now holds position on stop instead of going briefly limp.

Related Issues

Relates to TDS-188

Drop the lifecycle callback_demo, the config-only realsense_rgb sample, and the backend/hardware/producer registry demos along with their CMake targets. These were scratch demos that no longer pull their weight; the three AI example apps remain the supported entry points.

ArmConfig dropped staged_position and teleop_moving_time_s in to_json(), so the home pose declared in config.json never reached TrossenArmComponent and staging silently no-op'd. Carry both fields through from_json/to_json (omit staged_position when empty so the no-staging case stays valid). Add an opt-in, default-off flag SdkConfig.stage_each_episode. When set, each example moves every arm with a configured staged_position to that pose at the start of *every* episode (not just the first); when unset, no staging happens at all. The shipped example configs set it to false. Make staging cooperate with teleop. The mirror loop runs continuously across episodes, so staging mid-session would fight it and leave the leader stuck in position mode. TeleopController gains pause_mirror(), which stops the control thread without tearing down the drivers; the examples pause the mirror, stage the arms (blocking move to home), then prepare_teleop() re-arms teleop modes (leader gravity-compensation) before teleop() restarts the loop. Staging moves are blocking (set_all_positions blocking=true, = a goal_time sleep in the driver) so the arm reaches home before recording resumes; stage()/end_teleop() log their moves (flushed) for field diagnosis. Tests: ArmConfig staging round-trip + empty-omission, SdkConfig stage_each_episode default/parse, and TeleopController pause_mirror stop/restart and idle no-op. READMEs document the opt-in flag. Also drops the one-shot stage() that was in the TeleopController constructor; staging now lives entirely in the application episode loop.

end_teleop() switched the arm to idle mode before returning to rest, which drops motor torque and lets the arm fall under gravity on Ctrl+C before position control re-engages. Switch straight into position mode and command the current measured pose first: entering position mode locks the arm where it is (matching the trossen_arm teleop demo's safe-stop sequence), so it holds its position, then moves to rest over the configured trajectory time.

Copilot

Pull request overview

This PR cleans up the examples surface area and improves teleoperation episode ergonomics/safety by making arm staging explicitly opt-in per episode and ensuring teleop shutdown holds pose before returning to rest.

Changes:

Remove scratch/demo example apps and their CMake targets, keeping the supported AI example entry points.
Add opt-in SdkConfig.stage_each_episode and fix ArmConfig JSON round-tripping for staging fields; update examples + docs accordingly.
Improve teleop lifecycle: add TeleopController::pause_mirror() for per-episode re-staging, and change arm teleop stop to hold current pose before moving to rest.

Reviewed changes

Copilot reviewed 24 out of 24 changed files in this pull request and generated 1 comment.

Show a summary per file

File	Description
`tests/test_teleop_controller.cpp`	Adds unit tests for `pause_mirror()` stop/restart and idle no-op behavior.
`tests/test_config.cpp`	Adds tests for `ArmConfig` staging JSON round-trip/omission and `SdkConfig.stage_each_episode` parsing/default.
`src/hw/teleop/teleop_controller.cpp`	Removes constructor-driven staging and adds `pause_mirror()` implementation.
`include/trossen_sdk/hw/teleop/teleop_controller.hpp`	Documents and exposes `pause_mirror()` in the public API.
`src/hw/arm/trossen_arm_component.cpp`	Changes `end_teleop()` to hold pose before resting; makes `stage()` blocking and adds progress logging.
`include/trossen_sdk/configuration/types/hardware/arm_config.hpp`	Adds `staged_position` and `teleop_moving_time_s` to config schema and JSON serialization logic.
`include/trossen_sdk/configuration/sdk_config.hpp`	Adds `stage_each_episode` flag to SDK config schema.
`src/configuration/sdk_config.cpp`	Parses `stage_each_episode` from top-level SDK config JSON.
`examples/trossen_stationary_ai/trossen_stationary_ai.cpp`	Enables line-buffered stdout and adds per-episode optional staging flow (pause mirror → stage → prepare teleop).
`examples/trossen_stationary_ai/README.md`	Documents the new `stage_each_episode` opt-in behavior.
`examples/trossen_stationary_ai/config.json`	Adds `"stage_each_episode": false` default.
`examples/trossen_solo_ai/trossen_solo_ai.cpp`	Enables line-buffered stdout and adds per-episode optional staging flow.
`examples/trossen_solo_ai/README.md`	Documents the new `stage_each_episode` opt-in behavior.
`examples/trossen_solo_ai/config.json`	Adds `"stage_each_episode": false` default.
`examples/trossen_mobile_ai/trossen_mobile_ai.cpp`	Enables line-buffered stdout and adds per-episode optional staging flow.
`examples/trossen_mobile_ai/README.md`	Documents the new `stage_each_episode` opt-in behavior.
`examples/trossen_mobile_ai/config.json`	Adds `"stage_each_episode": false` default.
`examples/CMakeLists.txt`	Removes build targets for deleted demo executables.
`examples/realsense_rgb/config.json`	Deletes config-only demo sample.
`examples/realsense_rgb/config_depth.json`	Deletes config-only demo sample (depth variant).
`examples/backend_registry_demo.cpp`	Deletes scratch registry demo.
`examples/hardware_registry_demo.cpp`	Deletes scratch registry demo.
`examples/producer_registry_demo.cpp`	Deletes scratch registry demo.
`examples/callback_demo/callback_demo.cpp`	Deletes scratch lifecycle callback demo.

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

control_loop() catches exceptions and clears running_ but does not join, so a mirror loop that exits on its own (e.g. a hardware read throws) leaves thread_ joinable. The only per-episode reaper, pause_mirror(), runs only when stage_each_episode is enabled, so under the default config a subsequent teleop() reassigns a joinable std::thread -> std::terminate. Join any stale, non-running thread in teleop() before starting a new one. Add a regression test (RestartAfterControlLoopExceptionDoesNotTerminate) that reproduces the restart-after-exception path; it aborts (SIGABRT) without the fix and passes with it. Also tighten the end_teleop() hold-pose comment: drop the inaccurate "matches the trossen_arm teleop demo's safe stop" claim (the demo does not command the measured pose first) and describe what the code actually does (goal_time 0 = zero-displacement command to seed the position setpoint).

lukeschmitt-tr · 2026-06-03T16:19:48Z

+  std::vector<float> staged_position;
+
+  /// @brief Trajectory time (seconds) used by staging and the rest move.
+  float teleop_moving_time_s{2.0f};


If this is only used for staging, we should name this variable more appropriately - right now sounds like it's only related to teleop

lukeschmitt-tr · 2026-06-03T20:11:26Z

+   * Used to re-stage arms to their home pose between episodes without tearing
+   * down teleop.
+   */
+  void pause_mirror();


Can we call this similarly to other methods? How's pause_teleop?

lukeschmitt-tr · 2026-06-03T20:12:02Z

+   *
+   * Joins the control thread but, unlike stop_teleop(), does NOT call
+   * end_teleop() — the drivers stay configured and the arms hold their last
+   * commanded pose. After this, the arms can be re-staged, and a subsequent


This comment is very specific about arms - make it more general to hardware

lukeschmitt-tr · 2026-06-03T20:12:16Z

 {
  "robot_name": "trossen_mobile_ai",

+  "stage_each_episode": false,


Does it make sense for this to be a top level config option?

shantanuparab-tr · 2026-06-10T14:59:42Z

Closed in favor of #248

## Summary Moves the per-episode teleop + staging choreography out of the example apps and into the SDK: the `SessionManager` now owns a per-episode hardware lifecycle, components opt in and implement their own per-episode work, and the three AI examples collapse to a single `add_teleop()` registration. Supersedes #246. ## Changes - In `include/trossen_sdk/hw/hardware_component.hpp`: add opt-in, no-op per-episode lifecycle hooks `on_pre_episode()` / `on_episode_started()` / `on_episode_ended()` plus a virtual `is_episode_lifecycle_enabled()` (default `false`). The base parses no config — each component answers the getter from its own config. The per-episode lifecycle runs on both a clean stop and a discard/re-record (user episode-ended *callbacks* stay clean-stop only); a component only participates if it backs a registered producer. - In `include/trossen_sdk/hw/producer_base.hpp` + `src/runtime/producer_registry.cpp` + `src/runtime/push_producer_registry.cpp`: producers retain their backing `HardwareComponent` and expose it via `hardware()`; the registries set it once after construction, so the `SessionManager` can reach every component's hooks with no per-producer code. - In `include/trossen_sdk/runtime/session_manager.hpp` + `src/runtime/session_manager.cpp`: add `add_teleop()` + a `teleop_controllers_` list and `collect_lifecycle_components_()` (the unique, opted-in, producer-backed components). Drive the per-episode bracket — pause mirrors → `on_pre_episode()` (arms re-home) → re-arm in the pre-episode phase; `teleop()` + `on_episode_started()` once recording is live; `reset_teleop()` + `on_episode_ended()` on episode end; `stop_teleop()` at shutdown. Opted-in components stage **in parallel** (independent hardware via `std::async`), so the episode-boundary wait is bounded by the slowest arm rather than the sum. A staging hook that throws is caught and routed through the same `cleanup_and_abort()` path as every other `start_episode()` failure. - In `include/trossen_sdk/hw/arm/trossen_arm_component.hpp` + `src/hw/arm/trossen_arm_component.cpp`: override `on_pre_episode()` → `stage()`, parse `episode_lifecycle_enabled`, and override `is_episode_lifecycle_enabled()`. Rename `teleop_moving_time_s` → `staging_time_s` (it governs any SDK-commanded point-to-point move — staging to home and the return-to-rest move — not just teleop). On teleop stop, hold the measured pose before returning to rest so the arm doesn't drop under gravity. - In `include/trossen_sdk/hw/teleop/teleop_controller.hpp` + `src/hw/teleop/teleop_controller.cpp`: rename `pause_mirror()` → `pause_teleop()` for naming consistency, and join a stale control thread on restart so repeated `teleop()` across episodes cannot `std::terminate`. - In `include/trossen_sdk/configuration/types/hardware/arm_config.hpp`: carry `staged_position`, `staging_time_s`, and `episode_lifecycle_enabled` through `from_json`/`to_json`. - In `examples/trossen_{solo,stationary,mobile}_ai/{trossen_*_ai.cpp,config.json,README.md}`: hand controllers to the manager via `add_teleop()` and remove the per-episode pause/stage/re-arm, mirror-start, reset, and stop choreography the manager now owns. Example configs stay opt-out (no `episode_lifecycle_enabled`); each README's "Optional: home staging" section documents how to enable staging per arm. ## Test Plan - [x] Builds cleanly (`make build`) - [x] Tests pass — `test_config`, `test_teleop_controller`, and the session-manager suite - [x] Lint passes (`pre-commit` on changed files — cpplint, codespell, json, whitespace) - [x] Tested on hardware — with staging enabled, opted-in arms re-home at the start of every episode; on Ctrl+C the arm holds its pose (no gravity drop) before returning to rest ## Breaking Changes The new lifecycle fields — `episode_lifecycle_enabled`, `staged_position`, and `staging_time_s` — are optional and default off/empty, so configs that don't set them load unchanged. One existing key is **renamed, not aliased**: `teleop_moving_time_s` → `staging_time_s`. A config still using the old `teleop_moving_time_s` key will have it silently ignored and fall back to the `2.0s` default; update such configs to `staging_time_s`. This is a deliberate clean break (pre-1.0, no config-compat promise), not a dual-key alias. ABI is not preserved — `HardwareComponent` gains virtuals and the producer bases gain a member; downstream consumers rebuild on upgrade (pre-1.0, no ABI promise). The teleop-stop sequence is a deliberate runtime behavior change — the arm holds position on stop instead of going briefly limp. ## Related Issues Relates to TDS-188

shantanuparab-tr added 3 commits June 2, 2026 12:00

Copilot AI review requested due to automatic review settings June 2, 2026 17:02

Copilot started reviewing on behalf of shantanuparab-tr June 2, 2026 17:02 View session

Copilot AI reviewed Jun 2, 2026

View reviewed changes

Comment thread src/hw/teleop/teleop_controller.cpp

shantanuparab-tr changed the title ~~examples: cleanup demos + opt-in per-episode arm staging and safe teleop stop~~ control: per-episode arm staging to a home pose and hold-pose on teleop stop Jun 2, 2026

shantanuparab-tr self-assigned this Jun 2, 2026

shantanuparab-tr requested review from abhichothani42 and lukeschmitt-tr June 2, 2026 21:01

shantanuparab-tr added the enhancement New feature or request label Jun 2, 2026

lukeschmitt-tr requested changes Jun 3, 2026

View reviewed changes

shantanuparab-tr mentioned this pull request Jun 9, 2026

control: per-episode hardware lifecycle driven by SessionManager #248

Merged

4 tasks

shantanuparab-tr closed this Jun 10, 2026

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

control: per-episode arm staging to a home pose and hold-pose on teleop stop#246

control: per-episode arm staging to a home pose and hold-pose on teleop stop#246
shantanuparab-tr wants to merge 4 commits into
mainfrom
05-29-examples-cleanup-arm-staging

shantanuparab-tr commented Jun 2, 2026 •

edited

Loading

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

lukeschmitt-tr Jun 3, 2026

Uh oh!

lukeschmitt-tr Jun 3, 2026

Uh oh!

lukeschmitt-tr Jun 3, 2026

Uh oh!

lukeschmitt-tr Jun 3, 2026

Uh oh!

shantanuparab-tr commented Jun 10, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Uh oh!

Conversation

shantanuparab-tr commented Jun 2, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Summary

Changes

Test Plan

Breaking Changes

Related Issues

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Uh oh!

lukeschmitt-tr Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

lukeschmitt-tr Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

lukeschmitt-tr Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

lukeschmitt-tr Jun 3, 2026

Choose a reason for hiding this comment

Uh oh!

shantanuparab-tr commented Jun 10, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

shantanuparab-tr commented Jun 2, 2026 •

edited

Loading