MIT impedance control mode

.gitignore

@@ -8,6 +8,7 @@ VERSION_POSTFIX
 *.egg-info/
 .pytest_cache/
 .ruff_cache/
+.venv/
 .mypy_cache/
 version.py
 

AGENTS.md

@@ -0,0 +1,38 @@
+# META Instructions:
+1. For each user interaction, if the user prompt based on the current observation is general, try to summarize the learning into AGENTS.md for the project or for the user, depending on how general it is.
+
+## General Architecture Notes
+- For long-running autonomous Codex workflows, use a thin deterministic supervisor around `codex exec` or Codex Cloud tasks. Use `AGENTS.md`, skills, and prompts to define behavior and constraints inside each run, but do not rely on prompt text alone for orchestration, persistence, retries, approvals, scheduling, or phase transitions.
+- For reusable human steering, keep a reviewed guidance library on disk. Store raw per-run notes separately from distilled reusable guidance, and promote only stable patterns into the reusable set so future Codex runs can retrieve concise, high-signal instructions without inheriting noisy or contradictory history.
+- For SSH access to the cluster target `ssh -p 2222 le.zhao-labs@le.zhao-labs@10.34.8.87@blj.horizon.cc` or to `gail7`, use the `discover-ssh-agent` skill to find and validate the active `SSH_AUTH_SOCK` instead of hardcoding stale `/tmp/ssh-*` agent paths.
+- To use multiple Codex accounts on one machine, isolate each account with a separate `CODEX_HOME` directory or a separate OS user. Codex profiles layer config files but do not provide independent auth storage by themselves. The VS Code Codex extension also uses the Codex auth home on the remote/local host running the extension, so launch separate VS Code extension hosts with different `CODEX_HOME` values or use separate OS users for concurrent extension accounts.
+
+## HoloBrain Data Recording Notes
+- Friction characterization is hardware-online but deployment-offline: collection may command guarded bidirectional sweeps with compensation disabled, while fitting writes standalone artifacts under `/data`. Characterization tools must not mutate the runtime calibration store or push live friction parameters; use a separate temporary, auto-restoring A/B harness for any future compensation validation.
+- Keep reusable scripted hardware evaluations in `robo_orchard_teleop_ros2.robot_eval`; keep `calibration/` limited to calibration, controller characterization, diagnostics, and their shared store/support code. Scenario-specific planners and analyzers belong beside the evaluation registry and CLI rather than in calibration.
+- For Piper MIT velocity feedforward, keep `v_des` tied to the same reference sample as `p_des`. The safe default is `mit_velocity_feedforward_source=position_delta`, which estimates velocity from consecutive command positions using the command stamp; use `message` only for planner/analytic velocities generated from the same trajectory sample. Do not blindly feed master hardware `JointState.velocity` into follower MIT, because Piper position and high-speed velocity feedback come from separate SDK messages/update rates and can create a steady follower-master gap.
+- For scripted motion with recording enabled, do not block up to the full recorder `__RECORDING__` wait before launching motion. After the start service succeeds, use only a short confirmation wait and continue so one Run Scripted Motion click starts both recording and motion.
+- Scripted-motion recording start should follow the same resilient rule as manual Start: retry `start_recording` in the same click, and do not abort scripted motion merely because the `__RECORDING__` flag was not observed after the start service succeeded.
+- Recorder Start should retry the `start_recording` service within the same click before returning; ROS service discovery through rosbridge can be briefly stale, and without an internal retry users may need to press Start twice.
+- In the inference app recorder panel, keep Start/Stop as single synchronous `if st.button(...):` handlers that perform the full service call, state update, polling/finalize, and `st.rerun()`. Do not split one recorder action between `on_click=` and button-return handling; that can require two presses or leave stale widget state.
+- MIT tuning per-joint latency metrics from `~/Hobot/tools/mit_tuning` are cross-correlations of recorded master and follower `JointState` measurements, so they include actuator/control dynamics and feedback timing, not just ROS/CAN transport. When MIT is enabled, Joints 1-6 are driven through streamed `JointMitCtrl` frames; the gripper is separate from that arm-joint MIT path, so its measured latency can differ substantially from wrist joints.
+- Before trusting inference-app MIT kp/kd sweeps, verify the running container exposes `mit_kp`, `mit_kd`, `mit_vel_ref`, and `mit_torque_ref` on `/robot/left/robot_left_controller` and `/robot/right/robot_right_controller`. Older installed `single_ctrl` nodes only declare `enable_mit_ctrl`, so app-side kp changes or launch-config fields may not affect the live follower controller until the ROS package is rebuilt/reinstalled in the container.
+- `robo_orchard_piper_ros2.single_ctrl` enters Piper MIT move mode with `MotionCtrl_2(0x01, 0x04, 0, 0xAD)` when MIT is enabled. While `enable_mit_ctrl` is true, each incoming `joint_cmd` is sent as per-joint `JointMitCtrl(joint, p_des, 0.0, mit_kp, mit_kd, 0.0)` frames instead of `JointCtrl`; gripper commands are not sent from that MIT path. Scripted motion forces master and follower MIT enabled before starting so scripted runs do not use the joint-control path.
+- Piper MIT gravity compensation in `robo_orchard_piper_ros2.ros_bridge` splits desired gravity torque into clamped MIT `t_ref` and residual `kp` position bias when `mit_gravity_compensation_max_t_ref` saturates. The bias is `dq = (tau_des - t_ref) / mit_kp`, clamped to the MIT position field `[-12.5, 12.5]`; gravity CSV recording logs desired torque, applied `t_ref`, `kp_position_offset`, `kp_offset_tau`, and residual torque.
+- For follower-kp sweep overlays, do not interpret the initial flat segment as controller startup unless traces are aligned to command onset. The recorder waits for camera/follower state topics, not `/left_algo_cmd` or `/right_algo_cmd`, and the overlay scripts may set `t=0` from the first follower `JointState`; variable delay before the first algo command can therefore look like a variable episode startup time.
+- `scripted_joint_master` cold-starts after the app starts recording, has default `start_delay_s=1.0`, and with `use_current_state=true` waits for current follower joint state before publishing. Process startup, ROS discovery, and current-state subscription timing can add variable delay before the first `/left_algo_cmd` and `/right_algo_cmd` samples.
+- For recorded scripted motion, prefer prelaunching `scripted_joint_master` behind an explicit trigger, waiting a fixed setup interval, then starting recording and releasing the trigger. This makes the recorded episode length follow the requested duration while moving process startup/discovery jitter before recording.
+- `robo_orchard_teleop_ros2 scripted_joint_master` publishes a deterministic multi-sine `JointState` trajectory. By default it targets `/left_algo_cmd` and `/right_algo_cmd`, so use the DAgger/takeover mux in autonomous/release mode for scripted follower motion without human teleop.
+- In the inference app, scripted motion belongs under Robot Control as an alternative auto/algo command producer, not as Arm Control or Inference Control. Starting scripted motion should disable inference and set the mux to Auto; any robot Stop, arm Disable, or arm Reset path should stop the scripted-motion subprocess first.
+- Keep scripted-motion app controls conservative: expose duration, amplitude scale, frequency scale, and an optional recording checkbox; prefer frequency scale over raw per-joint frequency editing so users can slow/speed the whole trajectory without changing its shape. Scripted motion does not auto-reset/home; manually reset before recording when a normalized physical pose is needed. Each scripted run waits for follower joint state and shifts its trajectory center so the first commanded point equals the current follower joint state, avoiding a start jump. If the app's scripted recording checkbox is enabled, the active/scripted episode is auto-stopped after the scripted duration. The Streamlit UI must execute the scheduled auto-stop from the main render path and rerun so recorder controls reflect the stopped state.
+- Piper shoulder/joint2 scripted-motion traces can appear almost flat even when the command has the expected range. Local Piper dual-arm URDFs define joint2 with lower limit `0` and upper `3.14` rad; if the reset/current pose is near `0` and scripted joint2 phase drives negative, `/left_algo_cmd` or `/right_algo_cmd` may sweep down to about `-0.8` rad while the measured follower joint2 stays near `0`. Check command topics before assuming the scripted amplitude is small.
+- Piper joint3 scripted-motion traces can show a hard positive-side plateau near `0` rad while tracking negative motion normally. The scripted generator does not clamp joint3, so this pattern points to downstream controller/hardware limit behavior; inspect `/left_algo_cmd` or `/right_algo_cmd` and Piper `joint_3_angle_limit` status before treating it as a frequency-response issue.
+- New recordings made through `robo_orchard_inference_app` write a `mit_params` snapshot into `episode_meta.json` on stop. The snapshot queries configured master/follower controller nodes for `enable_mit_ctrl`, `mit_kp`, `mit_kd`, `mit_vel_ref`, and `mit_torque_ref`, and stores per-node `confirmed` or `error` status.
+- `robot-lab-nexus launch --mount ...` replaces the configured default mount list. The inference app writes recordings under `/data/holobrain/`, so include `--mount ~/data:/data` when also mounting a development workspace. For example: `robot-lab-nexus launch --no-pull --mount ~/data:/data --mount ~/roboorchard-work/roboorchard-dev:/opt/roboorchard --mode inference --record command`. Mounting the dev repo at `/opt/roboorchard` makes ROS install-space binaries and launch/config files available, but the Streamlit `robo-orchard inference-app` CLI still imports `robo_orchard_inference_app` from `/opt/robot-venv/lib/python.../site-packages` unless the package is reinstalled in the container or `PYTHONPATH` is pointed at the mounted source.
+- In `piper_control_compat.launch.py`, follower controller `joint_cmd` subscriptions are remapped directly to `/master/joint_left` and `/master/joint_right`, so the recordable command is the master measured `JointState`; there is no separate `/robot/{left,right}/joint_cmd` publisher. Use the DAgger/takeover launch path when a distinct muxed follower command topic must be recorded as `/action/robot_command/{left,right}/joint`.
+- HoloBrain static TFs are loaded through the runtime `/set_static_transforms` service from the selected `tf_directory`; the app launch task-config generator preserves cached task settings and adds `projects/HoloBrain/data/tf/default_static`, which contains the baseline transforms that used to live in the hard-coded `andrewliu` TF template. Legacy `robot-lab-nexus` launches may instead configure static TFs through a generated `tf_publisher.json`; in that mode the inference app launch config has no `static_transform_service_name`, so the app should hide/ignore the TF Directory selector rather than treating "No TF directories configured" as meaning TFs are absent.
+- `tools/piper_mit_static_stiffness_check.py` is automated-only and averages post-settle high-speed effort for symmetric MIT stimuli. Its position mode commands `q0+e`/`q0-e`; its torque mode holds `q0` and commands `tau_ff +/- --auto-t-ref-nm`. For alpha checks, read `alpha_hat for tau_feedback = alpha * (kp*(p_des-q)+kd*(0-dq)+tau_ff)` and prefer torque mode with `kp=kd=0` when confirming SDK torque encoding/scale. MIT `t_ref` is only 8-bit over `[-8, 8]` and the SDK floors conversion, so tiny requests can encode identically (`+/-0.03` both encode to the same value), while requests above 8 wrap in the CAN payload rather than increasing range. Changing Python-side `t_min`/`t_max` to `[-18, 18]`, or to an asymmetric range such as `[-13, 3]`, only remaps the same 8-bit code unless firmware also decodes that range; it does not make the physical feedforward range shift. Use residual `kp` position bias for larger effective controller output. Use the tool's encoded torque printout/warning and choose a torque step that gives a nonzero encoded diff. This is an active command test, not passive logging; use small torques/offsets and support or mechanically constrain the arm.
+- In direct Piper MIT torque-mode tests on J6 with `kp=kd=0`, high-speed `effort/1000` tracked the SDK-encoded `t_ref` difference at alpha about 1.0 (i.e. 1x). Treat high-speed effort feedback as reporting SDK-input-scale torque for J4-J6 unless independently calibrated; there is no torque scale factor on this path.
+- Repeated direct Piper MIT torque tests on left-arm J1-J6 with `kp=kd=0`, encoded `t_ref` diff about `0.25098 Nm`, `auto-cycles=5`, and post-settle averaging found stable feedback-scale alphas: J1 fw18 about `0.965`, J2 fw18 about `1.396`, J3 fw18 about `1.006`, J4 div1000 about `0.743`, J5 div1000 about `1.244`, J6 div1000 about `0.994`. J1/J3/J6 are consistent with expected feedback scaling; J2/J4/J5 are repeatable joint-specific outliers and should not be forced into a single global alpha without external load calibration or a more constrained test.
+- When launching HoloBrain through `robot-lab-nexus --mount ~/roboorchard-work/roboorchard-dev:/opt/roboorchard`, remember that ROS nodes still execute from `/opt/roboorchard/ros2_package/install`. If a mounted source branch adds console entry points or imports, rebuild the affected packages inside the container with `colcon build --merge-install --symlink-install --packages-select ...`; otherwise launch can fail with missing executables such as `aloha_orchestrator` or stale imports such as `switch_piper_ctrl_mode`. Failed Piper launches can leave stale `take_over`/`aloha_orchestrator` nodes, so clear those before restarting to avoid duplicate-node warnings.
+- For HoloBrain teleop/inference, the active `ros2_package` source packages are `robo_orchard_teleop_ros2`, `robo_orchard_piper_ros2`, their message packages, `robo_orchard_deploy_ros2`, and the data recorder/encoder packages when the full inference tmux stack is used. `robo_orchard_handeye_calib_ros2` is only for the separate hand-eye calibration workflow, and top-level `build/`, `install/`, and `log/` are generated colcon artifacts rather than source packages.

