Refactor homing control logic in mujoco_server.py for improved accuracy and responsiveness

- Adjusted joint velocity calculations to prevent overshoot and ensure smoother homing behavior.
- Introduced tighter tolerance checks for stopping individual joints, enhancing precision during the homing process.
- Removed redundant debug print statements to streamline code and improve clarity.
- Updated velocity damping mechanism to gradually reduce speed as joints approach their target positions, ensuring a more controlled movement.

mujoco_server.py

@@ -1175,14 +1175,12 @@ def handle_ws_message(ws, data):
         with mujoco_lock, homing_lock:
             # Update last home message timestamp
             homing_state["last_home_time"] = time.time()
-            print(f"[Home] Processing home message, robot={current_robot}, env={env is not None}")
             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)
 
-                print(f"[Home] home_pose={home_pose is not None}, get_joints={callable(get_joints)}, multi_jog={callable(start_multi_joint_jog)}, stop={callable(stop_jog)}")
                 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()
@@ -1191,7 +1189,6 @@ def handle_ws_message(ws, data):
 
                     current_positions = get_joints()
                     tolerance = 0.01  # rad (~0.57 degrees)
-                    print(f"[Home] target={target_positions[:3]}, current={current_positions[:3]}")
 
                     # Calculate errors for all joints
                     errors = []
@@ -1212,6 +1209,7 @@ def handle_ws_message(ws, data):
                             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
@@ -1219,40 +1217,67 @@ def handle_ws_message(ws, data):
                         homing_state["previous_errors"] = [0.0] * len(errors)
 
                     # Calculate proportional velocities for all joints
-                    base_velocity = 0.3  # rad/s (increased from 0.1 for faster homing)
+                    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 - reduce velocity
-                            base_velocity_joint = base_velocity * 0.5
-                        else:
-                            base_velocity_joint = base_velocity
+                            # Overshoot detected - stop this joint completely
+                            velocities.append(0.0)
+                            continue
 
-                        if abs_error < tolerance:
-                            # Already at target
+                        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_joint * (abs_error / max_abs_error)
+                                velocity = base_velocity * (abs_error / max_abs_error)
                             else:
                                 velocity = 0.0
 
-                            # Apply velocity in correct direction
-                            if error > 0:
-                                velocities.append(velocity)
+                            # 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:
-                                velocities.append(-velocity)
+                                # 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")
 
@@ -1521,9 +1546,6 @@ def _create_env(robot, scene):
 @sock.route(f'{API_PREFIX}/ws')
 def websocket_handler(ws):
     """Handle WebSocket connections."""
-    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 (track all clients, even before they identify)
@@ -1541,10 +1563,6 @@ 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}")