Teleop & manual control

Lupin's manual layer lets an operator drive the mecanum base and pose the 4-DOF arm from an Xbox pad, a DualShock4, or the web HMI, while never fighting the autonomy stack for the same command stream. Four velocity sources share one chassis bus, arbitrated by priority and per-source timeout, so a held dead-man instantly preempts Nav2 and releasing it hands the bus straight back. The arm is harder than the base, because the real enemy is a vendor controller that quietly snaps the arm back to its own stale setpoint, and three different mechanisms exist only to defeat it.

This page is the runbook for that layer. It traces the twist_mux bus end to end, names why the dead-man is the load-bearing piece, walks one arm slider from the browser to the servo and back, explains the open-loop re-assert war that the startup-pin and re-seed logic were written to win, and closes with the two failure modes that actually take teleop down on the day, a cooking shoulder servo and a BLE pad that reconnect-loops forever. The whole layer lives in lupin_hmi, and the same code runs in Gazebo and on the real Mirte, only the single downstream drive topic changes.

The command-arbitration bus

Up to four nodes publish geometry_msgs/Twist velocity commands; one arbiter forwards exactly one to the base controller. On the real robot and in the full sim, that arbiter is twist_mux, configured in lupin_hmi/config/twist_mux.yaml. It replaced the old custom cmd_vel_mux.py once we needed real priority-with-timeout arbitration rather than a single manual-takeover window.

twist_mux forwards the highest-priority source that has published within its timeout. When that source goes silent for timeout seconds, the next-highest is allowed through. The timeouts are tuned to each producer's publish rate:

• joy 0.3 s, humans expect a snappy release, and 0.3 s covers a single dropped joy frame at the pad's rate without dropping out mid-drive.
• manual 0.5 s, the web HMI sends roughly 10 Hz over rosbridge and needs a wider window.
• auto 0.5 s, Nav2's velocity_smoother runs at 20 Hz and matches the vendor downstream twist_mux timeout.

Why the dead-man makes priority honest

The Xbox dead-man is not a convenience, it is the reason the whole bus works. teleop_twist_joy is launched with require_enable_button: true and enable_button set to LB (button 6 on Bluetooth, 4 on a USB cable — the index is transport-specific, see Xbox control layout below), so /cmd_vel_joy goes completely silent the instant LB is released. Silent means literally no publication, not a stream of zero-valued Twist messages. That distinction is the entire mechanism.

Walk the counterfactual. Priority arbitration only demotes a source after its timeout elapses with no message. If teleop instead emitted an unbroken all-zero Twist stream, priority 100 would stay perpetually fresh, the 0.3 s timeout would never fire, and Nav2 and the web HMI would be starved of the bus forever. So the rule is strict: the highest-priority source must be able to fall silent, and only the dead-man gives it that ability.

The fourth source: the sim idle-stop guard

/zero_cmd_vel (priority 1, timeout 1.0 s) is a sim-only safety floor. The Gazebo gazebo_ros_planar_move driver holds the last commanded twist indefinitely once its input goes silent, so a sim robot mid-drive would coast forever after joy, manual, and auto all time out. The sim bring-up therefore publishes a constant zero on /zero_cmd_vel as the lowest-priority input, which twist_mux lets through only when every higher source has timed out, halting the robot instead of letting it drift. On hardware nothing publishes /zero_cmd_vel, so this input never activates and the base controller's own command timeout stops the wheels.

The 2-source variant

teleop.launch.py (the standalone DS4 plus sim teleop) uses a separate custom node, cmd_vel_mux (lupin_hmi/cmd_vel_mux.py), that implements the same manual-takeover idea for just two sources. It subscribes /cmd_vel_manual and /cmd_vel_auto, passes manual messages straight through, and drops auto messages for manual_takeover_seconds (default 0.5) after each manual message, giving the operator a takeover window without fighting Nav2. This path predates twist_mux and survives only for the DS4 bench case.

Why the publisher is BEST_EFFORT

