Implement blocking homing endpoint and refactor homing logic in mujoco_server.py

- Introduced a new blocking API endpoint (`/homing`) for UR5 that pauses jogging commands while executing homing, enhancing control during the homing process.
- Refactored the homing logic to remove the need for continuous home messages, improving efficiency and reducing potential drift.
- Updated the frontend to utilize the new blocking endpoint, simplifying the user interaction for homing.
- Removed deprecated home and stop_home message types from protocol_types.py to streamline the codebase.
- Enhanced tests to validate the new blocking homing functionality and ensure proper behavior during homing operations.
- Updated README.md to document the new homing endpoint and its usage.

README.md

@@ -422,6 +422,7 @@ Nova-Sim provides a minimal HTTP API for static information:
 | `/video_feed` | `GET` | MJPEG video stream of the main camera |
 | `/camera/<name>/video_feed` | `GET` | MJPEG video stream of auxiliary cameras (added via `/cameras`) |
 | `/cameras` | `POST` | Register a new auxiliary camera for the current robot/scene |
+| `/homing` | `GET` | **Blocking** homing for UR5 via Nova jogging (pauses joggers while executing) |
 
 **Example `/env` response:**
 ```json
@@ -478,6 +479,14 @@ Notes:
 - Cameras are scoped to the current robot + scene and show up in `/env` under `camera_feeds`.
 - The UI listens for a `state` message containing `camera_event` and refreshes tiles automatically.
 
+**Blocking homing (`GET /homing`)**
+```
+/nova-sim/api/v1/homing?timeout_s=30&tolerance=0.01&poll_interval_s=0.1
+```
+Notes:
+- This endpoint is **blocking**: it returns only after the robot reaches `home_pose` or a timeout occurs.
+- While homing is active, jogging commands are paused (incoming jog/teleop actions are ignored).
+
 ### Client → Server WebSocket Messages
 
 **`action`** - Send velocity actions to all robots:
@@ -511,16 +520,6 @@ For robot arms (UR5):
 - `robot`: Required. One of: `"g1"`, `"spot"`, `"ur5"`, `"ur5_t_push"`
 - `scene`: Optional scene name (see `/metadata` for available scenes)
 
-**`home`** - Move robot to home position (UR5 only):
-```json
-{"type": "home"}
-```
-
-**`stop_home`** - Stop homing sequence (UR5 only):
-```json
-{"type": "stop_home"}
-```
-
 **`teleop_action`** - Send teleoperation action (primarily for UR5 keyboard control):
 ```json
 {"type": "teleop_action", "data": {"vx": 0.01, "vy": 0.0, "vz": 0.0}}

frontend/index.html

@@ -1078,9 +1078,7 @@
 
             <button class="danger" style="width: 100%; margin-top: 15px;" onclick="resetEnv()">Reset
                 Environment</button>
-            <button id="homeBtn" style="width: 100%; margin-top: 10px; display: none;" onmousedown="startHoming()"
-                onmouseup="stopHoming()" onmouseleave="stopHoming()" ontouchstart="startHoming()"
-                ontouchend="stopHoming()">🏠 Move to Home</button>
+            <button id="homeBtn" style="width: 100%; margin-top: 10px; display: none;" onclick="startHoming()">🏠 Move to Home</button>
         </div>
     </div>
 
@@ -2129,10 +2127,18 @@
         }
 
         // Homing functionality for UR5
-        let homingInterval = null;
-        let homingJoints = [];  // Joints being homed
+        let homingInProgress = false;
 
-        function startHoming() {
+        function setHomeButtonState(inProgress) {
+            const homeBtn = document.getElementById('homeBtn');
+            if (!homeBtn) {
+                return;
+            }
+            homeBtn.disabled = inProgress;
+            homeBtn.textContent = inProgress ? '🏠 Homing...' : '🏠 Move to Home';
+        }
+
+        async function startHoming() {
             if (currentRobot !== 'ur5' && currentRobot !== 'ur5_t_push') {
                 return;
             }
@@ -2142,24 +2148,30 @@
                 return;
             }
 
-            // Send home messages continuously while button is pressed
-            send('home', {});
+            if (homingInProgress) {
+                return;
+            }
 
-            // Send home message every 100ms while button is pressed
-            if (homingInterval) clearInterval(homingInterval);
-            homingInterval = setInterval(() => {
-                send('home', {});
-            }, 100);
-        }
+            homingInProgress = true;
+            setHomeButtonState(true);
 
