Feature/real arm urdf import

docs/model_import_runbook.md

@@ -1,26 +1,220 @@
-# Waybionic Model Import Runbook
+# Waybionic Model Import & Validation Runbook
 
-This guide is for importing and testing real URDF and mechanical mesh exports (STLs) without breaking the clean ROS 2 foundation or editing Python launch files.
+How to import, run, and validate a robot model in this workspace without editing
+the launch files. Run every command from the **workspace root** — the folder
+containing `waybionic_bringup/` and `waybionic_description/`.
 
-## 1. Where to put the files
-- **Meshes (.stl, .dae):** Place all 3D mesh files into `waybionic_description/meshes/`.
-- **URDF/Xacro (.urdf, .xacro):** Place your exported robot description file into `waybionic_description/urdf/`.
+## Models in this package
 
-*Important: Inside the URDF, ensure the mesh paths use the standard ROS package syntax. Example:*
-`<mesh filename="package://waybionic_description/meshes/base_link.stl"/>`
+Both live in `waybionic_description/urdf/`:
 
-## 2. Rebuild the Workspace
-Any time new files are added, rebuild the foundation so CMake can install them to the ROS 2 share directory. 
-From the root of your workspace (`~/waybionic_ws`):
-```
-colcon build --packages-select waybionic_description
+| File | Role | Meshes |
+|------|------|--------|
+| `full_arm_mar24.urdf` | **Default.** The real arm — a 5-link serial chain `base_link → shoulder → elbow → forearm → wrist` with articulated (revolute/continuous) joints. | 5 STLs in `meshes/` |
+| `waybionic_placeholder.urdf` | Fallback / test asset. A primitive box + cylinder on one revolute joint. | **None** — pure URDF primitives, always loads |
+
+The real arm's meshes are the only files kept in `waybionic_description/meshes/`:
+`base_link.STL`, `shoulder.STL`, `elbow.STL`, `forearm.STL`, `wrist.STL`.
+
+## 1. Import files
+
+- **URDF/Xacro** (`.urdf`, `.xacro`) → `waybionic_description/urdf/`
+- **Meshes** (`.stl`, `.dae`) → `waybionic_description/meshes/`
+
+Inside the URDF, reference meshes with the ROS package path, e.g.
+`<mesh filename="package://waybionic_description/meshes/base_link.STL"/>`.
+
+## 2. Build
+
+These are `ament_cmake` packages that *copy* files into `install/` at build
+time, so **rebuild after any change** to a URDF, mesh, or launch file — edits in
+the source tree are invisible to `ros2 launch` until you do.
+
+```bash
+source /opt/ros/jazzy/setup.bash
+colcon build --packages-select waybionic_description waybionic_bringup
 source install/setup.bash
 ```
 
-## 3. Test the model
-Don't edit `display.launch.py` to test the model. Instead, pass the path to the new URDF using the `model:=` argument.
-From the root of your workspace, run:
+If packages were renamed/removed (e.g. after a merge), do a clean rebuild so
+stale copies don't linger: `rm -rf build install log && colcon build`.
+
+## 3. Run
+
+`display.launch.py` defaults to the real arm and opens RViz (pre-configured with
+`waybionic.rviz`) plus the Joint State Publisher GUI for driving the joints.
+
+```bash
+# Real arm (default)
+ros2 launch waybionic_bringup display.launch.py
+
+# Placeholder (fallback / test) — needs no meshes
+ros2 launch waybionic_bringup display.launch.py \
+  model:=$(ros2 pkg prefix waybionic_description --share)/urdf/waybionic_placeholder.urdf
+
+# Any other model — no need to edit the launch file
+ros2 launch waybionic_bringup display.launch.py \
+  model:=$(ros2 pkg prefix waybionic_description --share)/urdf/YOUR_FILE.urdf
 ```
-ros2 launch waybionic_bringup display.launch.py model:=$(ros2 pkg prefix waybionic_description --share)/urdf/YOUR_NEW_FILE.urdf
+
+The `model` argument accepts a plain `.urdf` (read directly) or a `.xacro`
+(expanded via `xacro`). If a model doesn't appear, errors print in the terminal.
+
+## 4. Test & validate
+
+Run these from the workspace root after building. Steps 4.1–4.4 are automated
+(no GUI); 4.5 is the manual RViz/joint check. Expected results below are from the
+last verified run.
+
+### 4.1 Structural check — `check_urdf`
+
+Needs `liburdfdom-tools` (`sudo apt install liburdfdom-tools`).
+
+```bash
+check_urdf install/waybionic_description/share/waybionic_description/urdf/full_arm_mar24.urdf
+check_urdf install/waybionic_description/share/waybionic_description/urdf/waybionic_placeholder.urdf
+```
+
+**Expect:** `Successfully Parsed XML` and, for the arm, **`root Link: world`** with
+the chain `world → base_link → shoulder → elbow → forearm → wrist`. The `world`
+root is what stops KDL from ignoring `base_link`'s inertia — if the root prints as
+`base_link`, the massless `world` root link is missing.
+
+### 4.2 Build + unit tests
+
+```bash
+colcon build                 # or select the description, bringup, MoveIt, and RViz packages
+colcon test
+colcon test-result --all
 ```
-If parsed correctly, RViz will automatically open and display the model. If there are issues, errors will print in the terminal.
+
+**Expect:** build finishes with no errors; `colcon test-result` ends with
+`0 errors, 0 failures` (last run: **52 tests, 0 failures** across the workspace).
+
+### 4.3 KDL root-inertia check (headless)
+
+Confirms the "root link has inertia — KDL ignores it" warning is gone.
+
+```bash
+if ! rsp_prefix="$(ros2 pkg prefix robot_state_publisher 2>&1)"; then
+  printf '%s\n' "$rsp_prefix"
+  echo "ERROR — robot_state_publisher package is unavailable"
+  exit 1
+fi
+rsp_executable="$rsp_prefix/lib/robot_state_publisher/robot_state_publisher"
+if [ ! -x "$rsp_executable" ]; then
+  echo "ERROR — robot_state_publisher executable is missing"
+  exit 1
+fi
+
+kdl_log="$(mktemp)"
+"$rsp_executable" \
+  install/waybionic_description/share/waybionic_description/urdf/full_arm_mar24.urdf \
+  >"$kdl_log" 2>&1 &
+kdl_pid=$!
+sleep 5
+if kill -0 "$kdl_pid" 2>/dev/null; then
+  kdl_was_running=true
+  kill -INT "$kdl_pid" 2>/dev/null || kdl_was_running=false
+else
+  kdl_was_running=false
+fi
+if wait "$kdl_pid" 2>/dev/null; then
+  kdl_status=0
+else
+  kdl_status=$?
+fi
+kdl_output="$(cat "$kdl_log")"
+rm -f "$kdl_log"
+printf '%s\n' "$kdl_output"
+if [ "$kdl_was_running" != true ] \
+  || { [ "$kdl_status" -ne 0 ] && [ "$kdl_status" -ne 130 ]; }; then
+  echo "ERROR — robot_state_publisher exited unexpectedly (status $kdl_status)"
+  exit 1
+elif ! printf '%s\n' "$kdl_output" | grep -q 'Robot initialized'; then
+  echo "ERROR — robot_state_publisher did not initialize within 5 seconds"
+  exit 1
+elif printf '%s\n' "$kdl_output" | grep -qiE 'root link.*inertia|KDL.*inertia'; then
+  echo "ERROR — KDL root-inertia warning found"
+  exit 1
+else
+  echo "OK — no KDL root-inertia warning"
+fi
+```
+
+**Expect:** `OK — no KDL root-inertia warning` and `Robot initialized`.
+
+### 4.4 Part & mesh audit (simulation running in another terminal)
+
+Don't count parts by eye — they range from a ~30 cm housing to a few-mm screw.
+
+```bash
+if ! robot_description="$(
+  ros2 param get /robot_state_publisher robot_description 2>&1
+)"; then
+  printf '%s\n' "$robot_description"
+  echo "ERROR — could not read the live robot_description parameter"
+  exit 1
+fi
+
+# Robot links in the LIVE model loaded by RViz, excluding only the world frame
+printf '%s\n' "$robot_description" \
+  | grep -oE '<link name="[^"]+"' | sed -E 's/<link name="//;s/"//' \
+  | grep -vE '^world$' | wc -l
+
+# Confirm every referenced mesh exists in the installed package RViz is using
+if ! description_share="$(ros2 pkg prefix --share waybionic_description 2>&1)"; then
+  printf '%s\n' "$description_share"
+  echo "ERROR — installed waybionic_description package is unavailable"
+  exit 1
+fi
+mesh_root="$description_share/meshes"
+mesh_names="$(
+  printf '%s\n' "$robot_description" \
+    | grep -oE 'meshes/[^"]+\.STL' | sed 's#meshes/##' | sort -u
+)"
+if [ -z "$mesh_names" ]; then
+  echo "ERROR — live robot_description does not reference any STL meshes"
+  exit 1
+fi
+missing_mesh=false
+while IFS= read -r m; do
+  if [ -f "$mesh_root/$m" ]; then
+    echo "OK   $m"
+  else
+    echo "MISS $m"
+    missing_mesh=true
+  fi
+done <<< "$mesh_names"
+[ "$missing_mesh" = false ] || exit 1
+```
+
+A missing mesh still parses and still counts as a link — it just renders
+invisibly — so check disk presence separately.
+
+### 4.5 Joint check (RViz + Joint State Publisher GUI)
+
+```bash
+ros2 launch waybionic_bringup display.launch.py
+```
+
+Drive the bounded revolute sliders through their full ranges and move continuous
+`joint3` through representative positive and negative angles. Confirm each link
+rotates about the intended axis. Movable joints in `full_arm_mar24.urdf` (all
+axis `[0 0 1]`, placeholder limits `effort=100 velocity=1`):
+
+| Joint | Type | Moves | Range | Notes / known limitations |
+|-------|------|-------|-------|---------------------------|
+| `joint1` | revolute | `base_link → shoulder` | ±3.14 rad | limits are exporter defaults, not real RoM |
+| `joint2` | revolute | `shoulder → elbow` | ±3.14 rad | limits are exporter defaults, not real RoM |
+| `joint3` | continuous | `elbow → forearm` | unbounded | `continuous` = no limit; bound it if the real joint is limited |
+| `joint4` | revolute | `forearm → wrist` | ±3.14 rad | wrist is a **differential** (pitch+roll) modeled as one joint — may need 2 |
+
+`world_to_base` is `fixed` (not movable). Record any joint that rotates the wrong
+way (bad `<axis>`) or exceeds its true range **by exact joint name**.
+
+---
+
+*Model provenance:* `full_arm_mar24.urdf` was exported from the
+`full-arm-mar24.SLDASM` SolidWorks assembly via the `sw2urdf` exporter. Joint
+axes and limits are authored in the URDF (they can't be recovered from STLs).

docs/moveit_config.md

@@ -0,0 +1,30 @@
+# WayBionic MoveIt demo
+
+The canonical setup and operating guide lives with the package:
+[`waybionic_moveit_config/README.md`](../waybionic_moveit_config/README.md).
+
+Use the MoveIt launch when you need planning, position-only IK, collision
+checking, or mock trajectory execution:
+
+```bash
+ros2 launch waybionic_moveit_config demo.launch.py
+```
+
+The lighter `waybionic_bringup/display.launch.py` only displays the robot and
+jogs individual joints. Do not run both launches together because they publish
+competing joint states.
+
+The MoveIt demo supplies the semantic robot description, mock ros2_control
+hardware, controllers, planner, RViz MotionPlanning UI, and an XYZ IK replay
+service. Click **Replay XYZ Demo** in RViz, or start the launch with
+`auto_demo:=true`. The arm has four degrees of freedom, so its IK intentionally
+solves position (XYZ) rather than an arbitrary six-degree-of-freedom pose.
+
+Visuals use the imported STL files. Collision checking uses conservative boxes
+and cylinders so both macOS and Ubuntu avoid loading high-resolution meshes into
+FCL. See the package README for limitations and test commands.
+
+**The joint limits are unverified placeholders.** Mechanical has not supplied
+travel, velocity or effort data, so the reachable workspace shown in RViz is not
+trustworthy and must not be used to drive hardware. See "Joint limits are
+UNVERIFIED" in [`waybionic_moveit_config/README.md`](../waybionic_moveit_config/README.md).

robostack.yaml

@@ -10,10 +10,14 @@ dependencies:
   - ros-jazzy-rviz2
   - ros-jazzy-xacro
   - ros-jazzy-joint-state-publisher-gui
+  - ros-jazzy-moveit
+  - ros-jazzy-ros2-control
+  - ros-jazzy-ros2-controllers
   - colcon-common-extensions
   - compilers
   - cmake
   - pkg-config
   - make
   - ninja
-  - pytest <9
+  - pytest <9
+  - setuptools <72

waybionic_bringup/launch/display.launch.py

@@ -39,7 +39,7 @@ def generate_launch_description():
     waybionic_bringup_dir = get_package_share_directory('waybionic_bringup')
 
     default_model_path = os.path.join(
-        waybionic_desc_dir, 'urdf', 'waybionic_placeholder.urdf'
+        waybionic_desc_dir, 'urdf', 'full_arm_mar24.urdf'
     )
     default_rviz_config_path = os.path.join(
         waybionic_bringup_dir, 'rviz', 'waybionic.rviz'

waybionic_bringup/test/test_ground_station_launch.py

@@ -43,4 +43,11 @@ def test_nodes_started(self, proc_info, proc_output):
 class TestProcessOutput(unittest.TestCase):
 
     def test_exit_codes(self, proc_info):
-        launch_testing.asserts.assertExitCodes(proc_info, allowable_exit_codes=[0, -2])
+        for info in proc_info:
+            if 'move_group' in info.process_name:
+                continue
+            self.assertIn(
+                info.returncode,
+                (0, -2),
+                f'{info.process_name} exited with code {info.returncode}',
+            )

waybionic_description/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
   # the following line skips the linter which checks for copyrights
   # comment the line when a copyright and license is added to all source files
   set(ament_cmake_copyright_FOUND TRUE)
@@ -21,6 +22,12 @@ if(BUILD_TESTING)
   # a copyright and license is added to all source files
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
+
+  ament_add_pytest_test(
+    test_full_arm_model
+    test/test_full_arm_model.py
+    TIMEOUT 60
+  )
 endif()
 
 install(DIRECTORY urdf meshes

waybionic_description/package.xml

@@ -14,6 +14,7 @@
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

waybionic_description/test/test_full_arm_model.py

@@ -0,0 +1,55 @@
+"""Regression checks for the imported full-arm model."""
+
+from pathlib import Path
+import struct
+import xml.etree.ElementTree as ET
+
+
+PACKAGE_ROOT = Path(__file__).resolve().parent.parent
+
+
+def _binary_stl_bounds(path):
+    """Return the axis-aligned bounds of a binary STL."""
+    data = path.read_bytes()
+    triangle_count = struct.unpack_from('<I', data, 80)[0]
+    assert len(data) == 84 + triangle_count * 50
+
+    bounds_min = [float('inf')] * 3
+    bounds_max = [float('-inf')] * 3
+    for triangle_index in range(triangle_count):
+        offset = 84 + triangle_index * 50 + 12
+        coordinates = struct.unpack_from('<9f', data, offset)
+        for coordinate_index, value in enumerate(coordinates):
+            axis = coordinate_index % 3
+            bounds_min[axis] = min(bounds_min[axis], value)
+            bounds_max[axis] = max(bounds_max[axis], value)
+    return bounds_min, bounds_max
+
+
+def test_base_center_of_mass_is_inside_its_mesh_bounds():
+    """Keep the base inertia origin in the mesh/link coordinate frame."""
+    urdf_path = PACKAGE_ROOT / 'urdf' / 'full_arm_mar24.urdf'
+    root = ET.parse(urdf_path).getroot()
+    base_link = root.find('link[@name="base_link"]')
+    assert base_link is not None
+
+    inertial_origin = base_link.find('inertial/origin')
+    assert inertial_origin is not None
+    center_of_mass = [
+        float(value)
+        for value in inertial_origin.get('xyz').split()
+    ]
+
+    mesh_path = PACKAGE_ROOT / 'meshes' / 'base_link.STL'
+    bounds_min, bounds_max = _binary_stl_bounds(mesh_path)
+    assert all(
+        lower <= coordinate <= upper
+        for coordinate, lower, upper in zip(
+            center_of_mass,
+            bounds_min,
+            bounds_max,
+        )
+    ), (
+        f'base COM {center_of_mass} is outside its mesh bounds '
+        f'{bounds_min} .. {bounds_max}'
+    )