Both the sim's vendor twist_mux and the real mecanum controller subscribe BEST_EFFORT. A RELIABLE publisher silently drops every message on the QoS mismatch, and the symptom is idle wheels even though commands are clearly being published. cmd_vel_mux therefore publishes its downstream Twist with QoSProfile(depth=10, reliability=BEST_EFFORT), and the twist_mux output is BEST_EFFORT too. See Navigation for the matching Nav2-side QoS and Operations for the QoS diagnostic traps.

Driving the base

One launch graph, two targets. The only thing that changes is the downstream drive topic, and there are in fact two distinct sim drive paths, not one. On hardware cmd_vel_out remaps to /mirte_base_controller/cmd_vel, the stamped sink the real Mirte mecanum controller listens on. In sim_full.launch.py it remaps to /mirte_base_controller/cmd_vel_unstamped, which feeds the vendor twist_mux and the ros2_control mecanum controller. In the newer sim_robot.launch.py it remaps straight to /cmd_vel, which drives the Gazebo gazebo_ros_planar_move plugin directly, bypassing the ros2_control mecanum controller entirely (that controller is not spawned in this lineage). The joy/manual/auto bus above is byte-identical on all three, only the terminal sink differs.

ros2 launch lupin_hmi teleop.launch.py use_sim:=true world:=navigation

ros2 launch lupin_hmi xbox_teleop.launch.py     # standalone pad teleop
ros2 launch lupin_bringup hardware.launch.py joystick:=true   # full stack, pad included

Xbox control layout

The index order a pad presents to joy_node (SDL2) is decided by the kernel driver, and the driver is decided by how the pad is connected. A USB cable binds xpad (the standard Xbox layout); Bluetooth binds the Microsoft-HID path, a shifted layout the SDL2 defaults don't describe (LB lands at button 6, not 4, and the face buttons skip index 2). Switching transports silently remaps every control — exactly the trap when the pad moves from Bluetooth to a USB-C cable.

So the mapping lives in two files, and xbox_teleop.launch.py selects one at launch by reading the transport bus (lupin_hmi/pad_detect.py, parsing /proc/bus/input/devices: Bus=0003 is USB, Bus=0005 is Bluetooth):

• config/xbox_config.usb.yaml
• config/xbox_config.bluetooth.yaml

Force a transport with controller_mode:=usb|bluetooth; with no pad detected it falls back to USB. Each file carries the full joy_node + teleop_twist_joy_node + arm_teleop parameter sections. The control scheme is identical on both transports — only the indices and axis signs differ:

The USB triggers ship seeded disabled — the wired rest/full convention varies by kernel and a wrong guess could creep the jaw at rest, so the calibrator measures and enables them (see the trigger gotcha below). Re-probe a swapped pad interactively with ros2 run lupin_hmi calibrate_xbox: it detects the transport, spawns its own joy_node (teleop stays down, so nothing drives while you press buttons), walks each control one at a time — capturing the index and direction — and rewrites only the matching xbox_config.<mode>.yaml, backing up the old one and leaving the other transport's file alone. The raw ros2 topic echo /joy --field buttons peek still works for a quick look.

The Bluetooth-mode scheme in full:

Drive (hold LB = button 6, the dead-man):
    Left stick      -> translation (linear.x fwd/back, linear.y strafe)
    Right stick X   -> angular.yaw (turn in place)
    RB (button 7)   -> turbo (scale 0.6/0.6 linear, 1.4 angular vs 0.2/0.7 normal)

Arm (LB released, modes are mutually exclusive by construction):
    Y (4) / A (0)   -> shoulder_lift +/-
    B (1) / X (3)   -> shoulder_pan +/-
    D-pad left/right -> wrist +/-
    D-pad up/down    -> elbow +/-
    RT (axis 4)     -> gripper open  (rest at +1, full pull -1)
    LT (axis 5)     -> gripper close

Two de-confliction decisions are load-bearing:

• Shoulder pan/lift live on the face buttons, not the right stick. The right stick used to be dual-use, chassis yaw with LB held and shoulder pan with LB released, and brushing it during an LB release flicked the arm. Face buttons are physically separate from any chassis input, so the two modes can never bleed into each other.
• The shoulder is also gated off whenever LB is held. In arm_teleop the gate is shoulder_live = enable_held and not disable_held, with shoulder_enable_button defaulting to -1 (always true) and shoulder_disable_button = LB (6 on Bluetooth, 4 on USB; the calibrator keeps it equal to enable_button). The truth table is exactly the design intent:

joy_node runs with autorepeat_rate: 20.0 and a per-transport deadzone (0.15 on Bluetooth, 0.10 on USB). The 0.15 covers the Series X|S pad's resting BT-mode drift, the joy default of 0.05 was too tight and let stick drift times scale_angular spin the chassis in place while LB was held.

Arm and gripper teleop

arm_teleop (lupin_hmi/arm_teleop.py) drives the 4-DOF arm and the 1-DOF gripper directly from /joy. It is an open-loop integrator:

• Each active tick advances a joint by step_rad (Xbox launch 0.15) at update_rate_hz (10.0 Hz, matching the vendor controller_manager tick).
• Every joint is clamped to its real per-joint window via the shared clamp_arm_joint (arm_limits), the asymmetric Hiwonder servo software limit intersected with the URDF envelope, so a held stick can't drive a joint into a pose the servo silently rejects.
• It seeds the commanded pose from /joint_states before sending anything and refuses to publish until all four arm joints have been observed.
• On the idle to active edge it re-seeds a joint from live /joint_states, so a tap is one clean step rather than a jump from a gravity-sagged setpoint.
• Every trajectory point carries a non-zero velocity (joint_velocity, Xbox launch 2.0) because the Hiwonder Telemetrix interface was reported to reject points with velocity 0.0. That report is asserted by the original teleop comment but not yet empirically confirmed against the JTC-to-HW path (arm_traj.py), so the non-zero hint is kept as the conservative default, not as a proven requirement.

Arm motion is published as trajectory_msgs/JointTrajectory to /mirte_master_arm_controller/joint_trajectory. The gripper is driven by the Xbox triggers — open_axis/close_axis map to RT/LT (axes 4/5 on Bluetooth; on USB they are seeded disabled and set by the calibrator) — clamped to the URDF gripper_joint range [-0.20, 0.25] rad, and sent as control_msgs/action/GripperCommand goals to /mirte_master_gripper_controller/gripper_cmd.

The trigger math has a gotcha worth pinning, and it is why the triggers are calibrated per transport. A Bluetooth trigger axis rests at +1 and goes to -1 at full pull, but a wired xpad trigger commonly does the opposite (rests at -1, pulls to +1) or rests near 0 — so a fixed formula would read a wired trigger as part-pulled at rest and creep the jaw. _trigger_pull therefore takes the axis's rest and full values as parameters (gripper_open_rest/gripper_open_full and the matching close pair, measured by the calibrator) and computes pull = (v - rest) / (full - rest) clamped to [0, 1]. It still returns 0 until the axis has produced a real frame, ignoring the bogus 0.0 reading some joy drivers emit before the first physical event — when rest sits near ±1 that 0.0 would otherwise read as a half-pull (the _trigger_init guard; with rest near 0 there is no ambiguity, so it initialises immediately). The gripper is also held at NaN until gripper_joint first appears in /joint_states, so an early trigger pull can't snap a jaw that hasn't been seeded, mirroring the arm-seed gate.

The arm joint limits

The per-joint clamp windows are not a blanket plus-or-minus 90 degrees. Each is the intersection of the real Hiwonder servo software limit (derived from raw counts in the vendor mirte_master_config.yaml as deg = (angle_out - home_out) / 100) with the plus-or-minus 90 degree URDF/JTC envelope, rounded inward to whole degrees. Where the servo stops short of 90 degrees the window tightens to the servo; where the servo reaches past 90 the window stays at the conservative 90 as deliberate under-travel, widening it would mean editing the vendor URDF for no value to the arm's two jobs. arm_limits.py is the single source of truth, mirrored degree-for-degree in web/src/lib/arm.ts so the sliders, presets, and teleop never drift apart.