README.md

@@ -16,6 +16,22 @@ specifically designed for:
 - **[Model Deploy](./ros2_package/robo_orchard_deploy_ros2/)**: Efficiently deploy the manipulation model in real robot
 - **ROS2 Integration**: Full ROS2 Humble support for distributed robotic systems
 
+### Piper MIT compensation & evaluation (this branch)
+
+This branch adds a hardware-calibrated compensation stack that minimizes tracking error for the dual-arm Piper teleop rig. 
+Contributions:
+- Gravity compensation via calibrated offset-stiffness / deflection tables
+- Velocity estimation and feedforward
+- Deterministic evaluation runs and tracking-error analysis tooling
+- Operator console additions in the inference app
+
+Start here:
+
+- [robo_orchard_piper_ros2/README.md](./ros2_package/robo_orchard_piper_ros2/README.md) — why and how the MIT compensation stack works (gravity via calibrated position offsets, velocity feedforward + phase lead, oscillation guard), full parameter reference
+- [calibration/README.md](./calibration/README.md) — calibration tool usage and motivation; [calibration/CALIBRATION.md](./calibration/CALIBRATION.md) — store schema and value flow
+- [robo_orchard_teleop_ros2/README.md](./ros2_package/robo_orchard_teleop_ros2/README.md) — teleop stack (muxer, orchestrator, launch files); [scripted/README.md](./ros2_package/robo_orchard_teleop_ros2/robo_orchard_teleop_ros2/scripted/README.md) and [robot_eval/README.md](./ros2_package/robo_orchard_teleop_ros2/robo_orchard_teleop_ros2/robot_eval/README.md) — deterministic evaluation runs
+- [robo_orchard_inference_app/README.md](./python/robo_orchard_inference_app/README.md) — operator console (control modes, MIT tuning, kp calibration, scripted motion, recording)
+
 ## Project Structure
 
 ```