-        function stopHoming() {
-            // Stop the home message interval
-            if (homingInterval) {
-                clearInterval(homingInterval);
-                homingInterval = null;
+            const params = new URLSearchParams({
+                timeout_s: '30',
+                tolerance: '0.01',
+                poll_interval_s: '0.1'
+            });
+            try {
+                const response = await fetch(`${API_PREFIX}/homing?${params.toString()}`);
+                const payload = await response.json();
+                if (!response.ok) {
+                    console.warn('Homing failed:', payload);
+                }
+            } catch (err) {
+                console.warn('Homing request failed:', err);
+            } finally {
+                homingInProgress = false;
+                setHomeButtonState(false);
             }
-            // Send stop_home message to backend
-            send('stop_home', {});
         }
 
         function setCmd(vx, vy, vyaw) {

mujoco_server.py

@@ -145,6 +145,8 @@ homing_state = {
     "saved_velocity": None,  # Store velocity to restore after homing
 }
 homing_lock = threading.Lock()
+jogging_pause_lock = threading.Lock()
+jogging_paused = False
 
 # WebSocket clients
 ws_clients = set()
@@ -237,8 +239,6 @@ AVAILABLE_ACTIONS = [
     "camera_follow",
     "toggle_target_visibility",
     "teleop_action",
-    "home",
-    "stop_home",
     "arm_target",
     "gripper",
     "control_mode",
@@ -1207,19 +1207,6 @@ def simulation_loop():
             if env is None:
                 return
 
-            # Check for homing timeout (prevent drift when home messages stop)
-            with homing_lock:
-                if homing_state.get("is_jogging", False):
-                    last_home_time = homing_state.get("last_home_time", 0)
-                    timeout = 0.5  # 500ms timeout - if no home message for this long, stop
-                    if time.time() - last_home_time > timeout:
-                        stop_jog = getattr(env, "stop_jog", None)
-                        if callable(stop_jog):
-                            stop_jog()
-                            homing_state["is_jogging"] = False
-                            homing_state["active"] = False
-                            print("[Homing] Stopped jogging due to timeout (no home messages)")
-
             sim_dt = env.model.opt.timestep
 
             # Run physics steps
@@ -1408,6 +1395,137 @@ def generate_overlay_frames(name: str):
         time.sleep(0.04)
 
 
+def _set_jogging_paused(paused: bool) -> bool:
+    """Pause or resume jogging commands; returns the previous pause state."""
+    global jogging_paused
+    with jogging_pause_lock:
+        previous = jogging_paused
+        jogging_paused = paused
+    return previous
+
+
+def _is_jogging_paused() -> bool:
+    with jogging_pause_lock:
+        return jogging_paused
+
+
+def _step_multi_axis_homing_locked(tolerance: float = 0.01) -> dict:
+    """Run one homing control step (expects mujoco_lock + homing_lock held)."""
+    homing_state["last_home_time"] = time.time()
+
+    if env is None or current_robot not in ("ur5", "ur5_t_push"):
+        return {"status": "unsupported"}
+
+    home_pose = getattr(env, "home_pose", None)
+    get_joints = getattr(env, "get_joint_positions", None)
+    start_multi_joint_jog = getattr(env, "start_multi_joint_jog", None)
+    stop_jog = getattr(env, "stop_jog", None)
+
+    if home_pose is None or not callable(get_joints) or not callable(start_multi_joint_jog) or not callable(stop_jog):
+        return {"status": "unsupported"}
+
+    if hasattr(home_pose, "tolist"):
+        target_positions = home_pose.tolist()
+    else:
+        target_positions = list(home_pose)
+
+    current_positions = get_joints()
+
+    # Calculate errors for all joints
+    errors = []
+    for joint_idx in range(min(len(target_positions), len(current_positions))):
+        current = current_positions[joint_idx]
+        target = target_positions[joint_idx]
+        error = target - current
+        errors.append(error)
+
+    max_abs_error = max(abs(e) for e in errors) if errors else 0.0
+    if max_abs_error < tolerance:
+        if homing_state.get("is_jogging", False):
+            stop_jog()
+            homing_state["is_jogging"] = False
+            homing_state["active"] = False
+            homing_state.pop("previous_errors", None)
+            homing_state.pop("current_velocities", None)
+            print("[Homing] Completed - all joints at target")
+        return {
+            "status": "done",
+            "errors": errors,
+            "max_abs_error": max_abs_error,
+        }
+
+    if "previous_errors" not in homing_state:
+        homing_state["previous_errors"] = [0.0] * len(errors)
+
+    base_velocity = 0.15  # rad/s - slower to prevent overshoot
+    velocities = []
+    stop_tolerance = tolerance * 0.5
+
+    for joint_idx, error in enumerate(errors):
+        abs_error = abs(error)
+
+        prev_error = homing_state["previous_errors"][joint_idx]
+        if prev_error != 0 and ((prev_error > 0 and error < 0) or (prev_error < 0 and error > 0)):
+            velocities.append(0.0)
+            continue
+
+        if abs_error < stop_tolerance:
+            velocities.append(0.0)
+        else:
+            if max_abs_error > 0:
+                velocity = base_velocity * (abs_error / max_abs_error)
+            else:
+                velocity = 0.0
+
+            damping_zone = tolerance * 3.0
+            if abs_error < damping_zone:
+                damping_factor = (abs_error / damping_zone) ** 2
+                velocity *= damping_factor
+
+            min_velocity = 0.02
+            if velocity < min_velocity:
+                velocities.append(0.0)
+            else:
+                velocities.append(velocity if error > 0 else -velocity)
+
+    homing_state["previous_errors"] = errors
+
+    if all(abs(v) < 0.001 for v in velocities):
+        if homing_state.get("is_jogging", False):
+            stop_jog()
+            homing_state["is_jogging"] = False
+            homing_state["active"] = False
+            homing_state.pop("previous_errors", None)
+            homing_state.pop("current_velocities", None)
+            print("[Homing] All joints reached target or stopped")
+        return {
+            "status": "done",
+            "errors": errors,
+            "max_abs_error": max_abs_error,
+        }
+
+    current_velocities = homing_state.get("current_velocities")
+    velocities_changed = current_velocities is None or any(
+        abs(v1 - v2) > 0.01 for v1, v2 in zip(velocities, current_velocities)
+    )
+
+    if velocities_changed:
+        if homing_state.get("is_jogging", False):
+            stop_jog()
+
+        start_multi_joint_jog(velocities)
+        homing_state["is_jogging"] = True
+        homing_state["current_velocities"] = velocities
+        homing_state["active"] = True
+
+    return {
+        "status": "running",
+        "errors": errors,
+        "max_abs_error": max_abs_error,
+        "velocities": velocities,
+    }
+
+
 def handle_ws_message(ws, data):
     """Handle incoming WebSocket message."""
     global needs_robot_switch, camera_follow, t_target_visible, last_teleop_action
@@ -1443,6 +1561,13 @@ def handle_ws_message(ws, data):
         j6 = payload.get('j6', 0.0)
         gripper = payload.get('gripper', None)
 
+        if current_robot in ("ur5", "ur5_t_push") and _is_jogging_paused():
+            if gripper is not None:
+                with mujoco_lock:
+                    if env is not None:
+                        env.set_gripper(float(gripper))
+            return
+
         with mujoco_lock:
             if env is not None:
                 # For UR5: translate velocity actions to jogging commands
@@ -1510,155 +1635,6 @@ def handle_ws_message(ws, data):
         scene = payload.get('scene')
         _schedule_robot_switch(robot, scene)
 
-    elif msg_type == 'home':
-        # Multi-axis homing with Nova API v2
-        # UI sends home messages continuously while button is pressed
-        # When messages stop, jogging should stop to prevent drift
-        with mujoco_lock, homing_lock:
-            # Update last home message timestamp
-            homing_state["last_home_time"] = time.time()
-            if env is not None and current_robot in ("ur5", "ur5_t_push"):
-                home_pose = getattr(env, "home_pose", None)
-                get_joints = getattr(env, "get_joint_positions", None)
-                start_multi_joint_jog = getattr(env, "start_multi_joint_jog", None)
-                stop_jog = getattr(env, "stop_jog", None)
-
-                if home_pose is not None and callable(get_joints) and callable(start_multi_joint_jog) and callable(stop_jog):
-                    if hasattr(home_pose, "tolist"):
-                        target_positions = home_pose.tolist()
-                    else:
-                        target_positions = list(home_pose)
-
-                    current_positions = get_joints()
-                    tolerance = 0.01  # rad (~0.57 degrees)
-
-                    # Calculate errors for all joints
-                    errors = []
-                    for joint_idx in range(min(len(target_positions), len(current_positions))):
-                        current = current_positions[joint_idx]
-                        target = target_positions[joint_idx]
-                        # Compute shortest angular distance (no wrapping needed for UR5)
-                        # UR5 joints don't wrap around - they have limited ranges
-                        error = target - current
-                        errors.append(error)
-
-                    # Check if any joint needs homing
-                    max_abs_error = max(abs(e) for e in errors)
-                    if max_abs_error < tolerance:
-                        # All joints at home - stop
-                        if homing_state.get("is_jogging", False):
-                            stop_jog()
-                            homing_state["is_jogging"] = False
-                            homing_state["active"] = False
-                            homing_state.pop("previous_errors", None)
-                            print("[Homing] Completed - all joints at target")
-                        return
-
-                    # Initialize previous errors tracking if needed
-                    if "previous_errors" not in homing_state:
-                        homing_state["previous_errors"] = [0.0] * len(errors)
-
-                    # Calculate proportional velocities for all joints
-                    base_velocity = 0.15  # rad/s - slower to prevent overshoot
-                    velocities = []
-
-                    # Tighter tolerance for stopping individual joints
-                    stop_tolerance = tolerance * 0.5  # Half the main tolerance
-
-                    for joint_idx, error in enumerate(errors):
-                        abs_error = abs(error)
-
-                        # Check for overshoot (sign change)
-                        prev_error = homing_state["previous_errors"][joint_idx]
-                        if prev_error != 0 and ((prev_error > 0 and error < 0) or (prev_error < 0 and error > 0)):
-                            # Overshoot detected - stop this joint completely
-                            velocities.append(0.0)
-                            continue
-
-                        if abs_error < stop_tolerance:
-                            # Close enough - stop this joint
-                            velocities.append(0.0)
-                        else:
-                            # Proportional control: scale velocity by distance relative to max error
-                            # This ensures all joints reach home at approximately the same time
-                            if max_abs_error > 0:
-                                velocity = base_velocity * (abs_error / max_abs_error)
-                            else:
-                                velocity = 0.0
-
-                            # Apply additional damping when very close to target
-                            # Gradually reduce velocity as we approach the target
-                            damping_zone = tolerance * 3.0  # Start damping at 3x tolerance
-                            if abs_error < damping_zone:
-                                # Quadratic damping: velocity reduces rapidly near target
-                                damping_factor = (abs_error / damping_zone) ** 2
-                                velocity *= damping_factor
-
-                            # Minimum velocity threshold - below this, just stop
-                            min_velocity = 0.02  # rad/s
-                            if velocity < min_velocity:
-                                velocities.append(0.0)
-                            else:
-                                # Apply velocity in correct direction
-                                if error > 0:
-                                    velocities.append(velocity)
-                                else:
-                                    velocities.append(-velocity)
-
-                    # Store current errors for overshoot detection
-                    homing_state["previous_errors"] = errors
-
-                    # Check if all velocities are zero (all joints at target or stopped due to overshoot)
-                    if all(abs(v) < 0.001 for v in velocities):
-                        # All joints stopped - end homing
-                        if homing_state.get("is_jogging", False):
-                            stop_jog()
-                            homing_state["is_jogging"] = False
-                            homing_state["active"] = False
-                            homing_state.pop("previous_errors", None)
-                            homing_state.pop("current_velocities", None)
-                            print("[Homing] All joints reached target or stopped")
-                        return
-
-                    # Start multi-axis jogging with calculated velocities
-                    current_velocities = homing_state.get("current_velocities")
-
-                    # Only restart if velocities changed significantly (avoid unnecessary restarts)
-                    velocities_changed = current_velocities is None or any(
-                        abs(v1 - v2) > 0.01 for v1, v2 in zip(velocities, current_velocities)
-                    )
-
-                    if velocities_changed:
-                        if homing_state.get("is_jogging", False):
-                            stop_jog()
-
-                        start_multi_joint_jog(velocities)
-                        homing_state["is_jogging"] = True
-                        homing_state["current_velocities"] = velocities
-                        homing_state["active"] = True
-
-            elif env is not None:
-                # Instant homing for non-UR5 robots
-                home_pose = getattr(env, "home_pose", None)
-                set_joints = getattr(env, "set_joint_positions", None)
-                if home_pose is not None and callable(set_joints):
-                    if hasattr(home_pose, "tolist"):
-                        positions = home_pose.tolist()
-                    else:
-                        positions = list(home_pose)
-                    set_joints(positions)
-
-    elif msg_type == 'stop_home':
-        # Stop homing when button is released
-        with mujoco_lock, homing_lock:
-            if homing_state["active"]:
-                if env is not None and current_robot in ("ur5", "ur5_t_push"):
-                    stop_jog = getattr(env, "stop_jog", None)
-                    if callable(stop_jog) and homing_state["is_jogging"]:
-                        stop_jog()
-                homing_state["active"] = False
-                homing_state["is_jogging"] = False
-
     elif msg_type == 'camera':
         payload = data.get('data', {})
         a = cam.azimuth * np.pi / 180.0
@@ -1696,6 +1672,9 @@ def handle_ws_message(ws, data):
         vz = float(payload.get('vz', payload.get('dz', 0.0)))
         timestamp = time.time()
 
+        if current_robot in ("ur5", "ur5_t_push") and _is_jogging_paused():
+            return
+
         with mujoco_lock:
             if env is not None and current_robot in ("ur5", "ur5_t_push"):
                 # Check if all velocities are zero - if so, stop any active jogging
@@ -2313,6 +2292,112 @@ def get_env_info():
         return jsonify(response)
 
 
+@app.route(f'{API_PREFIX}/homing', methods=['GET'])
+@_env_request
+def home_blocking():
+    timeout_s = float(request.args.get("timeout_s", 30.0))
+    tolerance = float(request.args.get("tolerance", 0.01))
+    poll_interval = float(request.args.get("poll_interval_s", 0.1))
+
+    if timeout_s <= 0:
+        return jsonify({"error": "timeout_s must be > 0"}), 400
+    if tolerance <= 0:
+        return jsonify({"error": "tolerance must be > 0"}), 400
+    if poll_interval <= 0:
+        return jsonify({"error": "poll_interval_s must be > 0"}), 400
+
+    with mujoco_lock:
+        if env is None:
+            return jsonify({"error": "no environment initialized"}), 503
+
+    if current_robot not in ("ur5", "ur5_t_push"):
+        with mujoco_lock:
+            home_pose = getattr(env, "home_pose", None)
+            set_joints = getattr(env, "set_joint_positions", None)
+            if home_pose is None or not callable(set_joints):
+                return jsonify({"error": "homing not supported for current robot"}), 400
+            positions = home_pose.tolist() if hasattr(home_pose, "tolist") else list(home_pose)
+            set_joints(positions)
+        return jsonify({"status": "completed", "robot": current_robot, "homed": True})
+
+    with mujoco_lock, homing_lock:
+        if homing_state.get("active"):
+            return jsonify({"error": "homing already active"}), 409
+        home_pose = getattr(env, "home_pose", None)
+        get_joints = getattr(env, "get_joint_positions", None)
+        start_multi_joint_jog = getattr(env, "start_multi_joint_jog", None)
+        stop_jog = getattr(env, "stop_jog", None)
+        if home_pose is None or not callable(get_joints) or not callable(start_multi_joint_jog) or not callable(stop_jog):
+            return jsonify({"error": "Nova homing not supported for current robot"}), 400
+        homing_state["active"] = True
+
+    previous_pause = _set_jogging_paused(True)
+    start_time = time.time()
+    status = "running"
+    result_snapshot = {}
+
+    try:
+        with mujoco_lock:
+            stop_jog = getattr(env, "stop_jog", None)
+            if callable(stop_jog):
+                stop_jog()
+
+        while True:
+            with mujoco_lock, homing_lock:
+                result_snapshot = _step_multi_axis_homing_locked(tolerance=tolerance)
+
+            if result_snapshot.get("status") == "done":
+                status = "completed"
+                break
+
+            if time.time() - start_time >= timeout_s:
+                status = "timeout"
+                break
+
+            time.sleep(poll_interval)
+    finally:
+        with mujoco_lock, homing_lock:
+            stop_jog = getattr(env, "stop_jog", None)
+            if callable(stop_jog) and homing_state.get("is_jogging", False):
+                stop_jog()
+            homing_state["active"] = False
+            homing_state["is_jogging"] = False
+            homing_state.pop("previous_errors", None)
+            homing_state.pop("current_velocities", None)
+        _set_jogging_paused(previous_pause)
+
+    with mujoco_lock:
+        home_pose = getattr(env, "home_pose", None)
+        get_joints = getattr(env, "get_joint_positions", None)
+        final_positions = get_joints() if callable(get_joints) else []
+
+    home_pose_list = (
+        home_pose.tolist()
+        if home_pose is not None and hasattr(home_pose, "tolist")
+        else (list(home_pose) if home_pose is not None else None)
+    )
+
+    final_errors = None
+    max_abs_error = None
+    if home_pose_list is not None and final_positions is not None and len(final_positions) > 0:
+        final_errors = [float(t - c) for t, c in zip(home_pose_list, final_positions)]
+        max_abs_error = max(abs(e) for e in final_errors)
+
+    final_positions_list = [float(v) for v in final_positions] if final_positions is not None and len(final_positions) > 0 else None
+    response = {
+        "status": status,
+        "robot": current_robot,
+        "homed": status == "completed",
+        "duration_s": time.time() - start_time,
+        "home_pose": home_pose_list,
+        "final_positions": final_positions_list,
+        "final_errors": final_errors,
+        "max_abs_error": max_abs_error,
+        "jogging_paused": _is_jogging_paused(),
+    }
+    return jsonify(response)
+
+
 
 
 if __name__ == '__main__':

protocol_types.py

@@ -110,16 +110,6 @@ class SwitchRobotMessage(TypedDict):
     data: SwitchRobotData
 
 
-class HomeMessage(TypedDict):
-    """Home robot message (UR5 only)."""
-    type: Literal["home"]
-
-
-class StopHomeMessage(TypedDict):
-    """Stop homing sequence message (UR5 only)."""
-    type: Literal["stop_home"]
-
-
 class CameraRotateData(TypedDict):
     """Camera rotation data."""
     action: Literal["rotate"]
@@ -459,8 +449,6 @@ ClientMessage = Union[
     TeleopActionMessage,
     ResetMessage,
     SwitchRobotMessage,
-    HomeMessage,
-    StopHomeMessage,
     CameraMessage,
     CameraFollowMessage,
     ArmTargetMessage,

tests/test_api.py

@@ -178,17 +178,13 @@ class TestWebSocketMessages:
             print(f"\nInitial joints: {initial_joints}")
             print(f"Home pose: {home_pose}")
 
-            # Send multiple home messages to move robot incrementally
-            # Each message moves 5% closer, so ~20 messages should get very close
-            for i in range(20):
-                ws.send(json.dumps({"type": "home"}))
-                time.sleep(0.1)  # 100ms between messages
-
-                # Read state messages to keep connection alive
-                try:
-                    ws.recv(timeout=0.05)
-                except Exception:
-                    pass
+            # Call blocking homing endpoint
+            homing_resp = requests.get(
+                f"{BASE_URL}/homing",
+                params={"timeout_s": 10, "tolerance": 0.05, "poll_interval_s": 0.1},
+                timeout=15,
+            )
+            homing_resp.raise_for_status()
 
             # Wait for final state messages
             final_state = None

tests/test_client.py

@@ -54,9 +54,19 @@ class NovaSimTestClient:
         self.ws.send(json.dumps(msg))
         self._receive_state()
 
-    def send_home(self):
-        """Send a home message."""
-        self.send_message({"type": "home"})
+    def home_blocking(self, timeout_s: float = 30.0, tolerance: float = 0.01, poll_interval_s: float = 0.1):
+        """Invoke the blocking homing endpoint."""
+        resp = requests.get(
+            f"{self.base_url}/homing",
+            params={
+                "timeout_s": timeout_s,
+                "tolerance": tolerance,
+                "poll_interval_s": poll_interval_s,
+            },
+            timeout=max(5.0, timeout_s + 5.0),
+        )
+        resp.raise_for_status()
+        return resp.json()
 
     def get_joint_positions(self):
         """Get current joint positions from latest state."""

tests/test_homing.py

@@ -18,7 +18,7 @@ def test_homing():
     print("=" * 70)
     print("\nThis test verifies:")
     print("  1. Robot position can be read from state stream")
-    print("  2. Sending 'home' messages triggers multi-axis homing")
+    print("  2. Calling the blocking homing endpoint triggers multi-axis homing")
     print("  3. All joints move simultaneously toward home_pose")
     print("  4. Robot reaches home within acceptable tolerance")
     print("  5. Multi-axis jogging is faster than sequential")
@@ -51,33 +51,16 @@ def test_homing():
             print(f"    ℹ Robot already near home position")
             print(f"    Note: In digital twin mode, robot position comes from real hardware")
 
-        # Start homing by sending home messages continuously
+        # Start homing using the blocking endpoint
         print(f"\n[3] Starting multi-axis homing (Nova API v2)...")
         print("    All joints will move simultaneously toward home")
-        print("    Sending home messages for 30 seconds (increased for large movements)...")
+        print("    Calling /homing with a 30 second timeout...")
 
         home_start_time = time.time()
-        message_count = 0
         joints_before = initial_joints.copy()
-
-        # Send home messages for 30 seconds (increased from 10 for large joint movements)
-        while time.time() - home_start_time < 30.0:
-            client.send_home()
-            message_count += 1
-            time.sleep(0.1)
-
-            # Check progress every second
-            if message_count % 10 == 0:
-                current_joints = client.get_joint_positions()
-                current_errors = [abs(actual - target) for actual, target in zip(current_joints, expected_home)]
-                current_max_error = max(current_errors)
-                elapsed = time.time() - home_start_time
-                print(f"    t={elapsed:.1f}s: max_error={math.degrees(current_max_error):.2f}° ({message_count} messages)")
-
-                # Check if reached home
-                if current_max_error < 0.05:
-                    print(f"\n✓ Homing completed in {elapsed:.2f}s!")
-                    break
+        home_result = client.home_blocking(timeout_s=30.0, tolerance=0.01, poll_interval_s=0.1)
+        elapsed = time.time() - home_start_time
+        print(f"    Homing result: {home_result.get('status')} in {elapsed:.2f}s")
 
         homing_time = time.time() - home_start_time
 
@@ -162,12 +145,11 @@ def test_homing():
 
 
 def test_drift_prevention():
-    """Test that robot stops moving when home messages cease."""
+    """Test that robot doesn't drift after blocking homing completes."""
     print("\n" + "=" * 70)
     print("Drift Prevention Test")
     print("=" * 70)
-    print("\nThis test verifies that the robot stops moving when home")
-    print("messages stop, preventing drift.\n")
+    print("\nThis test verifies that the robot stops moving after blocking homing.\n")
 
     client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")
 
@@ -181,20 +163,14 @@ def test_drift_prevention():
         print(f"\n[1] Initial joint positions:")
         print(f"    [{', '.join(f'{j:7.4f}' for j in initial_joints)}]")
 
-        # Send home messages for 2 seconds (not enough to fully home)
-        print(f"\n[2] Sending home messages for 2 seconds...")
-        duration = 2.0
-        rate_hz = 10
-        num_messages = int(duration * rate_hz)
-
-        for i in range(num_messages):
-            client.send_home()
-            time.sleep(1.0 / rate_hz)
+        # Run homing
+        print(f"\n[2] Calling blocking homing endpoint...")
+        client.home_blocking(timeout_s=10.0, tolerance=0.05, poll_interval_s=0.1)
 
         # Get position right after stopping
         time.sleep(0.1)
         position_after_homing = client.get_joint_positions()
-        print(f"\n[3] Position immediately after stopping home messages:")
+        print(f"\n[3] Position immediately after homing:")
         print(f"    [{', '.join(f'{j:7.4f}' for j in position_after_homing)}]")
 
         # Wait 2 seconds and check if robot drifted
@@ -202,7 +178,7 @@ def test_drift_prevention():
         time.sleep(2.0)
 
         final_position = client.get_joint_positions()
-        print(f"\n[5] Position 2 seconds after stopping:")
+        print(f"\n[5] Position 2 seconds after homing:")
         print(f"    [{', '.join(f'{j:7.4f}' for j in final_position)}]")
 
         # Calculate drift
@@ -228,9 +204,8 @@ def test_drift_prevention():
             print(f"✓ NO DRIFT - Robot stopped properly (drift < {drift_threshold:.4f} rad)")
             success = True
         else:
-            print(f"✗ DRIFT DETECTED - Robot kept moving after home messages stopped!")
-            print(f"   This indicates the jogging command wasn't stopped.")
-            print(f"   Check the timeout mechanism in the simulation loop.")
+            print(f"✗ DRIFT DETECTED - Robot kept moving after homing!")
+            print(f"   Check server logs for jogging stop behavior.")
             success = False
         print("=" * 70)
 
@@ -295,42 +270,9 @@ def test_overshoot_detection():
         print(f"    [{', '.join(f'{j:7.4f}' for j in moved_joints)}]")
         print(f"    Joint 1 error from home: {abs(moved_joints[0] - home_joints[0]):.4f} rad")
 
-        # Now home and monitor for overshoots
-        print(f"\n[4] Starting homing (monitoring for overshoots)...")
-        print(f"    Sending home messages for 15 seconds...")
-
-        duration = 15.0
-        rate_hz = 10
-        num_messages = int(duration * rate_hz)
-
-        prev_j1_error = None
-        overshoot_count = 0
-
-        for i in range(num_messages):
-            # Send home message
-            client.send_home()
-            time.sleep(1.0 / rate_hz)
-
-            # Check position every second
-            if (i + 1) % 10 == 0:
-                current_joints = client.get_joint_positions()
-                j1_error = current_joints[0] - home_joints[0]
-
-                # Check for overshoot (sign change)
-                if prev_j1_error is not None:
-                    if (prev_j1_error > 0 and j1_error < 0) or (prev_j1_error < 0 and j1_error > 0):
-                        overshoot_count += 1
-                        print(f"    t={(i+1)/10:.1f}s: ⚠️  OVERSHOOT #{overshoot_count} detected!")
-                        print(f"           prev_error={prev_j1_error:+.4f} -> current_error={j1_error:+.4f}")
-
-                # Check if close to home
-                if abs(j1_error) < 0.01:
-                    print(f"    t={(i+1)/10:.1f}s: ✓ Reached home (error={abs(j1_error):.4f} rad)")
-                    break
-                else:
-                    print(f"    t={(i+1)/10:.1f}s: j1_error={j1_error:+.4f} rad ({i+1} messages)")
-
-                prev_j1_error = j1_error
+        # Now home using the blocking endpoint
+        print(f"\n[4] Starting homing...")
+        client.home_blocking(timeout_s=15.0, tolerance=0.01, poll_interval_s=0.1)
 
         # Get final position
         time.sleep(0.5)
@@ -349,7 +291,6 @@ def test_overshoot_detection():
             print(f"    {status} Joint {i+1}: error={error:.4f} rad ({math.degrees(error):.2f}°)")
 
         print(f"\n[7] Results:")
-        print(f"    Total overshoots detected: {overshoot_count}")
         print(f"    Max final error: {max_error:.4f} rad ({math.degrees(max_error):.2f}°)")
 
         print("\n" + "=" * 70)
@@ -360,10 +301,7 @@ def test_overshoot_detection():
             print(f"⚠ Homing incomplete (max error = {max_error:.4f} rad)")
             success = False
 
-        if overshoot_count > 0:
-            print(f"ℹ Overshoot detection working ({overshoot_count} overshoots detected and corrected)")
-        else:
-            print(f"✓ No overshoots needed (smooth convergence)")
+        print(f"✓ Homing completed via blocking endpoint")
         print("=" * 70)
 
         client.close()