Merge OpenArm VR teleop and supermarket sim into dev

.gitignore

@@ -17,7 +17,7 @@ unity/VrRos/.utmp/
 unity/VrRos/*_BurstDebugInformation_DoNotShip/
 
 # VIVE OpenXR UPM tarball, 361 MB: a redistributable release artefact, so it is a setup step.
-# Run tools/fetch_vive_plugin.sh to get it.
+# Run python3 scripts/fetch_vive_plugin.py to get it.
 unity/VrRos/vendor/
 
 # TMP's own .unitypackage, unpacked by VrRosSetup.ImportTmpResources. It keeps its GUIDs across
@@ -39,3 +39,7 @@ unity/VrRos/Assets/Resources.meta
 # Python bytecode from running the scripts in place
 __pycache__/
 *.pyc
+
+# Local headset configuration and generated OpenArm output (if kept in this checkout).
+/vr_config.json
+/models/openarm_v1/

CMakeLists.txt

@@ -27,10 +27,11 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   msg/EyeGaze.msg
   msg/BodyPoses.msg
+  msg/TeleopStatus.msg
   DEPENDENCIES std_msgs geometry_msgs
 )
 
-find_package(mj_kdl_wrapper REQUIRED)
+find_package(mj_kdl_wrapper 0.4 REQUIRED)
 
 # The .glb embeds images as PNG, and PNG means deflate.
 find_package(ZLIB REQUIRED)
@@ -136,5 +137,91 @@ install(PROGRAMS scripts/vive_vr_discovery DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
 
 ament_export_targets(export_vive_vr_ros2 HAS_LIBRARY_TARGET)
+
+# Independent OpenArm applications sit above shared VR transport and kinematics.
+option(BUILD_OPENARM_SIM "Build the OpenArm simulation application" ON)
+option(BUILD_OPENARM_VR_BRIDGE "Build the physical-robot VR feedback/command bridge" ON)
+option(BUILD_OPENARM_HARDWARE "Build the guarded OpenArm CAN hardware plugin" OFF)
+if(BUILD_OPENARM_HARDWARE)
+  find_package(hardware_interface REQUIRED)
+  find_package(pluginlib REQUIRED)
+  find_package(OpenArmCAN REQUIRED)
+  add_library(openarm_guarded_hardware SHARED src/openarm/hardware/guarded_driver.cpp)
+  target_include_directories(openarm_guarded_hardware PRIVATE include)
+  target_link_libraries(openarm_guarded_hardware OpenArmCAN::openarm_can rclcpp::rclcpp ${std_msgs_TARGETS})
+  ament_target_dependencies(openarm_guarded_hardware hardware_interface pluginlib)
+  pluginlib_export_plugin_description_file(hardware_interface config/openarm_guarded_hardware.xml)
+  install(TARGETS openarm_guarded_hardware LIBRARY DESTINATION lib)
+endif()
+# Shared kinematics and clutch semantics contain neither physics stepping nor CAN I/O.
+if(BUILD_OPENARM_SIM OR BUILD_OPENARM_VR_BRIDGE)
+  add_library(openarm_teleop_support SHARED src/openarm/common/ik.cpp src/openarm/common/teleop.cpp)
+  target_link_libraries(openarm_teleop_support PUBLIC vive_vr_core mj_kdl_wrapper::mj_kdl_wrapper)
+  set_target_properties(openarm_teleop_support PROPERTIES
+    BUILD_RPATH "${MUJOCO_LIB_DIR}" INSTALL_RPATH "${MUJOCO_LIB_DIR}")
+  install(TARGETS openarm_teleop_support LIBRARY DESTINATION lib)
+endif()
+if(BUILD_OPENARM_SIM)
+  add_executable(openarm_sim_node src/openarm/simulation/node.cpp)
+  target_link_libraries(openarm_sim_node PRIVATE openarm_teleop_support ${std_srvs_TARGETS})
+  set_target_properties(openarm_sim_node PROPERTIES
+    BUILD_RPATH "${MUJOCO_LIB_DIR}" INSTALL_RPATH "${MUJOCO_LIB_DIR}")
+  install(TARGETS openarm_sim_node DESTINATION lib/${PROJECT_NAME})
+endif()
+if(BUILD_OPENARM_VR_BRIDGE)
+  find_package(trajectory_msgs REQUIRED)
+  find_package(controller_manager_msgs REQUIRED)
+  # Retain executable/service names for existing launch files and headset workflows.
+  add_executable(openarm_hardware_preview_node src/openarm/hardware/vr_bridge.cpp)
+  target_link_libraries(openarm_hardware_preview_node PRIVATE openarm_teleop_support ${trajectory_msgs_TARGETS} ${std_srvs_TARGETS} ${controller_manager_msgs_TARGETS})
+  set_target_properties(openarm_hardware_preview_node PROPERTIES
+    BUILD_RPATH "${MUJOCO_LIB_DIR}" INSTALL_RPATH "${MUJOCO_LIB_DIR}")
+  install(TARGETS openarm_hardware_preview_node DESTINATION lib/${PROJECT_NAME})
+endif()
+
+if(BUILD_TESTING AND (BUILD_OPENARM_SIM OR BUILD_OPENARM_VR_BRIDGE))
+  enable_testing()
+  find_package(Python3 REQUIRED COMPONENTS Interpreter)
+  add_executable(hardware_guard_check tests/hardware_guard_check.cpp)
+  target_include_directories(hardware_guard_check PRIVATE include)
+  add_test(NAME hardware_guard_check COMMAND hardware_guard_check)
+  if(BUILD_OPENARM_VR_BRIDGE)
+    add_test(NAME openarm_return_to_zero_check
+      COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_return_to_zero_check.py $<TARGET_FILE:openarm_hardware_preview_node>)
+    add_test(NAME openarm_gripper_check
+      COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_gripper_check.py $<TARGET_FILE:openarm_hardware_preview_node>)
+    add_test(NAME openarm_live_bridge_check
+      COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_live_bridge_check.py $<TARGET_FILE:openarm_hardware_preview_node>)
+    add_test(NAME openarm_hardware_preview_check
+      COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_hardware_preview_check.py $<TARGET_FILE:openarm_hardware_preview_node>)
+  endif()
+  add_test(NAME openarm_modes_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_modes_check.py)
+  add_test(NAME openarm_visuals_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_visuals_check.py)
+  add_test(NAME openarm_launch_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_launch_check.py)
+  add_test(NAME openarm_alignment_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_alignment_check.py)
+  add_test(NAME openarm_alignment_capture_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_alignment_capture_check.py)
+  add_test(NAME openarm_left_alignment_capture_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/openarm_alignment_capture_check.py left)
+  add_test(NAME teleop_toggle_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/teleop_freshness_check.py $<TARGET_FILE:teleop_node> --toggle)
+  add_test(NAME teleop_freshness_check
+    COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/tests/teleop_freshness_check.py $<TARGET_FILE:teleop_node>)
+  # These run against tests/openarm_test_arm.xml; OPENARM_TEST_MODEL adds a run on the real robot.
+  set(OPENARM_TEST_MODEL "" CACHE FILEPATH "Generated OpenArm controlled MJCF for integration tests")
+  foreach(check openarm_ik_check openarm_bridge_check openarm_dual_check)
+    add_executable(${check} tests/${check}.cpp)
+    target_link_libraries(${check} PRIVATE openarm_teleop_support)
+    target_compile_definitions(${check} PRIVATE TEST_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/tests")
+    add_test(NAME ${check} COMMAND ${check})
+    if(OPENARM_TEST_MODEL)
+      add_test(NAME ${check}_openarm COMMAND ${check} "${OPENARM_TEST_MODEL}")
+    endif()
+  endforeach()
+endif()
 ament_export_dependencies(rclcpp geometry_msgs std_msgs)
 ament_package()

README.md

@@ -33,6 +33,7 @@ python3 -m venv --system-site-packages venv && source venv/bin/activate
 
 source /opt/ros/jazzy/setup.bash
 colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_SCENES=ON
+source install/setup.bash
 ```
 
 `BUILD_SCENES=ON` exports the worlds in `scenes/` — a Kinova arm on a table, and a RoboCasa
@@ -52,10 +53,142 @@ ros2 launch vive_vr_ros2 tracking.launch.py
 And the headset half, on any machine with the Unity editor installed:
 
 ```bash
+cd ~/work/p/vrws/src/vive-vr-ros2
+python3 scripts/fetch_vive_plugin.py
 ./scripts/build_apk.sh --setup --install --logcat    # build, install, launch, tail the log
 ```
 
-### Headset settings
+## OpenArm V1 teleoperation
+
+Simulation and physical control have independent application code, configuration and
+launch entry points: **`openarm_virtual.launch.py`** for simulation and
+**`openarm_real.launch.py`** for the physical robot. Both use the existing VR app.
+See [switching modes, separate builds and start commands](docs/openarm_modes.md).
+
+The simulation application drives **both arms in MuJoCo** from their matching controllers. A
+resolved-rate servo over the Orocos KDL Jacobian moves all seven arm joints to follow the
+gripper position and orientation independently for each arm. This is a motion-test simulation with gravity and contacts disabled,
+not physical robot control or a grasping simulation. Your elbow is not tracked, so the robot
+can use a different elbow posture to reach the same gripper pose.
+
+The commands below use **Zsh**, `~/vive_vr_ws` for the ROS workspace, and
+`~/openarm_ws` for the OpenArm description workspace. The source checkout is
+`~/vive_vr_ws/src/vive-vr-ros2` (a symlink to a Desktop checkout also works).
+For Bash, use `setup.bash` instead of `setup.zsh`. Install the base dependencies using
+[Installation](docs/install.md) first. The helper expects the OpenArm V1 description at
+`~/openarm_ws/src/openarm_description`; see [OpenArm setup](docs/openarm_sim.md) for path overrides.
+An existing headset app still runs the PC-side changes; rebuild the APK to also pick up the
+clock fix in [Known limits](docs/limits.md), which recorded data needs but the arm no longer does.
+
+### Build and prepare once
+
+```bash
+source /opt/ros/jazzy/setup.zsh
+source ~/openarm_ws/install/setup.zsh
+source ~/vive_vr_ws/install/setup.zsh
+cd ~/vive_vr_ws
+colcon build --packages-select vive_vr_ros2 \
+  --cmake-args -DBUILD_SCENES=OFF -DBUILD_OPENARM_SIM=ON
+source ~/vive_vr_ws/install/setup.zsh
+
+cd ~/vive_vr_ws/src/vive-vr-ros2
+python3 scripts/openarm_sim.py prepare
+```
+
+`prepare` generates the robot models, scene export and configuration under
+`~/vive_vr_ws/models/openarm_v1`, outside the source checkout. It preserves a saved controller
+alignment. Rebuild after C++ changes; re-run `prepare` when the generated model/configuration
+needs updating. The OpenArm source description is not modified.
+
+### Start and align with one terminal
+
+Stop any previous simulation or tracking launch with **Ctrl+C**, then:
+
+```bash
+source /opt/ros/jazzy/setup.zsh
+source ~/openarm_ws/install/setup.zsh
+source ~/vive_vr_ws/install/setup.zsh
+export ROS_DOMAIN_ID=42
+cd ~/vive_vr_ws/src/vive-vr-ros2
+
+python3 scripts/openarm_sim.py launch --teleop --align --delay 10
+```
+
+1. Put on the headset during the 10-second delay and open the VR app.
+2. Keep the right side grip released. Hold your hand down beside you, matching the hanging
+   robot gripper, with your chosen controller tip direction pointing down.
+3. Hold still for at least one second. The script captures alignment and automatically restarts
+   the stack. Wait for the headset to reconnect and the robot to reappear.
+4. Rotate the white controller outline into the cyan target until it turns green, then hold the right side grip.
+   Start with a small hand movement and wrist rotation. Release the grip to hold position.
+
+Keep the terminal open while using VR. This one launch runs the robot simulation, ROS bridge,
+discovery server and scene server. **Ctrl+C stops them all.**
+
+To enable the left hand, stop the launch and calibrate it once:
+
+```bash
+python3 scripts/openarm_sim.py configure
+python3 scripts/openarm_sim.py launch --teleop --align --arm left --delay 10
+```
+
+Keep the **left** grip released and hold that controller in the same comfortable hanging-tool
+pose during capture. Each arm has its own calibration, grip reference, trigger and visual guide.
+Your right calibration is preserved. Both arms are blocked during calibration, and neither
+controller can command the other arm. Rebuild ROS and the APK after updating the source.
+`configure` updates controller settings on an existing model without reimporting meshes.
+
+For later sessions with the same controller grip and orientation pairing, reuse the saved
+alignment instead of capturing again:
+
+```bash
+python3 scripts/openarm_sim.py launch --teleop
+```
+
+| Input | Effect while the side grip is held |
+|---|---|
+| Move either hand | Move the gripper target at 1:1 translation scale. |
+| Rotate the wrist | Rotate the gripper target; 90-degree turns fit within the 100-degree bound per press. |
+| Pull/release the index trigger | Close/open the gripper after the initial trigger sample is captured. |
+| Release the side grip | Hold the arm; re-grip starts a new relative movement. |
+
+Robot reach, joint limits and speed limits still apply. The generated configuration bounds
+translation to 1 m per press. Engagement requires a saved alignment and a fresh controller pose
+within about 11 degrees of the calibrated gripper orientation. Your hand position is free;
+you do not need to reach the robot or the floor. Match the white outline to the cyan one
+until the target turns green, then hold that side grip. VR shows only hand indicators, with
+no permanent information panel. A temporary message explains how to recover from a movement
+limit or tracking pause and disappears when you resume. Past its reach the arm holds the closest pose
+it can and keeps following; move back towards it and it picks you up again. Motion is relative
+to each grip press, not an absolute hand-to-robot calibration.
+
+### Connection and diagnostics
+
+The headset and PC must be able to reach each other on the local network. If the app stays on
+“searching for PC,” check that the launch terminal is still running and reports
+`Rosbridge WebSocket server started on port 9090`. Discovery uses UDP 9091; scene downloads
+use TCP 8000. Stop duplicate launches before starting another.
+
+If automatic address selection chooses the wrong network interface, append
+`--host-ip YOUR_PC_LAN_IP` to the launch command. This selects the scene download address;
+the headset's ROS bridge address is discovered separately or set in `vr_config.json`.
+Use the same `ROS_DOMAIN_ID` in any additional ROS terminals.
+
+From another sourced terminal, these commands inspect input or reset the simulated robot:
+
+```bash
+python3 scripts/openarm_sim.py inputs
+python3 scripts/openarm_sim.py reset
+```
+
+Release the side grip before resetting. For configuration, manual alignment, model details
+and test commands, see [OpenArm simulation](docs/openarm_sim.md).
+
+## Headset settings
+
+Guarded physical OpenArm bring-up, low-speed live VR testing, and read-only preview are
+described in [OpenArm hardware](docs/openarm_hardware.md). Live commands require explicit
+arming; the guarded driver starts by holding measured positions without homing.
 
 Everything the client can be told without rebuilding the APK lives in one file, on the headset:
 
@@ -91,13 +224,18 @@ in [Configuration](docs/configuration.md#headset-settings).
 - [First run on the headset](docs/bringup.md) — the on-device checklist
 - [Testing](docs/testing.md) — what is verified, how, and what is not
 - [Design notes](docs/design.md) — why the system is shaped this way, and what was rejected
+- [OpenArm simulation](docs/openarm_sim.md) — dual-arm IK, alignment, controls and tests
 - [Teleoperation](docs/teleop.md) — the clutch, the delta stream, and why it needs no calibration
 - [Known limits](docs/limits.md) — what is missing or approximate
 
 ## Status
 
-The PC side works and is verified by running it. **Nothing has run on the headset yet** — see
-[Testing](docs/testing.md) for which claims rest on evidence, and
-[First run](docs/bringup.md) for the on-device checklist.
+The Focus Vision client has connected to the PC, downloaded the OpenArm scene, streamed
+controller data and supplied a saved controller/tool alignment. Automated checks cover IK,
+alignment math and capture, clutch gating, limits, timeouts and calibration-launch cleanup.
+Full headset motion testing across the workspace remains to be completed; contact-based grasping remains outside the simulation bridge. The separate guarded hardware
+path is experimental; see [hardware testing](docs/openarm_hardware.md) for its limits.
+See [OpenArm verification](docs/openarm_sim.md#verification-and-next-work) and
+[Testing](docs/testing.md) for the scope of the checks.
 
 MIT licensed.

config/openarm_guarded_hardware.xml

@@ -0,0 +1,5 @@
+<library path="openarm_guarded_hardware">
+  <class name="vive_vr_ros2/GuardedOpenArm" type="vive_vr_ros2::GuardedOpenArm" base_class_type="hardware_interface::SystemInterface">
+    <description>OpenArm V1 measured-pose startup, CAN freshness checks and bounded position commands.</description>
+  </class>
+</library>