Two-Tier Hierarchical Architecture

The control system follows a two-tier hierarchical design. Tier 1 resides on the Jetson Orin NX running ROS 2 Humble, which hosts the GUI (unified_GUI_V3.py), publishes motor commands at 100 Hz, and performs all combat FSM decision-making, strike sequencing, and impact detection.

Tier 2 resides on the Teensy 4.0 running micro-ROS, which executes a deterministic 200 Hz unified loop (5 ms period). Each cycle performs CAN motor control, I2C IMU polling, MDDS10 height PWM/DIR output, and publishes a 21-double feedback payload via micro-ROS.

Teensy 4.0 Firmware — 200 Hz Unified Loop

The Teensy 4.0 executes a deterministic 200 Hz unified loop (5 ms period) integrating CAN motor control, I2C IMU polling, MDDS10 height PWM/DIR, and micro-ROS publishing into a single cycle. The CAN motor control layer employs sparse edge-triggering, in which commands are sent only on position changes exceeding 0.01 rad or upon a 100 ms keep-alive timeout, thereby preventing the Damiao internal trajectory planner from resetting. The I2C subsystem polls four MPU6050 sensors at 200 Hz across a dual-bus 400 kHz configuration, packing raw 3-axis acceleration data into the 21-double feedback payload. An I2C hard-STOP protocol provides manual clock recovery (9 SCL toggles followed by a STOP condition) for long-wire bus hang prevention. Additionally, a micro-ROS disconnect recovery mechanism triggers a SCB_AIRCR hardware reset when the transport heartbeat is lost.

Feedback Payload Structure

The /motor_feedback topic carries a Float64MultiArray of 21 doubles:

[pos×4, current×4, CAN_count, IMU_accel×12]
 ─────  ────────   ─────────  ──────────────
  0-3     4-7          8         9-20
      

Kinematic Model

The coaxial differential joint couples Motor 1 (roll) and Motor 2 (pitch) through a 3:1 helical gear reduction into a shared bevel gear stack. Because the bevel gear walks when only the roll motor moves, pitch and roll are mechanically coupled and cannot be commanded independently without software decoupling.

Before deployment, a three-step Homing Wizard calibrates the digital twin to the physical hardware. (1) Set Roll Zero — the operator positions the arm at neutral and captures \(o_r\). (2) Run Pitch Scan — the software drives the pitch motor automatically in 0.05 rad steps until motor current exceeds a configurable threshold (default 0.3 A), discovering the physical pitch limits. (3) Calibrate Directions — the GUI commands short test movements and presents binary prompts (e.g. "did the arm swing FORWARD?"), from which \(s_r\), \(s_p\), and \(c\) are resolved. The resulting constants are persisted to data/arm_config.yaml, grounding all FK and IK calculations to the specific build's tolerances.

Forward Kinematics — Motor Space → Joint Space

The calibrated forward kinematics resolve the bevel coupling by incorporating the home offsets and sign parameters:

$$\theta_{\text{roll}} = s_r \cdot \frac{m_r - o_r}{G} \tag{Eq. 1}$$ $$\theta_{\text{pitch}} = s_p \cdot \frac{(m_p - o_p)\; +\; c\,(m_r - o_r)}{G} \tag{Eq. 2}$$

where \(G = 3\) (helical gear reduction). Eq. 2 exposes the differential coupling: pitch depends on the sum of the pitch motor displacement and a coupling-weighted roll motor displacement.

Cartesian Projection — Joint Space → Arm-Tip Direction

The joint angles are projected into a normalised Cartesian arm-tip direction vector using the Rotation-Around-Arm-Axis model. At \(\theta_{\text{pitch}} = 0\) the arm points straight outward along the X-axis; pitching the arm sweeps a circle in the Y–Z plane whose orientation is governed by the roll angle:

$$d_x = s_x \cdot \cos(\theta_{\text{pitch}}) \tag{Eq. 3}$$ $$d_y = \sin(\theta_{\text{pitch}}) \cdot \cos(\theta_{\text{roll}}) \tag{Eq. 4}$$ $$d_z = \sin(\theta_{\text{pitch}}) \cdot \sin(\theta_{\text{roll}}) \tag{Eq. 5}$$

where \(s_x = -1\) (left arm) or \(+1\) (right arm).

This Cartesian representation forms the software digital twin of permissible arm configurations. Every incoming motor-space target is converted to joint space in Python, validated, and — if necessary — clamped before any CAN command is issued to the Teensy 4.0. Pitch is constrained to a software operating zone of ±1.0 rad (physical hard-stop limit: ±1.57 rad). Applying the clamp in joint space rather than as independent per-motor bounds eliminates the Jump-Back defect, where hardware-side endstops issued sudden counter-commands to an uncommanded motor.

Inverse Kinematics — Joint Space → Motor Space

To command a desired joint-space pose, the inverse kinematics convert back to raw motor positions and apply the software decoupling term that cancels the bevel walking effect in real-time:

$$\Delta_r = \left(\frac{\theta_{\text{roll}}}{s_r}\right) \cdot G, \qquad m_r = \Delta_r + o_r \tag{Eq. 6}$$ $$m_p = \left[\left(\frac{\theta_{\text{pitch}}}{s_p}\right) \cdot G \;-\; c\,\Delta_r\right] + o_p \tag{Eq. 7}$$

Strike Library

Each strike is defined by a Windup position and an Apex (impact) position, both stored in joint space so that entries survive recalibration. Strikes are authored interactively in the 3D digital twin: an azimuth slider selects one of 30 pre-computed workspace arcs rendered against the calibrated arm model; clicking the arc places Windup and Apex markers. A quadratic Bézier path is drawn in real-time (green = within calibrated limits, red = out of limits). Curvature and arc-angle sliders shape the trajectory before saving. Libraries are persisted as JSON:

{
  "left":  { "Jab": {"space": "joint", "windup": [roll, pitch],
                      "apex": [roll, pitch], "curvature": 0.0,
                      "arc_angle": 0.0, "waypoints": [...]} },
  "right": { "Cross": { ... } }
}
      

State-Space Strike Planning

The Cartesian arm-tip vector produced by the kinematic model drives three planning behaviours in the Dynamic Sparring FSM. Vector alignment skip logic computes the dot-product angle between a strike's intended direction and the current approach vector; if below 30°, the Windup node is bypassed for a more fluid transition. Perimeter orbit transit routing inserts ordered library Windup positions as via-points when a direct inter-strike transit would cross the user-facing centre-line, keeping the arm on a safe perimeter path. Synchronised per-motor speed scaling proportionally adjusts each motor's speed to its share of the total travel distance, producing straight-line motor-space trajectories between any two positions.

ROS Control Interface

A dedicated ROS Control Tab enables external front-end integration via 7 ROS 2 topics:

Subscriber Topics (Front-End → Robot)

Topic	Function
/robot/strike_command	Execute strike by slot + duration
/robot/punch_slots	Assign strikes to numbered slots
/robot/system_enable	Arm or disarm motors
/robot/imu_calibration_cmd	Trigger IMU calibration

Publisher Topics (Robot → Front-End)

Topic	Function
/robot/strike_feedback	Strike completion status + timing
/robot/imu_calibration_status	Calibration progress + results
/robot/strike_detected	Real-time pad strike events