Enhance homing functionality and client management in mujoco_server.py

- Updated the homing state management to include timeout checks, preventing drift when home messages cease.
- Improved the handling of identified and unidentified clients in the `broadcast_state()` function, ensuring accurate client status reporting.
- Introduced continuous command sending in the NovaJogger class for smoother joint movements during jogging.
- Enhanced the UI to display both identified and unidentified clients, improving user feedback on connection status.
- Added comprehensive tests for homing behavior, including drift prevention and overshoot detection, ensuring robust performance in various scenarios.
- Updated README.md to reflect changes in homing behavior and client management.

README.md

@@ -847,9 +847,7 @@ env = UR5Env(
 )
 ```
 
-**Note**: When state streaming is enabled, the simulation becomes a read-only digital twin that displays the real robot's movements. Local target controls and IK computations are ignored since the robot is controlled by external hardware/software.
-
-**Sending Actions to Robot**: To send motion actions to the real robot through Nova API, use Nova's motion execution endpoints directly (not currently implemented in the web UI). The simulation is designed to visualize robot state, not to control it.
+**Note**: When state streaming is enabled, the simulation becomes a digital twin that mirrors the real robot's state. The robot can still be controlled via Nova API jogging commands (when `use_jogging` is enabled). Local IK target updates are ignored in favor of the real robot's state, but the robot can be moved through Nova API commands.
 
 ### Environment Variables Reference
 
@@ -876,7 +874,7 @@ When `use_state_stream` is enabled:
 4. The simulation updates its joint positions to match the real robot
 5. MuJoCo forward kinematics computes the end-effector pose
 
-**Note:** When state streaming is active, the simulation becomes a digital twin of the real robot.
+**Note:** When state streaming is active, the simulation becomes a digital twin that mirrors the real robot's state. The robot can still be moved via Nova API commands (e.g., jogging).
 
 #### Inverse Kinematics
 When `use_ik` is enabled:

frontend/index.html

@@ -765,9 +765,6 @@
                         style="font-size: 0.85em; color: var(--wb-success); display: flex; align-items: center; gap: 6px;">
                         Nova Connected
                     </strong>
-                    <div style="font-size: 0.75em; color: rgba(255, 255, 255, 0.85);">
-                        <span id="nova_mode_text">Digital Twin Mode</span> - Read Only
-                    </div>
                 </div>
             </div>
         </div>
@@ -1190,19 +1187,30 @@
         const NOVA_JOINT_VELOCITY = 0.5;
         let novaVelocitiesConfigured = false;
 
-        function updateConnectedClients(clients) {
+        function updateConnectedClients(clients, unidentifiedClients = []) {
             if (!clientsStatusCard || !clientsStatusText || !clientsList) {
                 return;
             }
 
             // Update text and list
-            if (!clients || clients.length === 0) {
+            const totalClients = (clients || []).length + (unidentifiedClients || []).length;
+            if (totalClients === 0) {
                 clientsStatusText.innerText = "No clients connected";
                 clientsList.innerHTML = "";
                 clientsList.style.display = "none";
             } else {
-                clientsStatusText.innerText = `${clients.length} client${clients.length > 1 ? 's' : ''} connected`;
-                clientsList.innerHTML = clients.map(id => `<li>${id}</li>`).join('');
+                clientsStatusText.innerText = `${totalClients} client${totalClients > 1 ? 's' : ''} connected`;
+
+                // Build list with identified clients first, then unidentified
+                let html = '';
+                if (clients && clients.length > 0) {
+                    html += clients.map(id => `<li><strong>${id}</strong></li>`).join('');
+                }
+                if (unidentifiedClients && unidentifiedClients.length > 0) {
+                    html += unidentifiedClients.map(id => `<li style="color: #888; font-style: italic;">${id} (not identified)</li>`).join('');
+                }
+
+                clientsList.innerHTML = html;
                 clientsList.style.display = "block";
             }
         }
@@ -1429,6 +1437,12 @@
                 metadataCache = await resp.json();
                 const selection = populateRobotOptions(metadataCache);
                 if (selection) {
+                    // Set pending selection so the backend switches to the default robot when WebSocket opens
+                    pendingRobotSelection = {
+                        value: buildSelectionValue(selection.robot, selection.scene),
+                        robot: selection.robot,
+                        scene: selection.scene
+                    };
                     updateRobotUI(selection.robot, selection.scene);
                     refreshOverlayTiles();
                 }
@@ -1471,6 +1485,12 @@
             loadEnvData();
 
             ws.onopen = () => {
+                // Send client identification immediately upon connection
+                send('client_identity', {
+                    client_id: 'ui',
+                    name: 'UI Client'
+                });
+
                 markConnectedState();
                 refreshVideoStreams();
                 refreshOverlayTiles();
@@ -1540,8 +1560,10 @@
                         }
 
                         // Handle both new and legacy client status formats
-                        if (data.connected_clients && Array.isArray(data.connected_clients)) {
-                            updateConnectedClients(data.connected_clients);
+                        if (data.connected_clients !== undefined) {
+                            const clients = data.connected_clients || [];
+                            const unidentifiedClients = data.unidentified_clients || [];
+                            updateConnectedClients(clients, unidentifiedClients);
                         } else if (typeof data.trainer_connected === 'boolean') {
                             // Legacy format
                             updateTrainerStatus(data.trainer_connected);
@@ -1659,6 +1681,9 @@
                                     const badgeTitle = document.getElementById('nova_badge_title');
                                     const modeText = document.getElementById('nova_mode_text');
 
+                                    // Check if this is the first update (all prev values are null)
+                                    const isFirstUpdate = prevNovaState.available === null && prevNovaState.enabled === null;
+
                                     // Only update toggle state if available or enabled changed
                                     if (prevNovaState.available !== novaApi.available || prevNovaState.enabled !== novaApi.enabled) {
                                         setNovaToggleState(novaApi.available, novaApi.enabled);
@@ -1695,12 +1720,14 @@
 
                                     // Update badge visibility and classes
                                     if (badge) {
-                                        if (prevNovaState.available !== novaApi.available) {
+                                        // Always update on first load, or when values change
+                                        if (isFirstUpdate || prevNovaState.available !== novaApi.available) {
                                             badge.style.display = novaApi.available ? 'block' : 'none';
                                         }
-                                        if (prevNovaState.enabled !== novaApi.enabled) {
-                                            badge.classList.toggle('connected', novaApi.enabled);
-                                            badge.classList.toggle('disconnected', !novaApi.enabled);
+                                        if (isFirstUpdate || prevNovaState.enabled !== novaApi.enabled) {
+                                            const isEnabled = Boolean(novaApi.enabled);
+                                            badge.classList.toggle('connected', isEnabled);
+                                            badge.classList.toggle('disconnected', !isEnabled);
                                         }
                                     }
 
@@ -1710,13 +1737,13 @@
                                         stateController.innerText = novaApi.state_streaming ? 'Nova API' : 'Internal';
                                         stateController.style.color = connectedColor;
                                     }
-                                    if (ikController && (prevNovaState.ik !== novaApi.ik)) {
+                                    if (ikController && (prevNovaState.ik !== novaApi.ik || prevNovaState.connected !== novaApi.connected)) {
                                         ikController.innerText = novaApi.ik ? 'Nova API' : 'Internal';
-                                        ikController.style.color = novaApi.ik ? 'var(--wb-success)' : 'var(--wb-highlight)';
+                                        ikController.style.color = novaApi.ik && novaApi.connected ? 'var(--wb-success)' : 'var(--wb-highlight)';
                                     }
 
                                     // Update badge title
-                                    if (badgeTitle && (prevNovaState.enabled !== novaApi.enabled || prevNovaState.connected !== novaApi.connected)) {
+                                    if (badgeTitle && (isFirstUpdate || prevNovaState.enabled !== novaApi.enabled || prevNovaState.connected !== novaApi.connected)) {
                                         if (novaApi.enabled) {
                                             badgeTitle.innerText = novaApi.connected ? '🌐 Nova Connected' : '🌐 Nova Powering Up';
                                         } else {
@@ -1727,16 +1754,22 @@
                                     // Update mode text
                                     if (modeText) {
                                         let newModeText = '';
-                                        if (novaApi.connected) {
-                                            if (novaApi.state_streaming && novaApi.ik) {
+                                        if (novaApi.enabled) {
+                                            // Check what Nova features are active
+                                            const hasStateStream = novaApi.state_streaming && novaApi.connected;
+                                            const hasIK = novaApi.ik;
+
+                                            if (hasStateStream && hasIK) {
                                                 newModeText = 'Hybrid Mode';
-                                            } else if (novaApi.state_streaming) {
+                                            } else if (hasStateStream) {
                                                 newModeText = 'Digital Twin Mode';
-                                            } else if (novaApi.ik) {
-                                                newModeText = 'Nova IK Mode';
+                                            } else if (hasIK) {
+                                                newModeText = novaApi.connected ? 'Nova IK Mode' : 'Nova IK Mode (connecting...)';
+                                            } else if (novaApi.connected) {
+                                                newModeText = 'Nova Connected (idle)';
+                                            } else {
+                                                newModeText = 'Awaiting connection';
                                             }
-                                        } else if (novaApi.enabled) {
-                                            newModeText = 'Awaiting connection';
                                         } else {
                                             newModeText = 'Internal control';
                                         }
@@ -1836,9 +1869,9 @@
                         }
                     } else if (msg.type === 'connected_clients') {
                         const payload = msg.data || {};
-                        if (payload.clients && Array.isArray(payload.clients)) {
-                            updateConnectedClients(payload.clients);
-                        }
+                        const clients = payload.clients || [];
+                        const unidentifiedClients = payload.unidentified_clients || [];
+                        updateConnectedClients(clients, unidentifiedClients);
                     } else if (msg.type === 'trainer_status') {
                         // Legacy support
                         const payload = msg.data || {};
@@ -1953,6 +1986,11 @@
             if (sceneLabel) {
                 sceneLabel.innerText = humanizeScene(scene);
             }
+            // Sync robot selector with active robot
+            const expectedValue = buildSelectionValue(robot, scene);
+            if (robotSelect.value !== expectedValue) {
+                robotSelect.value = expectedValue;
+            }
             // Toggle controls based on robot type
             if (robot === 'ur5' || robot === 'ur5_t_push') {
                 locomotionControls.classList.add('hidden');

mujoco_server.py

@@ -127,12 +127,13 @@ homing_state = {
     "active": False,
     "current_joint": None,
     "joint_errors": [],
-    "current_velocity": 0.2,
+    "current_velocity": 0.5,  # Max velocity for homing
     "previous_error": None,
     "previous_abs_error": None,
     "stuck_count": 0,
     "is_jogging": False,
     "current_direction": None,
+    "saved_velocity": None,  # Store velocity to restore after homing
 }
 homing_lock = threading.Lock()
 
@@ -622,8 +623,19 @@ def broadcast_state():
         if hasattr(env, "get_task_reward"):
             reward_value = env.get_task_reward()
 
-        with external_ws_clients_lock:
-            connected_clients = [info.get("identity", "client") for ws, info in list(client_metadata.items()) if ws in external_ws_clients]
+        # Build lists of identified and unidentified clients
+        with client_metadata_lock:
+            identified_clients = []
+            unidentified_clients = []
+            for ws, info in list(client_metadata.items()):
+                identity = info.get("identity")
+                if identity:
+                    # Client has identified themselves
+                    if ws in external_ws_clients:
+                        identified_clients.append(identity)
+                else:
+                    # Client connected but hasn't sent identity yet
+                    unidentified_clients.append(f"unidentified-{id(ws) % 10000}")
 
         # Get scene objects
         scene_objects = _get_scene_objects()
@@ -645,11 +657,17 @@ def broadcast_state():
             nova_ik = getattr(env, '_use_nova_ik', False)
             # Check if actually connected (WebSocket active and receiving data)
             nova_connected = False
-            if nova_client is not None and hasattr(nova_client, 'is_state_stream_connected'):
-                nova_connected = nova_client.is_state_stream_connected()
-            elif nova_client is not None and not nova_state_streaming:
-                # If not using state streaming, consider connected if client exists
-                nova_connected = True
+            if nova_client is not None:
+                if hasattr(nova_client, 'is_state_stream_connected'):
+                    # Check actual stream connection status
+                    nova_connected = nova_client.is_state_stream_connected()
+                elif hasattr(nova_client, 'is_connected'):
+                    # Fallback: check general connection status
+                    nova_connected = nova_client.is_connected()
+                else:
+                    # If client exists but we can't verify connection, assume connected
+                    # This handles the IK-only case where state streaming isn't used
+                    nova_connected = True
 
             nova_available = False
             nova_enabled_pref = False
@@ -687,7 +705,8 @@ def broadcast_state():
                         'available': nova_available,
                         'enabled': nova_enabled_pref
                     },
-                    'connected_clients': connected_clients
+                    'connected_clients': identified_clients,
+                    'unidentified_clients': unidentified_clients
                 }
             })
         else:
@@ -708,7 +727,8 @@ def broadcast_state():
                     'reward': reward_value,
                     'teleop_action': teleop_snapshot,
                     'scene_objects': scene_objects,
-                    'connected_clients': connected_clients
+                    'connected_clients': identified_clients,
+                    'unidentified_clients': unidentified_clients
                 }
             })
 
@@ -732,7 +752,7 @@ def _handle_external_client_message(ws, data):
         identity = payload.get("client_id") or payload.get("trainer_id") or payload.get("name") or "client"
         with external_ws_clients_lock:
             external_ws_clients.add(ws)
-        _register_external_client(ws)
+        # Don't call _register_external_client again - client is already registered on connection
         _set_client_identity(ws, identity)
         broadcast_state()
         broadcast_connected_clients_status()
@@ -746,9 +766,21 @@ def _handle_external_client_message(ws, data):
 def _build_connected_clients_payload():
     """Build a summary payload describing connected external clients."""
     with client_metadata_lock:
-        identities = [info.get("identity", "client") for ws, info in client_metadata.items() if ws in external_ws_clients and info.get("identity")]
+        identified = []
+        unidentified = []
+        for ws, info in client_metadata.items():
+            identity = info.get("identity")
+            if identity:
+                # Client has identified themselves
+                if ws in external_ws_clients:
+                    identified.append(identity)
+            else:
+                # Client connected but hasn't sent identity yet
+                unidentified.append(f"unidentified-{id(ws) % 10000}")
+
     payload = {
-        "clients": identities,
+        "clients": identified,
+        "unidentified_clients": unidentified,
     }
     return payload
 
@@ -767,6 +799,9 @@ def _set_client_identity(ws, identity: str):
     with client_metadata_lock:
         info = client_metadata.get(ws)
         if not info:
+            # Client wasn't registered on connection (shouldn't happen with new flow)
+            # Register them now for backward compatibility
+            client_metadata[ws] = {"identity": identity, "connected_at": time.time(), "last_seen": time.time()}
             return
         info["identity"] = identity
         info["last_seen"] = time.time()
@@ -879,6 +914,19 @@ 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
@@ -1122,9 +1170,12 @@ def handle_ws_message(ws, data):
         _schedule_robot_switch(robot, scene)
 
     elif msg_type == 'home':
-        # State-based homing: processes one step each time a 'home' message is received
+        # Simplified homing with overshoot detection and drift prevention
         # 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)
@@ -1140,121 +1191,87 @@ def handle_ws_message(ws, data):
                     current_positions = get_joints()
                     tolerance = 0.01  # rad (~0.57 degrees)
 
-                    # Initialize homing state if not active
-                    if not homing_state["active"]:
-                        # Compute differences for all joints
-                        joint_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
-                            # Wrap angle to [-pi, pi]
-                            error = (error + np.pi) % (2 * np.pi) - np.pi
-                            abs_error = abs(error)
-
-                            if abs_error > tolerance:
-                                joint_errors.append({
-                                    'index': joint_idx,
-                                    'error': error,
-                                    'abs_error': abs_error
-                                })
-
-                        if joint_errors:
-                            # Sort by absolute error (largest first)
-                            joint_errors.sort(key=lambda x: x['abs_error'], reverse=True)
-                            homing_state["active"] = True
-                            homing_state["joint_errors"] = joint_errors
-                            homing_state["current_joint"] = joint_errors[0]['index']
-                            homing_state["current_velocity"] = 0.15
-                            homing_state["previous_error"] = None
-                            homing_state["previous_abs_error"] = None
-                            homing_state["stuck_count"] = 0
-                            homing_state["is_jogging"] = False
-                            homing_state["current_direction"] = None
-                        else:
-                            return
-
-                    # Process current joint
-                    joint_idx = homing_state["current_joint"]
-                    current_pos = current_positions[joint_idx]
-                    target_pos = target_positions[joint_idx]
-
-                    # Compute shortest angular distance
-                    error = target_pos - current_pos
-                    error = (error + np.pi) % (2 * np.pi) - np.pi
-                    abs_error = abs(error)
-
-                    # Check if current joint reached target
-                    if abs_error <= tolerance:
-                        if homing_state["is_jogging"]:
+                    # Find the joint with the largest error
+                    max_error = 0
+                    max_error_joint = None
+
+                    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
+                        error = target - current
+                        error = (error + np.pi) % (2 * np.pi) - np.pi
+                        abs_error = abs(error)
+
+                        if abs_error > max_error and abs_error > tolerance:
+                            max_error = abs_error
+                            max_error_joint = (joint_idx, error)
+
+                    # If no joint needs homing, we're done
+                    if max_error_joint is None:
+                        if homing_state.get("is_jogging", False):
                             stop_jog()
                             homing_state["is_jogging"] = False
-
-                        # Move to next joint
-                        homing_state["joint_errors"].pop(0)
-                        if homing_state["joint_errors"]:
-                            homing_state["current_joint"] = homing_state["joint_errors"][0]['index']
-                            homing_state["current_velocity"] = 0.15
-                            homing_state["previous_error"] = None
-                            homing_state["previous_abs_error"] = None
-                            homing_state["stuck_count"] = 0
-                            homing_state["is_jogging"] = False
-                            homing_state["current_direction"] = None
-                        else:
-                            # All joints homed
                             homing_state["active"] = False
+                            homing_state.pop("joint_velocities", None)
+                            homing_state.pop("previous_errors", None)
                         return
 
-                    # Check if stuck
-                    if homing_state["previous_abs_error"] is not None:
-                        if abs(abs_error - homing_state["previous_abs_error"]) < 0.0001:
-                            homing_state["stuck_count"] += 1
-                            if homing_state["stuck_count"] > 20:  # Stuck for ~2 seconds
-                                if abs_error <= tolerance * 3:
-                                    if homing_state["is_jogging"]:
-                                        stop_jog()
-                                        homing_state["is_jogging"] = False
-                                    # Move to next joint
-                                    homing_state["joint_errors"].pop(0)
-                                    if homing_state["joint_errors"]:
-                                        homing_state["current_joint"] = homing_state["joint_errors"][0]['index']
-                                        homing_state["current_velocity"] = 0.15
-                                        homing_state["previous_error"] = None
-                                        homing_state["previous_abs_error"] = None
-                                        homing_state["stuck_count"] = 0
-                                        homing_state["is_jogging"] = False
-                                        homing_state["current_direction"] = None
-                                    else:
-                                        homing_state["active"] = False
-                                    return
-                        else:
-                            homing_state["stuck_count"] = 0
-
-                    # Check for overshoot
-                    if homing_state["previous_error"] is not None:
-                        prev_err = homing_state["previous_error"]
-                        if (prev_err > 0 and error < 0) or (prev_err < 0 and error > 0):
-                            homing_state["current_velocity"] = homing_state["current_velocity"] / 2.0
-                            homing_state["current_velocity"] = max(0.005, homing_state["current_velocity"])
-                            if homing_state["is_jogging"]:
-                                stop_jog()
-                                homing_state["is_jogging"] = False
-
-                    # Calculate velocity and direction
+                    # Jog the joint with the largest error
+                    joint_idx, error = max_error_joint
                     direction = '+' if error > 0 else '-'
-                    desired_velocity = min(homing_state["current_velocity"], max(0.005, abs_error * 1.0))
 
-                    # Start/restart jogging if needed
-                    if homing_state["current_direction"] != direction or not homing_state["is_jogging"]:
-                        if homing_state["is_jogging"]:
+                    # Initialize velocity tracking per joint if needed
+                    if "joint_velocities" not in homing_state:
+                        homing_state["joint_velocities"] = {i: 0.5 for i in range(6)}
+                    if "previous_errors" not in homing_state:
+                        homing_state["previous_errors"] = {}
+
+                    # Check for overshoot (direction changed)
+                    prev_error = homing_state["previous_errors"].get(joint_idx)
+                    if prev_error is not None:
+                        # If error sign changed, we overshot - reduce velocity aggressively
+                        if (prev_error > 0 and error < 0) or (prev_error < 0 and error > 0):
+                            homing_state["joint_velocities"][joint_idx] *= 0.3  # More aggressive reduction
+                            # Minimum velocity to keep moving
+                            homing_state["joint_velocities"][joint_idx] = max(0.05, homing_state["joint_velocities"][joint_idx])
+
+                    # Store current error for next iteration
+                    homing_state["previous_errors"][joint_idx] = error
+
+                    # Calculate velocity based on distance
+                    # When far from target: use stored velocity (default 0.5 rad/s)
+                    # When close to target: scale velocity proportionally to distance
+                    abs_error = abs(error)
+                    base_velocity = homing_state["joint_velocities"][joint_idx]
+
+                    # Define distance threshold for proportional control (0.15 rad ≈ 8.6°)
+                    proximity_threshold = 0.15
+                    min_velocity = 0.1  # Minimum velocity to ensure convergence
+
+                    if abs_error < proximity_threshold:
+                        # Proportional control: velocity scales linearly with distance
+                        # Velocity range: min_velocity to base_velocity
+                        # Formula: v = min_vel + (base_vel - min_vel) * (error / threshold)
+                        velocity_range = base_velocity - min_velocity
+                        velocity = min_velocity + velocity_range * (abs_error / proximity_threshold)
+                    else:
+                        # Far from target: use base velocity
+                        velocity = base_velocity
+
+                    # Only call start_jog if we're not already jogging this joint in this direction
+                    current_jog = homing_state.get("current_jog")
+                    new_jog = (joint_idx, direction, velocity)
+
+                    # Restart jog if joint, direction, or velocity changed
+                    if current_jog != new_jog:
+                        if homing_state.get("is_jogging", False):
                             stop_jog()
 
-                        start_jog('joint', joint=joint_idx + 1, direction=direction, velocity=desired_velocity)
+                        start_jog('joint', joint=joint_idx + 1, direction=direction, velocity=velocity)
                         homing_state["is_jogging"] = True
-                        homing_state["current_direction"] = direction
-
-                    homing_state["previous_error"] = error
-                    homing_state["previous_abs_error"] = abs_error
+                        homing_state["current_jog"] = new_jog
+                        homing_state["active"] = True
 
             elif env is not None:
                 # Instant homing for non-UR5 robots
@@ -1500,12 +1517,19 @@ def _create_env(robot, scene):
 @sock.route(f'{API_PREFIX}/ws')
 def websocket_handler(ws):
     """Handle WebSocket connections."""
-    print('WebSocket client connected')
+    with open('/tmp/ws_debug.log', 'a') as f:
+        f.write('WebSocket client connected\n')
+        f.flush()
+    print('WebSocket client connected', flush=True)
 
-    # Register client
+    # Register client (track all clients, even before they identify)
     with ws_clients_lock:
         ws_clients.add(ws)
 
+    # Register in metadata as unidentified
+    _register_external_client(ws)
+    broadcast_connected_clients_status()
+
     try:
         while True:
             message = ws.receive()
@@ -1513,6 +1537,10 @@ def websocket_handler(ws):
                 break
             try:
                 data = json.loads(message)
+                with open('/tmp/ws_debug.log', 'a') as f:
+                    f.write(f"Received message type: {data.get('type', 'unknown')}\n")
+                    f.flush()
+                print(f"[WS] Received message type: {data.get('type', 'unknown')}")
                 handle_ws_message(ws, data)
             except json.JSONDecodeError:
                 print(f"Invalid JSON received: {message}")
@@ -1527,10 +1555,10 @@ def websocket_handler(ws):
         with external_ws_clients_lock:
             was_external = ws in external_ws_clients
             external_ws_clients.discard(ws)
-        if was_external:
-            _unregister_external_client(ws)
-            broadcast_state()
-            broadcast_connected_clients_status()
+        # Always unregister from metadata (all clients are now tracked, not just external ones)
+        _unregister_external_client(ws)
+        broadcast_state()
+        broadcast_connected_clients_status()
         print('WebSocket client disconnected')
 
 

robots/ur5/model/scene.xml

@@ -345,7 +345,7 @@
   </actuator>
 
   <keyframe>
-    <!-- Official MuJoCo Menagerie UR5e home pose with gripper open (0=open) -->
+    <!-- UR5e home pose updated to stable settled position with gripper open (0=open) -->
     <key name="home" qpos="-3.22 -1.14 1.93 -2.36 -1.57 -1.72 0 0 0 0 0 0 0 0 0.5 0.2 0.45 1 0 0 0"
          ctrl="-3.22 -1.14 1.93 -2.36 -1.57 -1.72 0"/>
   </keyframe>

robots/ur5/model/scene_with_gripper.xml

@@ -185,7 +185,7 @@
   </actuator>
 
   <keyframe>
-    <!-- Official MuJoCo Menagerie UR5e home pose with gripper open (0=open) -->
+    <!-- UR5e home pose updated to stable settled position with gripper open (0=open) -->
     <key name="home" qpos="-3.22 -1.14 1.93 -2.36 -1.57 -1.72 0 0 0 0 0 0 0 0 0.5 0.2 0.45 1 0 0 0"
          ctrl="-3.22 -1.14 1.93 -2.36 -1.57 -1.72 0"/>
   </keyframe>

robots/ur5/nova_jogger.py

@@ -66,6 +66,41 @@ class NovaJogger:
         self._receive_thread = None
         self._stop_thread = False
 
+        # Continuous command sending
+        self._send_thread = None
+        self._stop_send_thread = False
+        self._current_command: Optional[Dict[str, Any]] = None
+        self._command_rate = 0.1  # Send command every 100ms
+
+    def _send_loop(self):
+        """Background thread to send velocity commands continuously."""
+        import sys
+        print(f"[Nova Jogger] Send loop started", flush=True)
+        sys.stdout.flush()
+        send_count = 0
+        while not self._stop_send_thread:
+            try:
+                with self._lock:
+                    if self._current_command and self._jogger_ws:
+                        self._jogger_ws.send(json.dumps(self._current_command))
+                        send_count += 1
+                        if send_count == 1:  # Log first send
+                            print(f"[Nova Jogger] First command sent", flush=True)
+                            sys.stdout.flush()
+                        if send_count % 10 == 0:  # Log every 10 sends (every second)
+                            print(f"[Nova Jogger] Sent {send_count} commands (continuous mode active)", flush=True)
+                            sys.stdout.flush()
+                time.sleep(self._command_rate)
+            except Exception as e:
+                if not self._stop_send_thread:
+                    print(f"[Nova Jogger] Send error: {e}", flush=True)
+                    sys.stdout.flush()
+                    import traceback
+                    traceback.print_exc()
+                break
+        print(f"[Nova Jogger] Send loop exited (sent {send_count} total commands)", flush=True)
+        sys.stdout.flush()
+
     def _receive_loop(self):
         """Background thread to receive WebSocket messages."""
         while not self._stop_thread and self._jogger_ws is not None:
@@ -138,6 +173,12 @@ class NovaJogger:
                 self._receive_thread = threading.Thread(target=self._receive_loop, daemon=True)
                 self._receive_thread.start()
 
+                # Start send loop for continuous commands
+                self._stop_send_thread = False
+                self._send_thread = threading.Thread(target=self._send_loop, daemon=True)
+                self._send_thread.start()
+                print(f"[Nova Jogger] Send thread started (continuous command mode)")
+
                 self._current_mode = mode
                 print(f"[Nova Jogger] Connected to {mode} endpoint")
                 return True
@@ -169,13 +210,15 @@ class NovaJogger:
             else:
                 joint_velocities[joint] = -velocity_rads_per_sec
 
-            # Send velocity command (matching nova-js format)
+            # Store command for continuous sending
             command = {
                 "motion_group": self.motion_group_id,
                 "joint_velocities": joint_velocities
             }
-            self._jogger_ws.send(json.dumps(command))
-            print(f"[Nova Jogger] Joint {joint} jogging at {joint_velocities[joint]:.2f} rad/s")
+            with self._lock:
+                self._current_command = command
+
+            print(f"[Nova Jogger] Joint {joint} jogging at {joint_velocities[joint]:.2f} rad/s (continuous)")
             return True
 
         except Exception as e:
@@ -198,7 +241,7 @@ class NovaJogger:
             else:
                 position_direction[axis] = -1
 
-            # Send velocity command (matching nova-js format)
+            # Store command for continuous sending
             command = {
                 "motion_group": self.motion_group_id,
                 "position_direction": position_direction,
@@ -208,8 +251,10 @@ class NovaJogger:
                 "tcp": tcp_id,
                 "coordinate_system": coord_system_id
             }
-            self._jogger_ws.send(json.dumps(command))
-            print(f"[Nova Jogger] Cartesian {axis.upper()} translation at {velocity_mm_per_sec} mm/s (dir: {position_direction[axis]})")
+            with self._lock:
+                self._current_command = command
+
+            print(f"[Nova Jogger] Cartesian {axis.upper()} translation at {velocity_mm_per_sec} mm/s (dir: {position_direction[axis]}, continuous)")
             return True
 
         except Exception as e:
@@ -232,7 +277,7 @@ class NovaJogger:
             else:
                 rotation_direction[axis] = -1
 
-            # Send velocity command (matching nova-js format)
+            # Store command for continuous sending
             command = {
                 "motion_group": self.motion_group_id,
                 "position_direction": {"x": 0, "y": 0, "z": 0},
@@ -242,8 +287,10 @@ class NovaJogger:
                 "tcp": tcp_id,
                 "coordinate_system": coord_system_id
             }
-            self._jogger_ws.send(json.dumps(command))
-            print(f"[Nova Jogger] Cartesian {axis.upper()} rotation at {velocity_rads_per_sec} rad/s (dir: {rotation_direction[axis]})")
+            with self._lock:
+                self._current_command = command
+
+            print(f"[Nova Jogger] Cartesian {axis.upper()} rotation at {velocity_rads_per_sec} rad/s (dir: {rotation_direction[axis]}, continuous)")
             return True
 
         except Exception as e:
@@ -251,33 +298,40 @@ class NovaJogger:
             return False
 
     def stop(self) -> bool:
-        """Stop any active jogging movement by sending zero velocities."""
-        if not self._connected or self._jogger_ws is None:
+        """Stop any active jogging movement by clearing the command."""
+        if not self._connected:
             return False
 
         try:
-            if self._current_mode == "joint":
-                # Send zero velocities for all joints
-                command = {
-                    "motion_group": self.motion_group_id,
-                    "joint_velocities": [0.0] * 6
-                }
-            elif self._current_mode == "cartesian":
-                # Send zero velocities for cartesian
-                command = {
-                    "motion_group": self.motion_group_id,
-                    "position_direction": {"x": 0, "y": 0, "z": 0},
-                    "rotation_direction": {"x": 0, "y": 0, "z": 0},
-                    "position_velocity": 0,
-                    "rotation_velocity": 0,
-                    "tcp": getattr(self, '_tcp_id', 'Flange'),
-                    "coordinate_system": getattr(self, '_coord_system_id', 'world')
-                }
-            else:
-                return False
-
-            self._jogger_ws.send(json.dumps(command))
-            print("[Nova Jogger] Stopped (zero velocities sent)")
+            # Clear the current command so the send loop stops sending
+            with self._lock:
+                self._current_command = None
+
+            # Send one final zero-velocity command
+            if self._jogger_ws:
+                if self._current_mode == "joint":
+                    # Send zero velocities for all joints
+                    command = {
+                        "motion_group": self.motion_group_id,
+                        "joint_velocities": [0.0] * 6
+                    }
+                elif self._current_mode == "cartesian":
+                    # Send zero velocities for cartesian
+                    command = {
+                        "motion_group": self.motion_group_id,
+                        "position_direction": {"x": 0, "y": 0, "z": 0},
+                        "rotation_direction": {"x": 0, "y": 0, "z": 0},
+                        "position_velocity": 0,
+                        "rotation_velocity": 0,
+                        "tcp": getattr(self, '_tcp_id', 'Flange'),
+                        "coordinate_system": getattr(self, '_coord_system_id', 'world')
+                    }
+                else:
+                    return False
+
+                self._jogger_ws.send(json.dumps(command))
+
+            print("[Nova Jogger] Stopped (cleared command)")
             return True
 
         except Exception as e:
@@ -286,7 +340,13 @@ class NovaJogger:
 
     def disconnect(self):
         """Disconnect from the jogger."""
-        # Stop the receive thread first
+        # Stop the send thread first
+        self._stop_send_thread = True
+        if self._send_thread is not None:
+            self._send_thread.join(timeout=2.0)
+            self._send_thread = None
+
+        # Stop the receive thread
         self._stop_thread = True
         if self._receive_thread is not None:
             self._receive_thread.join(timeout=2.0)

robots/ur5/ur5_env.py

@@ -171,11 +171,15 @@ class UR5Env(gym.Env):
         self._nova_api_config = dict(nova_api_config) if nova_api_config else None
         self._target_pos = None
         self._target_euler = None
-        self._nova_user_enabled = False
+        # Initialize Nova enabled state based on config presence to avoid race condition
+        # This will be set to the actual success/failure status after configuration attempt
+        self._nova_user_enabled = bool(self._nova_api_config)
         if self._nova_api_config:
             enabled = self.set_nova_enabled(True)
             if not enabled:
                 print("[Nova] WARNING: Nova API configuration was provided but failed to initialize.")
+                # Update to reflect actual failure
+                self._nova_user_enabled = False
 
         # IK controller for end-effector control
         self.controller = IKController(
@@ -190,6 +194,9 @@ class UR5Env(gym.Env):
         self.home_pose = loaded_pose if loaded_pose is not None else self.DEFAULT_HOME_POSE.copy()
         self.data.qpos[:6] = self.home_pose.copy()
         self.data.ctrl[:6] = self.home_pose.copy()
+        # Run physics for several steps to let the controllers stabilize at home position
+        for _ in range(100):
+            mujoco.mj_step(self.model, self.data)
         mujoco.mj_forward(self.model, self.data)
 
         # Target position/orientation from FK of home pose
@@ -210,7 +217,8 @@ class UR5Env(gym.Env):
         # Control mode: 'ik' (end-effector target) or 'joint' (direct joint positions)
         self._control_mode = 'ik'
         # Direct joint targets (used when control_mode is 'joint')
-        self._joint_targets = self._compute_joint_targets()
+        # Initialize to home pose instead of computing from IK to ensure robot starts at home
+        self._joint_targets = self.home_pose.copy()
 
         # Configure Nova API if requested (must be after Nova attributes are initialized)
         if nova_api_config:
@@ -272,8 +280,8 @@ class UR5Env(gym.Env):
                 print("[Nova] Starting state stream...")
                 self._nova_client.start_state_stream()
                 print("[Nova] State stream started successfully")
-                print("[Nova] WARNING: State streaming enabled - simulation is now a digital twin.")
-                print("[Nova]          Robot state comes from Nova API, local targets/IK are ignored.")
+                print("[Nova] Digital twin mode: Simulation mirrors real robot state from Nova API.")
+                print("[Nova] Robot can still be moved via jogging commands sent to Nova API.")
 
             if self._use_nova_jogging:
                 if nova_jogger is None:
@@ -518,14 +526,33 @@ class UR5Env(gym.Env):
             True if command sent successfully
         """
         # Try Nova jogging first if available
+        print(f"[Jog] start_jog called: type={jog_type}, kwargs={kwargs}")
+        print(f"[Jog] Checking Nova jogging: _use_nova_jogging={self._use_nova_jogging}, _nova_jogger={self._nova_jogger is not None}")
+        with open('/tmp/ws_debug.log', 'a') as f:
+            f.write(f"[Jog] start_jog called: type={jog_type}, kwargs={kwargs}\n")
+            f.write(f"[Jog] _use_nova_jogging={self._use_nova_jogging}, _nova_jogger={self._nova_jogger is not None}\n")
+            f.flush()
         if self._use_nova_jogging and self._nova_jogger is not None:
             try:
                 if jog_type == 'joint':
-                    return self._nova_jogger.start_joint_jog(
-                        joint=max(0, min(5, kwargs.get('joint', 1) - 1)),
-                        direction=kwargs.get('direction', '+'),
-                        velocity_rads_per_sec=kwargs.get('velocity', 0.5)
+                    joint_num = kwargs.get('joint', 1)
+                    joint_idx = max(0, min(5, joint_num - 1))
+                    direction = kwargs.get('direction', '+')
+                    velocity = kwargs.get('velocity', 0.5)
+                    print(f"[Jog] Calling Nova jogger: joint={joint_idx} ({joint_num}), dir={direction}, vel={velocity:.3f}")
+                    with open('/tmp/ws_debug.log', 'a') as f:
+                        f.write(f"[Jog] Calling Nova jogger: joint={joint_idx} ({joint_num}), dir={direction}, vel={velocity:.3f}\n")
+                        f.flush()
+                    result = self._nova_jogger.start_joint_jog(
+                        joint=joint_idx,
+                        direction=direction,
+                        velocity_rads_per_sec=velocity
                     )
+                    print(f"[Jog] Nova jogger result: {result}")
+                    with open('/tmp/ws_debug.log', 'a') as f:
+                        f.write(f"[Jog] Nova jogger result: {result}\n")
+                        f.flush()
+                    return result
                 elif jog_type == 'cartesian_translation':
                     return self._nova_jogger.start_cartesian_translation(
                         axis=kwargs.get('axis', 'x'),
@@ -750,6 +777,10 @@ class UR5Env(gym.Env):
         elif self.t_target_body_id != -1:
             self._reset_freejoint("t_object_joint", [0.45, 0.2, 0.43], [1, 0, 0, 0])
 
+        # Run physics for several steps to let the robot stabilize at home position
+        for _ in range(100):
+            mujoco.mj_step(self.model, self.data)
+
         # Compute forward kinematics to get EE pose from home joints
         mujoco.mj_forward(self.model, self.data)
 
@@ -826,8 +857,9 @@ class UR5Env(gym.Env):
     def step_with_controller(self, dt: float = 0.002):
         """Step using controller (IK or direct joint control)."""
         if self._use_nova_state_stream:
-            # In digital twin mode, sync robot pose from Nova but still run physics
+            # In digital twin mode: sync robot pose from Nova API and run physics
             # This allows simulated objects (like T-shape) to interact with the real robot
+            # Note: Robot can still be moved via Nova API jogging commands
             self._sync_nova_state()
 
             # Set robot joint positions from Nova (override qpos)

tests/test_client.py

@@ -0,0 +1,68 @@
+"""Lightweight WebSocket test client for nova-sim tests (no external dependencies)."""
+
+import json
+import time
+import requests
+from websockets.sync.client import connect
+
+
+class NovaSimTestClient:
+    """Simple WebSocket client for testing nova-sim."""
+
+    def __init__(self, base_url: str = "http://localhost:3004/nova-sim/api/v1"):
+        self.base_url = base_url
+        self.ws = None
+        self.latest_state = {}
+
+    def connect(self):
+        """Connect to nova-sim via WebSocket."""
+        # Build WebSocket URL
+        ws_url = self.base_url.replace("http://", "ws://").replace("https://", "wss://") + "/ws"
+
+        # Connect WebSocket
+        self.ws = connect(ws_url, timeout=10, ping_interval=None)
+
+        # Send client identity
+        self.ws.send(json.dumps({
+            "type": "client_identity",
+            "data": {"client_id": "test_client"}
+        }))
+
+        # Wait for first state
+        time.sleep(0.5)
+        self._receive_state()
+
+    def close(self):
+        """Close WebSocket connection."""
+        if self.ws:
+            self.ws.close()
+            self.ws = None
+
+    def _receive_state(self):
+        """Receive and parse state message."""
+        try:
+            msg = self.ws.recv(timeout=0.1)
+            if msg:
+                data = json.loads(msg)
+                if data.get("type") == "state":
+                    self.latest_state = data.get("data", {})
+        except TimeoutError:
+            pass
+
+    def send_message(self, msg: dict):
+        """Send a message and update latest state."""
+        self.ws.send(json.dumps(msg))
+        self._receive_state()
+
+    def send_home(self):
+        """Send a home message."""
+        self.send_message({"type": "home"})
+
+    def get_joint_positions(self):
+        """Get current joint positions from latest state."""
+        obs = self.latest_state.get("observation", {})
+        return obs.get("joint_positions", [0, 0, 0, 0, 0, 0])
+
+    def get_scene_objects(self):
+        """Get scene objects from latest state."""
+        return self.latest_state.get("scene_objects", [])

tests/test_homing.py

@@ -0,0 +1,453 @@
+"""Comprehensive homing test for nova-sim."""
+
+import time
+import math
+import sys
+import os
+import json
+
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from test_client import NovaSimTestClient
+
+
+def test_homing():
+    """Test complete homing workflow: verify robot moves to configured home pose."""
+    print("\n" + "=" * 70)
+    print("Comprehensive Homing Test")
+    print("=" * 70)
+    print("\nThis test verifies:")
+    print("  1. Robot position can be read from state stream")
+    print("  2. Sending 'home' messages triggers homing behavior")
+    print("  3. Robot moves toward configured home_pose")
+    print("  4. Robot reaches home within acceptable tolerance")
+
+    # Expected home pose (from scene XML)
+    expected_home = [-3.22, -1.14, 1.93, -2.36, -1.57, -1.72]
+
+    # Create test client
+    print(f"\n[1] Connecting to Nova-Sim...")
+    client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")
+
+    try:
+        client.connect()
+        print("✓ Connected to Nova-Sim")
+
+        # Get initial position
+        time.sleep(0.5)
+        initial_joints = client.get_joint_positions()
+
+        print(f"\n[2] Initial joint positions:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in initial_joints)}]")
+
+        # Calculate initial error from home
+        initial_errors = [abs(actual - target) for actual, target in zip(initial_joints, expected_home)]
+        initial_max_error = max(initial_errors)
+        print(f"    Max error from home: {initial_max_error:.4f} rad ({math.degrees(initial_max_error):.2f}°)")
+
+        # Check if already at home
+        if initial_max_error < 0.1:
+            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
+        print(f"\n[3] Starting homing (sending 'home' messages)...")
+        print("    Sending home messages for 10 seconds...")
+
+        home_start_time = time.time()
+        message_count = 0
+        joints_before = initial_joints.copy()
+
+        # Send home messages for 10 seconds
+        while time.time() - home_start_time < 10.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
+
+        homing_time = time.time() - home_start_time
+
+        # Get final position
+        time.sleep(0.5)
+        final_joints = client.get_joint_positions()
+
+        print(f"\n[4] Final joint positions after {homing_time:.1f}s:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in final_joints)}]")
+
+        # Calculate movement
+        total_movement = sum(abs(after - before) for before, after in zip(joints_before, final_joints))
+        print(f"    Total joint movement: {total_movement:.3f} rad")
+
+        # Verify final position
+        print(f"\n[5] Verifying final position against configured home...")
+        print(f"    Expected home: [{', '.join(f'{j:7.4f}' for j in expected_home)}]")
+
+        joint_errors = [abs(actual - target) for actual, target in zip(final_joints, expected_home)]
+
+        print(f"\n    Joint errors:")
+        tolerance = 0.1  # 0.1 rad = 5.7 degrees
+        for i, (error, actual, target) in enumerate(zip(joint_errors, final_joints, expected_home), 1):
+            status = "✓" if error < tolerance else "⚠"
+            print(f"      {status} Joint {i}: error={error:.4f} rad ({math.degrees(error):.2f}°), "
+                  f"actual={actual:.4f}, target={target:.4f}")
+
+        max_error = max(joint_errors)
+        avg_error = sum(joint_errors) / len(joint_errors)
+        print(f"\n    Max error:     {max_error:.4f} rad ({math.degrees(max_error):.2f}°)")
+        print(f"    Average error: {avg_error:.4f} rad ({math.degrees(avg_error):.2f}°)")
+
+        # Check scene objects
+        scene_objects = client.get_scene_objects()
+        if scene_objects:
+            print(f"\n[6] Scene objects:")
+            for obj in scene_objects:
+                pos = obj["position"]
+                print(f"     - {obj['name']}: pos=({pos['x']:.3f}, {pos['y']:.3f}, {pos['z']:.3f})")
+
+        # Assessment
+        print("\n" + "=" * 70)
+        success = False
+        if max_error < 0.01:
+            print("✓ EXCELLENT - Robot at home within 0.01 rad (0.57°)")
+            success = True
+        elif max_error < 0.05:
+            print("✓ GOOD - Robot at home within 0.05 rad (2.9°)")
+            success = True
+        elif max_error < 0.1:
+            print("⚠ ACCEPTABLE - Robot within 0.1 rad (5.7°) of home")
+            success = True
+        else:
+            print(f"✗ FAILED - Robot not at home (max error: {math.degrees(max_error):.1f}°)")
+            print(f"\nNote: In digital twin mode with Nova API connected:")
+            print(f"  - Robot position mirrors real hardware")
+            print(f"  - Homing requires working Nova API jogging integration")
+            print(f"  - Check that Nova Jogger is properly configured and connected")
+
+        if total_movement < 0.01:
+            print(f"\n⚠ WARNING: Robot did not move during homing (movement: {total_movement:.3f} rad)")
+            print(f"  - This may indicate jogging commands are not reaching the robot")
+            print(f"  - Check server logs for jogging errors")
+
+        print("=" * 70)
+
+        # Only assert success on movement if robot wasn't already at home
+        if initial_max_error > 0.1:
+            assert success, f"Robot did not reach home position (max error: {math.degrees(max_error):.1f}°)"
+            if total_movement < 0.01:
+                print(f"\n⚠ Robot didn't move but test passed (was it already at home?)")
+        else:
+            print(f"\n✓ Test completed (robot was already near home)")
+
+    finally:
+        # Cleanup
+        try:
+            client.close()
+            print("\n✓ Disconnected from Nova-Sim")
+        except Exception as e:
+            print(f"\n⚠ Error during disconnect: {e}")
+
+
+def test_drift_prevention():
+    """Test that robot stops moving when home messages cease."""
+    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")
+
+    client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")
+
+    try:
+        client.connect()
+        print("✓ Connected to Nova-Sim")
+
+        # Get initial position
+        time.sleep(0.5)
+        initial_joints = client.get_joint_positions()
+        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)
+
+        # 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"    [{', '.join(f'{j:7.4f}' for j in position_after_homing)}]")
+
+        # Wait 2 seconds and check if robot drifted
+        print(f"\n[4] Waiting 2 seconds to check for drift...")
+        time.sleep(2.0)
+
+        final_position = client.get_joint_positions()
+        print(f"\n[5] Position 2 seconds after stopping:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in final_position)}]")
+
+        # Calculate drift
+        max_drift = 0
+        drift_joint = None
+        for i, (before, after) in enumerate(zip(position_after_homing, final_position)):
+            drift = abs(after - before)
+            if drift > max_drift:
+                max_drift = drift
+                drift_joint = i
+
+        print(f"\n[6] Results:")
+        print(f"    Max drift: {max_drift:.4f} rad ({math.degrees(max_drift):.2f}°)")
+        if drift_joint is not None:
+            print(f"    Joint with max drift: Joint {drift_joint + 1}")
+
+        # A small amount of drift is acceptable due to gravity/dynamics
+        # But large drift indicates jogging didn't stop
+        drift_threshold = 0.005  # 0.005 rad ≈ 0.29°
+
+        print("\n" + "=" * 70)
+        if max_drift < drift_threshold:
+            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.")
+            success = False
+        print("=" * 70)
+
+        client.close()
+        print("\n✓ Disconnected")
+
+        assert success, f"Drift detected: {max_drift:.4f} rad exceeds threshold {drift_threshold:.4f} rad"
+
+    except Exception as e:
+        print(f"\n✗ Test error: {e}")
+        import traceback
+        traceback.print_exc()
+        try:
+            client.close()
+        except:
+            pass
+        raise
+
+
+def test_overshoot_detection():
+    """Test that homing detects and corrects overshoots by reducing velocity."""
+    print("\n" + "=" * 70)
+    print("Overshoot Detection Test")
+    print("=" * 70)
+    print("\nThis test verifies that homing detects overshoots and reduces velocity.")
+    print("It will move joint 1 away from home, then home it and monitor for overshoots.\n")
+
+    client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")
+
+    try:
+        client.connect()
+        print("✓ Connected to Nova-Sim")
+
+        # Get home position
+        time.sleep(0.5)
+        home_joints = [-3.2200, -1.1400, 1.9300, -2.3600, -1.5700, -1.7200]
+        print(f"\n[1] Home position:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in home_joints)}]")
+
+        # Move joint 1 far from home
+        print(f"\n[2] Moving joint 1 to +1.0 rad (far from home of -3.22 rad)...")
+
+        # Send action messages to move joint 1 positive
+        duration = 2.0
+        rate_hz = 10
+        num_messages = int(duration * rate_hz)
+
+        for i in range(num_messages):
+            client.ws.send(json.dumps({
+                'type': 'action',
+                'data': {'j1': 1.0}  # Move joint 1 in positive direction
+            }))
+            time.sleep(1.0 / rate_hz)
+
+        # Stop moving
+        client.ws.send(json.dumps({'type': 'action', 'data': {}}))
+        time.sleep(0.5)
+
+        # Get position after movement
+        moved_joints = client.get_joint_positions()
+        print(f"\n[3] Position after moving joint 1:")
+        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
+
+        # Get final position
+        time.sleep(0.5)
+        final_joints = client.get_joint_positions()
+
+        print(f"\n[5] Final position:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in final_joints)}]")
+
+        # Check all joints
+        print(f"\n[6] Final errors from home:")
+        max_error = 0
+        for i, (actual, target) in enumerate(zip(final_joints, home_joints)):
+            error = abs(actual - target)
+            max_error = max(max_error, error)
+            status = "✓" if error < 0.01 else "✗"
+            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)
+        if max_error < 0.05:
+            print(f"✓ Homing succeeded (max error < 0.05 rad)")
+            success = True
+        else:
+            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("=" * 70)
+
+        client.close()
+        print("\n✓ Disconnected")
+
+        assert success, f"Homing failed with max error {max_error:.4f} rad"
+
+    except Exception as e:
+        print(f"\n✗ Test error: {e}")
+        import traceback
+        traceback.print_exc()
+        try:
+            client.close()
+        except:
+            pass
+        raise
+
+
+if __name__ == "__main__":
+    print("\n" + "=" * 70)
+    print("Nova-Sim Homing Test")
+    print("=" * 70)
+    print("\nThis test requires Nova-Sim to be running.")
+    print("Start it with:")
+    print("  cd /Users/georgpuschel/repos/robot-ml/nova-sim")
+    print("  source .venv/bin/activate")
+    print("  python mujoco_server.py --robot ur5_t_push")
+
+    if sys.stdin.isatty():
+        print("\nPress Enter when Nova-Sim is ready...")
+        input()
+    else:
+        print("\nRunning in non-interactive mode, proceeding in 2 seconds...\n")
+        time.sleep(2)
+
+    # Check if specific test requested
+    test_name = sys.argv[1] if len(sys.argv) > 1 else "all"
+
+    all_passed = True
+
+    try:
+        if test_name in ("all", "basic"):
+            print("\n" + "="*70)
+            print("Running: Basic Homing Test")
+            print("="*70)
+            try:
+                test_homing()
+                print("\n✓ Basic Homing Test PASSED")
+            except Exception as e:
+                print(f"\n✗ Basic Homing Test FAILED: {e}")
+                all_passed = False
+
+        if test_name in ("all", "drift"):
+            print("\n" + "="*70)
+            print("Running: Drift Prevention Test")
+            print("="*70)
+            try:
+                test_drift_prevention()
+                print("\n✓ Drift Prevention Test PASSED")
+            except Exception as e:
+                print(f"\n✗ Drift Prevention Test FAILED: {e}")
+                all_passed = False
+
+        if test_name in ("all", "overshoot"):
+            print("\n" + "="*70)
+            print("Running: Overshoot Detection Test")
+            print("="*70)
+            try:
+                test_overshoot_detection()
+                print("\n✓ Overshoot Detection Test PASSED")
+            except Exception as e:
+                print(f"\n✗ Overshoot Detection Test FAILED: {e}")
+                all_passed = False
+
+        print("\n" + "="*70)
+        if all_passed:
+            print("✓ ALL TESTS COMPLETED SUCCESSFULLY")
+            print("="*70)
+            print("\nCheck /tmp/nova_sim_server.log for detailed server logs")
+            sys.exit(0)
+        else:
+            print("✗ SOME TESTS FAILED")
+            print("="*70)
+            print("\nCheck /tmp/nova_sim_server.log for detailed server logs")
+            sys.exit(1)
+
+    except KeyboardInterrupt:
+        print("\n\n✗ Tests interrupted by user")
+        sys.exit(1)

tests/test_jogging.py

@@ -0,0 +1,112 @@
+"""Test jogging to compare UI jogging vs homing jogging."""
+
+import time
+import json
+import sys
+import os
+
+sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from test_client import NovaSimTestClient
+
+
+def test_ui_jogging():
+    """Test UI-style jogging (sending continuous action messages)."""
+    print("\n" + "=" * 70)
+    print("UI-Style Jogging Test")
+    print("=" * 70)
+    print("\nThis test simulates how the UI sends jogging commands.")
+    print("It sends continuous 'action' messages with joint velocities.\n")
+
+    client = NovaSimTestClient("http://localhost:3004/nova-sim/api/v1")
+
+    try:
+        client.connect()
+        print("✓ Connected to Nova-Sim")
+
+        # Get initial position
+        time.sleep(0.5)
+        initial_joints = client.get_joint_positions()
+        print(f"\n[1] Initial joint positions:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in initial_joints)}]")
+
+        # Send continuous action messages for joint 6, like the UI does
+        print(f"\n[2] Sending continuous 'action' messages (UI style)...")
+        print(f"    Jogging joint 6 in negative direction for 3 seconds")
+
+        duration = 3.0
+        rate_hz = 10  # UI typically sends at ~10 Hz
+        num_messages = int(duration * rate_hz)
+
+        for i in range(num_messages):
+            # Send action message with j6 velocity (like UI does)
+            client.ws.send(json.dumps({
+                'type': 'action',
+                'data': {
+                    'j6': -0.5  # Negative velocity for joint 6
+                }
+            }))
+            time.sleep(1.0 / rate_hz)
+            if (i + 1) % 10 == 0:
+                print(f"    Sent {i + 1}/{num_messages} messages")
+
+        # Stop jogging
+        client.ws.send(json.dumps({
+            'type': 'action',
+            'data': {
+
+            }
+        }))
+
+        time.sleep(0.5)
+
+        # Get final position
+        final_joints = client.get_joint_positions()
+        print(f"\n[3] Final joint positions:")
+        print(f"    [{', '.join(f'{j:7.4f}' for j in final_joints)}]")
+
+        # Calculate movement
+        movement = [abs(final - initial) for final, initial in zip(final_joints, initial_joints)]
+        total_movement = sum(movement)
+
+        print(f"\n[4] Results:")
+        print(f"    Total movement: {total_movement:.4f} rad")
+        print(f"    Joint 6 movement: {movement[5]:.4f} rad")
+        print(f"    Joint 6 change: {initial_joints[5]:.4f} -> {final_joints[5]:.4f}")
+
+        if movement[5] > 0.1:
+            print(f"\n✓ UI jogging works - joint 6 moved {movement[5]:.4f} rad")
+        else:
+            print(f"\n✗ UI jogging failed - joint 6 barely moved")
+
+        client.disconnect()
+        print("✓ Disconnected")
+
+    except Exception as e:
+        print(f"\n✗ Test error: {e}")
+        import traceback
+        traceback.print_exc()
+        try:
+            client.disconnect()
+        except:
+            pass
+
+if __name__ == "__main__":
+    print("\n" + "=" * 70)
+    print("JOGGING COMPARISON TEST")
+    print("=" * 70)
+    print("\nThis test compares UI jogging vs homing jogging.")
+    print("Both should trigger continuous jogging, but we'll see if they behave differently.\n")
+
+    input("Press Enter to start UI jogging test (or Ctrl+C to exit)...")
+    test_ui_jogging()
+
+    time.sleep(2)
+
+    input("\nPress Enter to start homing jogging test (or Ctrl+C to exit)...")
+    test_homing_jogging()
+
+    print("\n" + "=" * 70)
+    print("Check /tmp/nova_sim_server.log for detailed logs")
+    print("Compare the 'Nova Jogger' logs from both tests")
+    print("=" * 70)