The gripper command path

The HMI never calls the raw Hiwonder gripper service. The path is:

HMI slider
   -> /lupin/gripper/set_angle_with_speed   (mirte_msgs/srv/SetServoAngleWithSpeed)
   -> gripper_action_bridge                 (maps HMI +/-30 deg -> URDF [-0.20, 0.25] rad)
   -> mirte_master_gripper_controller/gripper_cmd  (control_msgs/action/GripperCommand)

gripper_action_bridge (lupin_hmi/gripper_action_bridge.py, class LupinArmCommandBridge, node name kept as gripper_action_bridge for systemd and log compatibility) runs on both sim and hardware and also exposes the per-joint arm sliders:

The open-loop re-assert war

Here is the subtle part a curious reader should stop on. The vendor mirte_master_arm_control ros2_control hardware interface runs open_loop_control: true, which means the JTC interpolates from its last commanded setpoint, not from /joint_states. Worse, on every control cycle the interface re-sends set_angle for a joint whenever the new command differs from the last by more than SERVO_COMMAND_DIFF (0.05 rad) or the measured position has drifted more than SERVO_MOVED_DIFF (0.05 rad). So a raw servo write, or a gravity sag, is treated as "moved by hand" and snapped back to the controller's stale commanded value (initially 0). This is a dead-band re-assert, not a fixed re-poll.

Three mechanisms exist purely to defeat this, and they are all the same defense from different angles:

• The startup pin publishes one trajectory holding the current pose on the first /joint_states covering all four joints, aligning the JTC's internal setpoint with physical reality, so the interface doesn't immediately snap the arm to near-zero.
• The arm_teleop idle-to-active re-seed copies live /joint_states into the integrator's commanded value on each fresh nudge, so the first tap doesn't step from a stale, sagged setpoint.
• The bridge resync re-adopts an untouched joint from fresh feedback only when it has diverged by more than RESYNC_THRESHOLD_RAD (0.08 rad), catching a deliberate external move (teleop, a preset, calibration) without yanking a still-settling joint on lag.

The arm slider command path

The Arm tab's per-joint sliders never command a servo directly. Every slider release becomes one mirte_msgs/srv/SetServoAngleWithSpeed call, which the same gripper_action_bridge folds into a whole-arm JointTrajectory for the JTC to execute. ros2_control owns the motion; the HMI only ever sends a target angle. Following the shoulder-lift slider end to end:

Five behaviours make this safe and lurch-free:

• It is armed, not live. Each slider is disabled until rosbridge is connected, the E-stop is clear, and torque is on. Torque reads unknown on every mount, including an in-app tab-switch remount, and unknown blocks the sliders, so a return to the Arm tab can never re-arm a freshly-seeded slider. Pressing Enable calls /lupin/arm/set_torque {true}, which re-pins the commanded pose to the current pose before re-energising (no lurch) and then drives the vendor enable_all_servos. Disable is the mirror, it closes the command gate first (stopping re-sends that would re-energize) and then cuts torque.
• The thumb tracks reality until you grab it. Before first touch each slider seeds its target from live /joint_states, so it never starts from a phantom 0 degrees. Its range is the real asymmetric servo window from arm_limits.py, not a blanket plus-or-minus 90.
• One joint in, four joints out. The bridge keeps an authoritative 4-joint commanded pose and rebuilds the trajectory from it, overriding only the joint you moved, except it resyncs any joint that fresh feedback shows another surface moved by more than 0.08 rad. Until the pose is seeded it returns status=False and the slider shows "rejected, joint not seeded."
• The rate slider sets slew speed, not torque. Travel time is max(0.1 s, abs(delta) / rate), and the JTC interpolates from the current setpoint to the target over that window.
• Status is closed-loop, not a fire-and-forget ack. The moving -> settled -> stalled badge is derived from /joint_states convergence (|current - target| within tolerance), not from the service reply, which returns True the instant the trajectory is published. Convergence is the only honest signal, and a joint that never arrives within its expected travel time is exactly how a silent servo reject or a shoulder_lift thermal trip surfaces in the UI.

Arm presets

arm_preset_server (lupin_hmi/arm_preset_server.py) exposes /lupin/arm/preset (lupin_msgs/srv/SetArmPreset). A caller asks for a pose by name, {name: 'home'}, without knowing joint angles, and the node clamps each joint then publishes a JointTrajectory to /mirte_master_arm_controller/joint_trajectory over PRESET_TRAVEL_SECONDS = 3.0 s. The same output topic works on the sim's Gazebo JTC and the real Hiwonder serial-bus JTC with no branching.

Six static presets, in joint order shoulder_pan / shoulder_lift / elbow / wrist (radians):

The gripper is intentionally not driven by presets, pick-and-place sequences interleave preset arm moves with explicit gripper open/close calls so the jaw state is never an accidental side effect of a pose change.

ros2 service call /lupin/arm/preset lupin_msgs/srv/SetArmPreset "{name: home}"

Boot-time auto-home

auto_home (lupin_hmi/auto_home.py, node name lupin_auto_home) is a one-shot run by lupin-auto-home.service after lupin-onboard.service. It is two jobs in one, a controller-stack healer first and an arm parker second, and the healer is the genuinely interesting half.

The healer returns a deliberate tri-state, and the third state is the one that matters operationally:

That None to exit-0 path is the load-bearing decision. If list_controllers never returns valid data, the node cannot know what to load, configure, or switch, so blindly issuing SwitchController would be guessing. Returning False (exit 1) under Restart=on-failure would spin a retry storm; the 2026-05-27 war story is exactly that storm firing. So None becomes a clean exit 0, no retry, and the operator re-triggers via post-boot-sync.sh. Opt out entirely with LUPIN_AUTO_HOME=false in ~/.mirte_settings.sh.

The Xbox BLE pairing trap

This is the single most operationally important teleop failure mode, and it is why the joystick moved to the laptop. The Series X|S pad pairs over BLE (HID-over-GATT), not classic BR/EDR.

Symptom. The pad's light blinks forever and never settles, bluetoothctl info shows Connected flapping no/yes every ~4 s, no /dev/input/event* is created, joy_node binds nothing, /joy is silent, and teleop is dead, even though the launch is "running."

Cause. Without a config pin, BlueZ 5.64 accepts the controller's suggested MinInterval=6 (7.5 ms), the GATT HID descriptor read fails with "Request attribute has encountered an unlikely error," and the connection loops every ~4 s. BlueZ 5.64 has no per-device override path and the pad doesn't advertise sane parameters.

Fix. install-bluetooth-xbox-fix.sh pins [LE] MinConnectionInterval=7, MaxConnectionInterval=9, ConnectionLatency=0 in /etc/bluetooth/main.conf (8.75-11.25 ms, matching the pad's 100 Hz protocol), restarts bluetooth, and re-pairs after deleting the stale LE bond, which otherwise locks in the old intervals until removed.

Sim-only adapters

Two pieces exist only in Gazebo so the unchanged web HMI works there:

• arm_sim_shim exposes the real-robot Hiwonder service/topic names (/io/servo/hiwonder/<arm_joint>/set_angle_with_speed, /io/servo/hiwonder/enable_all_servos) on top of ros2_control, forwarding to /mirte_master_arm_controller/joint_trajectory and republishing /joint_states as mirte_msgs/msg/ServoPosition for HMI feedback. It is not launched on the real robot, where mirte_telemetrix_cpp serves those names natively.
• arm_calibrate_server (hardware-only) exposes /lupin/arm/calibrate (lupin_msgs/srv/CalibrateArm, actions start|commit|cancel|status) for the operator-in-the-loop Hiwonder zero-offset flow. It has no effect in sim (no _set_offset services), so start times out there.

See also