re-added collision. Hardened app.py. Some more small changes.

app.py

@@ -1,4 +1,4 @@
-import logging, faulthandler, sys, time, tempfile, os, requests, zipfile, io, gc, threading, psutil, json, configargparse
+import logging, faulthandler, sys, time, os, requests, zipfile, io, gc, threading, psutil, json, configargparse
 faulthandler.enable()
 logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')
 logger = logging.getLogger("jax_ik_server")
@@ -38,8 +38,10 @@ def download_and_setup_files():
     if not os.path.isdir(pepper_dir):
         logger.info("Downloading Pepper model...")
         try:
-            r = requests.get("https://uni-bielefeld.sciebo.de/s/98Sy5s143XgNntb/download", stream=True); r.raise_for_status()
-            with zipfile.ZipFile(io.BytesIO(r.content)) as z: z.extractall("files/")
+            r = requests.get("https://uni-bielefeld.sciebo.de/s/98Sy5s143XgNntb/download", stream=True)
+            r.raise_for_status()
+            with zipfile.ZipFile(io.BytesIO(r.content)) as z:
+                z.extractall("files/")
         except Exception as e:
             logger.warning(f"Pepper download failed: {e}")
     # SMPLX
@@ -47,7 +49,8 @@ def download_and_setup_files():
     if not os.path.isfile(smplx_file):
         logger.info("Downloading SMPLX model...")
         try:
-            r = requests.get("https://uni-bielefeld.sciebo.de/s/B5StwQdiR4DW5mc/download"); r.raise_for_status()
+            r = requests.get("https://uni-bielefeld.sciebo.de/s/B5StwQdiR4DW5mc/download")
+            r.raise_for_status()
             open(smplx_file, "wb").write(r.content)
         except Exception as e:
             logger.warning(f"SMPLX download failed: {e}")
@@ -56,51 +59,189 @@ class IKServer:
     def __init__(self, args):
         self.args = args
         self.solve_lock = threading.Lock()
+        self.config_lock = threading.Lock()
         self.process = psutil.Process(os.getpid())
 
-        # Caches
+        # Failure / resilience counters
+        self.agent_fail_count = 0
+        self.urdf_fail_count = 0
+        self.last_agent_error = None
+        self.last_urdf_error = None
+        self.agent_solve_counter = 0
+        self.urdf_solve_counter = 0
+        self.max_fail_retries = 2          # total attempts (initial + retries)
+        self.circuit_breaker_threshold = 5 # open after N consecutive failures
+        self.circuit_open_until = 0        # epoch time until which solves short-circuit
+        self.circuit_backoff_seconds = 5
+
+        # Numeric safety
+        self.max_num_steps_global = 20000
+        self.max_learning_rate = 5.0
+
+        # Caches for solver reuse (LRU)
         self.solver_cache = {}
         self.urdf_solver_cache = {}
-        self.max_cache_size = 5
         self.cache_access_order = []
         self.urdf_cache_access_order = []
+        self.max_cache_size = 5
 
         # Animation buffers
         self.animation_frames_agent = []
         self.animation_frames_urdf = []
 
-        # FastAPI
+        # FastAPI app
         self.app = FastAPI()
         self.app.add_middleware(
             CORSMiddleware,
             allow_origins=["*"], allow_credentials=True,
-            allow_methods=["*"], allow_headers=["*"]
+            allow_methods=["*"], allow_headers=["*"],
         )
         os.makedirs("static", exist_ok=True)
         os.makedirs("files", exist_ok=True)
         self.app.mount("/static", StaticFiles(directory="static"), name="static")
         self.app.mount("/files", StaticFiles(directory="files"), name="files")
 
-        # Init models (once)
+        # Initialize models
         self._init_agent()
         self._setup_agent_objectives()
         self._init_urdf()
         self._setup_urdf_objectives()
 
-        # Cleanup tracking
+        # Cleanup timing
         self.last_cleanup_time = time.time()
         self.cleanup_interval = 30
 
-        self.agent_solve_counter = 0   # added
-        self.urdf_solve_counter  = 0   # added
-
         self._register_routes()
         logger.info("Server ready.")
-        # NEW warmup thread to JIT both solvers early
         if getattr(self.args, 'warmup', True):
             threading.Thread(target=self._warmup_all, daemon=True).start()
 
-    # ---- Warmup helpers (NEW) ----
+    # ---------- Utility / Safety ----------
+    def _safe_int(self, v, default, lo=None, hi=None):
+        try:
+            v = int(v)
+        except Exception:
+            v = default
+        if lo is not None and v < lo: v = lo
+        if hi is not None and v > hi: v = hi
+        return v
+
+    def _safe_float(self, v, default, lo=None, hi=None):
+        try:
+            v = float(v)
+            if np.isnan(v) or np.isinf(v): raise ValueError
+        except Exception:
+            v = default
+        if lo is not None and v < lo: v = lo
+        if hi is not None and v > hi: v = hi
+        return v
+
+    def _sanitize_num_steps(self, steps):
+        return self._safe_int(steps, self.args.num_steps, 1, self.max_num_steps_global)
+
+    def _sanitize_bones(self, bones, is_urdf=False):
+        if not isinstance(bones, (list, tuple)):
+            return []
+        allowed = self.urdf_available_bones if is_urdf else self.available_bones
+        return [b for b in bones if b in allowed]
+
+    def _cache_key(self, bones, num_steps):
+        return tuple(sorted(bones)) + (int(num_steps),)
+
+    def _evict_lru(self, is_urdf=False):
+        cache = self.urdf_solver_cache if is_urdf else self.solver_cache
+        order = self.urdf_cache_access_order if is_urdf else self.cache_access_order
+        if not order:
+            return
+        k = order.pop(0)
+        cache.pop(k, None)
+        gc.collect()
+
+    def _invalidate_caches(self):
+        self.solver_cache.clear(); self.urdf_solver_cache.clear()
+        self.cache_access_order.clear(); self.urdf_cache_access_order.clear()
+        gc.collect()
+
+    def _create_solver(self, bones, is_urdf, num_steps):
+        if is_urdf:
+            return InverseKinematicsSolver(
+                model_file=self.urdf_file,
+                controlled_bones=bones,
+                bounds=None,
+                threshold=self.args.threshold,
+                num_steps=int(num_steps),
+                compute_sdf=False,
+            )
+        bounds = []
+        for b in bones:
+            if b in self.bounds_dict:
+                lower, upper = self.bounds_dict[b]
+                bounds.extend(list(zip(lower, upper)))
+            else:
+                bounds.extend([(-90, 90)] * 3)
+        return InverseKinematicsSolver(
+            model_file=self.args.gltf_file,
+            controlled_bones=bones,
+            bounds=bounds,
+            threshold=self.args.threshold,
+            num_steps=int(num_steps),
+            compute_sdf=False,
+        )
+
+    def _get_solver(self, bones, is_urdf=False, num_steps=None):
+        if num_steps is None:
+            num_steps = self.args.num_steps
+        num_steps = self._sanitize_num_steps(num_steps)
+        key = self._cache_key(bones, num_steps)
+        cache = self.urdf_solver_cache if is_urdf else self.solver_cache
+        order = self.urdf_cache_access_order if is_urdf else self.cache_access_order
+        if key in cache:
+            if key in order: order.remove(key)
+            order.append(key)
+            return cache[key]
+        if len(cache) >= self.max_cache_size:
+            self._evict_lru(is_urdf)
+        try:
+            solver = self._create_solver(bones, is_urdf, num_steps)
+        except Exception as e:
+            logger.warning(f"Primary solver creation failure (is_urdf={is_urdf}): {e}; using defaults")
+            bones = self.urdf_default_controlled_bones if is_urdf else self.default_controlled_bones
+            solver = self._create_solver(bones, is_urdf, num_steps)
+        cache[key] = solver
+        order.append(key)
+        return solver
+
+    def _rebuild_solver_safe(self, is_urdf=False, force_defaults=False):
+        try:
+            if is_urdf:
+                bones = self.urdf_default_controlled_bones if force_defaults else self.urdf_current_controlled_bones
+                bones = self._sanitize_bones(bones, True) or self.urdf_default_controlled_bones
+                self.urdf_solver = self._create_solver(bones, True, self.urdf_current_num_steps)
+                self.urdf_initial_rotations = np.zeros(len(self.urdf_solver.controlled_bones)*3, dtype=np.float32)
+                self.urdf_best_angles = self.urdf_initial_rotations.copy()
+            else:
+                bones = self.default_controlled_bones if force_defaults else self.current_controlled_bones
+                bones = self._sanitize_bones(bones, False) or self.default_controlled_bones
+                self.solver = self._create_solver(bones, False, self.current_num_steps)
+                self.initial_rotations = np.zeros(len(self.solver.controlled_bones)*3, dtype=np.float32)
+                self.best_angles = self.initial_rotations.copy()
+            return True
+        except Exception as e:
+            (self.last_urdf_error if is_urdf else setattr(self, 'last_agent_error', str(e)))
+            logger.exception("Solver rebuild failed")
+            return False
+
+    def _attempt_solver_recovery(self, is_urdf=False):
+        logger.warning("Attempting full solver recovery (invalidate caches + defaults)")
+        self._invalidate_caches()
+        if is_urdf:
+            self.urdf_current_controlled_bones = self.urdf_default_controlled_bones.copy()
+            return self._rebuild_solver_safe(True, force_defaults=True)
+        else:
+            self.current_controlled_bones = self.default_controlled_bones.copy()
+            return self._rebuild_solver_safe(False, force_defaults=True)
+
+    # ---------- Warmup ----------
     def _warmup_agent(self):
         try:
             mand = [DistanceObjTraj(target_points=np.array([0.0,0.2,0.35]), bone_name=self.current_end_effector, use_head=True, weight=1.0)]
@@ -128,61 +269,9 @@ class IKServer:
         self._warmup_urdf()
         logger.info(f"Warmup finished in {time.time()-t0:.2f}s")
 
-    # ---------- Cache ----------
-    def _evict_lru(self, is_urdf=False):
-        cache = self.urdf_solver_cache if is_urdf else self.solver_cache
-        order = self.urdf_cache_access_order if is_urdf else self.cache_access_order
-        if not order: return
-        key = order.pop(0)
-        cache.pop(key, None)
-        gc.collect()
-
-    def _cache_key(self, bones, num_steps): return tuple(sorted(bones)) + (int(num_steps),)
-
-    def _get_solver(self, bones, is_urdf=False, num_steps=None):
-        if num_steps is None:
-            num_steps = self.args.num_steps
-        key = self._cache_key(bones, num_steps)
-        cache = self.urdf_solver_cache if is_urdf else self.solver_cache
-        order = self.urdf_cache_access_order if is_urdf else self.cache_access_order
-        if key in cache:
-            if key in order: order.remove(key)
-            order.append(key)
-            return cache[key]
-        if len(cache) >= self.max_cache_size:
-            self._evict_lru(is_urdf)
-        if is_urdf:
-            solver = InverseKinematicsSolver(
-                model_file=self.urdf_file,
-                controlled_bones=bones,
-                bounds=None,
-                threshold=self.args.threshold,
-                num_steps=int(num_steps),
-                compute_sdf=False,
-            )
-        else:
-            bounds = []
-            for b in bones:
-                if b in self.bounds_dict:
-                    lower, upper = self.bounds_dict[b]
-                    bounds.extend(list(zip(lower, upper)))
-                else:
-                    bounds.extend([(-90, 90)] * 3)
-            solver = InverseKinematicsSolver(
-                model_file=self.args.gltf_file,
-                controlled_bones=bones,
-                bounds=bounds,
-                threshold=self.args.threshold,
-                num_steps=int(num_steps),
-                compute_sdf=False,
-            )
-        cache[key] = solver
-        order.append(key)
-        return solver
-
     # ---------- Initialization ----------
     def _init_agent(self):
-        self.current_num_steps = self.args.num_steps  # NEW
+        self.current_num_steps = self._sanitize_num_steps(self.args.num_steps)
         basic = InverseKinematicsSolver(
             model_file=self.args.gltf_file,
             controlled_bones=["left_collar"],
@@ -209,7 +298,7 @@ class IKServer:
         self.animation_frames_agent = self._frames_from_angles([self.initial_rotations], False)
 
     def _init_urdf(self):
-        self.urdf_current_num_steps = self.args.num_steps  # NEW
+        self.urdf_current_num_steps = self._sanitize_num_steps(self.args.num_steps)
         self.urdf_file = "files/pepper_description-master/urdf/pepper.urdf"
         basic = InverseKinematicsSolver(
             model_file=self.urdf_file,
@@ -244,6 +333,10 @@ class IKServer:
             min_clearance=0.0,
             weight=1.0,
         )
+        # store defaults for config exposure
+        self.collision_default_center = [0.1,0.0,0.35]
+        self.collision_default_radius = 0.1
+        self.collision_default_min_clearance = 0.0
 
     def _setup_urdf_objectives(self):
         self.urdf_distance_obj = DistanceObjTraj(
@@ -257,58 +350,34 @@ class IKServer:
             min_clearance=0.0,
             weight=1.0,
         )
+        self.urdf_collision_default_center = [0.2,0.0,0.35]
+        self.urdf_collision_default_radius = 0.1
+        self.urdf_collision_default_min_clearance = 0.0
 
-    # ---------- Configuration ----------
-    def configure_agent(self, bones, eff, num_steps=None):
-        if num_steps is None:
-            num_steps = self.current_num_steps
-        if not bones: bones = self.default_controlled_bones
-        if eff not in self.available_bones: eff = self.default_end_effector
-        changed = (
-            bones != self.current_controlled_bones or
-            eff != self.current_end_effector or
-            int(num_steps) != int(self.current_num_steps)
-        )
-        if bones != self.current_controlled_bones or int(num_steps) != int(self.current_num_steps):
-            self.current_controlled_bones = bones
-            self.current_num_steps = int(num_steps)
-            self.solver = self._get_solver(bones, is_urdf=False, num_steps=self.current_num_steps)
-            self.initial_rotations = np.zeros(len(self.solver.controlled_bones)*3, dtype=np.float32)
-            self.best_angles = self.initial_rotations.copy()
-        if eff != self.current_end_effector:
-            self.current_end_effector = eff
-        if changed:
-            self._setup_agent_objectives()
-        return {"controlled_bones": self.current_controlled_bones, "end_effector": self.current_end_effector, "num_steps": self.current_num_steps}
-
-    def configure_urdf(self, bones, eff, num_steps=None):
-        if num_steps is None:
-            num_steps = self.urdf_current_num_steps
-        if not bones: bones = self.urdf_default_controlled_bones
-        if eff not in self.urdf_available_bones: eff = self.urdf_default_end_effector
-        changed = (
-            bones != self.urdf_current_controlled_bones or
-            eff != self.urdf_current_end_effector or
-            int(num_steps) != int(self.urdf_current_num_steps)
-        )
-        if bones != self.urdf_current_controlled_bones or int(num_steps) != int(self.urdf_current_num_steps):
-            self.urdf_current_controlled_bones = bones
-            self.urdf_current_num_steps = int(num_steps)
-            self.urdf_solver = self._get_solver(bones, is_urdf=True, num_steps=self.urdf_current_num_steps)
-            self.urdf_initial_rotations = np.zeros(len(self.urdf_solver.controlled_bones)*3, dtype=np.float32)
-            self.urdf_best_angles = self.urdf_initial_rotations.copy()
-        if eff != self.urdf_current_end_effector:
-            self.urdf_current_end_effector = eff
-        if changed:
-            self._setup_urdf_objectives()
-        return {"controlled_bones": self.urdf_current_controlled_bones, "end_effector": self.urdf_current_end_effector, "num_steps": self.urdf_current_num_steps}
-
-    # ---------- Objectives build ----------
+    # ---------- Build objectives per request ----------
     def _build_agent_objectives(self, payload):
         tgt = np.array(payload.get("target",[0.0,0.2,0.35]))
         self.distance_obj.update_params({"bone_name": self.current_end_effector, "target_points": tgt, "weight": float(payload.get("distance_weight",1.0))})
-        self.collision_obj.update_params({"weight": float(payload.get("collision_weight",1.0))})
-        subpoints = int(payload.get("subpoints",1))
+        # collision updates (weight, center, radius, min_clearance)
+        if payload.get("collision_enabled", False):
+            coll_update = {"weight": float(payload.get("collision_weight",1.0))}
+            center = payload.get("collision_center")
+            if isinstance(center, (list, tuple)) and len(center)==3:
+                coll_update["center"] = center
+            radius = payload.get("collision_radius")
+            if radius is not None:
+                coll_update["radius"] = float(radius)
+            if "collision_min_clearance" in payload:
+                coll_update["min_clearance"] = float(payload.get("collision_min_clearance", 0.0))
+            self.collision_obj.update_params(coll_update)
+        else:
+            # still update weight / min_clearance so they can be changed next enable
+            mc = payload.get("collision_min_clearance")
+            upd = {"weight": float(payload.get("collision_weight",1.0))}
+            if mc is not None:
+                upd["min_clearance"] = float(mc)
+            self.collision_obj.update_params(upd)
+        subpoints = self._safe_int(payload.get("subpoints",1), 1, 1, 100)
         mandatory, optional = [], []
         if payload.get("distance_enabled", True): mandatory.append(self.distance_obj)
         if payload.get("collision_enabled", False): optional.append(self.collision_obj)
@@ -318,7 +387,7 @@ class IKServer:
             optional.append(CombinedDerivativeObj(max_order=3, weights=[float(payload.get("derivative_weight",0.05))]*3))
         elif not payload.get("bone_zero_enabled", True) and not payload.get("derivative_enabled", True):
             optional.append(BoneZeroRotationObj(weight=0.01))
-        # Hand spec
+        # Hand specification
         hand_shape = payload.get("hand_shape","None")
         hand_position = payload.get("hand_position","None")
         params = {
@@ -352,8 +421,24 @@ class IKServer:
     def _build_urdf_objectives(self, payload):
         tgt = np.array(payload.get("target",[0.3,0.3,0.35]))
         self.urdf_distance_obj.update_params({"bone_name": self.urdf_current_end_effector, "target_points": tgt, "weight": float(payload.get("distance_weight",1.0))})
-        self.urdf_collision_obj.update_params({"weight": float(payload.get("collision_weight",1.0))})
-        subpoints = int(payload.get("subpoints",1))
+        if payload.get("collision_enabled", False):
+            coll_update = {"weight": float(payload.get("collision_weight",1.0))}
+            center = payload.get("collision_center")
+            if isinstance(center, (list, tuple)) and len(center)==3:
+                coll_update["center"] = center
+            radius = payload.get("collision_radius")
+            if radius is not None:
+                coll_update["radius"] = float(radius)
+            if "collision_min_clearance" in payload:
+                coll_update["min_clearance"] = float(payload.get("collision_min_clearance", 0.0))
+            self.urdf_collision_obj.update_params(coll_update)
+        else:
+            mc = payload.get("collision_min_clearance")
+            upd = {"weight": float(payload.get("collision_weight",1.0))}
+            if mc is not None:
+                upd["min_clearance"] = float(mc)
+            self.urdf_collision_obj.update_params(upd)
+        subpoints = self._safe_int(payload.get("subpoints",1),1,1,100)
         mandatory, optional = [], []
         if payload.get("distance_enabled", True): mandatory.append(self.urdf_distance_obj)
         if payload.get("collision_enabled", False): optional.append(self.urdf_collision_obj)
@@ -365,85 +450,212 @@ class IKServer:
             optional.append(BoneZeroRotationObj(weight=0.01))
         return mandatory, optional, subpoints
 
-    # ---------- Solving ----------
+    # ---------- Frames ----------
     def _frames_from_angles(self, angles_seq, is_urdf):
         frames = []
         for ang in angles_seq:
-            if is_urdf:
-                verts = deform_mesh(ang, self.urdf_solver.fk_solver, self.urdf_mesh_data)
-                faces = self.urdf_mesh_data["faces"]
-            else:
-                verts = deform_mesh(ang, self.solver.fk_solver, self.mesh_data)
-                faces = self.mesh_data["faces"]
-            frames.append({"vertices": verts.tolist(), "faces": faces.tolist()})
+            try:
+                if is_urdf:
+                    verts = deform_mesh(ang, self.urdf_solver.fk_solver, self.urdf_mesh_data)
+                    faces = self.urdf_mesh_data["faces"]
+                else:
+                    verts = deform_mesh(ang, self.solver.fk_solver, self.mesh_data)
+                    faces = self.mesh_data["faces"]
+                frames.append({"vertices": verts.tolist(), "faces": faces.tolist()})
+            except Exception as e:
+                logger.warning(f"Mesh deformation failed: {e}")
         return frames
 
-    def solve_agent(self, payload, last_only=False):
-        mand, opt, sub = self._build_agent_objectives(payload)
-        start = time.time()
-        best_angles, obj_val, steps = self.solver.solve(
-            initial_rotations=self.initial_rotations,
-            learning_rate=self.args.learning_rate,
+    # ---------- Configuration ----------
+    def configure_agent(self, bones, eff, num_steps=None):
+        with self.config_lock:
+            if num_steps is None:
+                num_steps = self.current_num_steps
+            num_steps = self._sanitize_num_steps(num_steps)
+            bones = self._sanitize_bones(bones, False) or self.default_controlled_bones
+            if eff not in getattr(self, 'available_bones', []):
+                eff = self.default_end_effector
+            changed = (bones != self.current_controlled_bones or eff != self.current_end_effector or int(num_steps) != int(self.current_num_steps))
+            if changed:
+                try:
+                    self.current_controlled_bones = bones
+                    self.current_end_effector = eff
+                    self.current_num_steps = int(num_steps)
+                    self.solver = self._get_solver(bones, False, self.current_num_steps)
+                    self.initial_rotations = np.zeros(len(self.solver.controlled_bones)*3, dtype=np.float32)
+                    self.best_angles = self.initial_rotations.copy()
+                    self._setup_agent_objectives()
+                except Exception as e:
+                    self.agent_fail_count += 1
+                    self.last_agent_error = str(e)
+                    logger.exception("configure_agent failed; attempting default recovery")
+                    if not self._attempt_solver_recovery(False):
+                        raise
+            return {"controlled_bones": self.current_controlled_bones, "end_effector": self.current_end_effector, "num_steps": self.current_num_steps}
+
+    def configure_urdf(self, bones, eff, num_steps=None):
+        with self.config_lock:
+            if num_steps is None:
+                num_steps = self.urdf_current_num_steps
+            num_steps = self._sanitize_num_steps(num_steps)
+            bones = self._sanitize_bones(bones, True) or self.urdf_default_controlled_bones
+            if eff not in getattr(self, 'urdf_available_bones', []):
+                eff = self.urdf_default_end_effector
+            changed = (bones != self.urdf_current_controlled_bones or eff != self.urdf_current_end_effector or int(num_steps) != int(self.urdf_current_num_steps))
+            if changed:
+                try:
+                    self.urdf_current_controlled_bones = bones
+                    self.urdf_current_end_effector = eff
+                    self.urdf_current_num_steps = int(num_steps)
+                    self.urdf_solver = self._get_solver(bones, True, self.urdf_current_num_steps)
+                    self.urdf_initial_rotations = np.zeros(len(self.urdf_solver.controlled_bones)*3, dtype=np.float32)
+                    self.urdf_best_angles = self.urdf_initial_rotations.copy()
+                    self._setup_urdf_objectives()
+                except Exception as e:
+                    self.urdf_fail_count += 1
+                    self.last_urdf_error = str(e)
+                    logger.exception("configure_urdf failed; attempting default recovery")
+                    if not self._attempt_solver_recovery(True):
+                        raise
+            return {"controlled_bones": self.urdf_current_controlled_bones, "end_effector": self.urdf_current_end_effector, "num_steps": self.urdf_current_num_steps}
+
+    # ---------- Solve (hardened) ----------
+    def _solve_core(self, solver, init, mand, opt, subpoints, lr):
+        return solver.solve(
+            initial_rotations=init,
+            learning_rate=lr,
             mandatory_objective_functions=tuple(mand),
             optional_objective_functions=tuple(opt),
-            ik_points=sub,
+            ik_points=subpoints,
             verbose=False,
         )
-        self.best_angles = best_angles[-1].copy()
-        self.initial_rotations = self.best_angles.copy()
-        if last_only:
-            self.animation_frames_agent = self._frames_from_angles([best_angles[-1]], False)
+
+    def solve_agent(self, payload, last_only=False):
+        if time.time() < self.circuit_open_until:
+            return {"status":"circuit_open","error":"agent circuit open","frames":len(self.animation_frames_agent)}
+        mand, opt, sub = self._build_agent_objectives(payload)
+        lr = self._safe_float(payload.get("learning_rate", self.args.learning_rate), self.args.learning_rate, 1e-5, self.max_learning_rate)
+        attempts = 0
+        best_angles = None; obj_val = float('inf'); steps = 0; error_msg=None; recovered=False
+        start=time.time()
+        while attempts < self.max_fail_retries:
+            attempts += 1
+            try:
+                best_angles, obj_val, steps = self._solve_core(self.solver, self.initial_rotations, mand, opt, sub, lr)
+                break
+            except Exception as e:
+                self.agent_fail_count += 1
+                error_msg = str(e)
+                self.last_agent_error = error_msg
+                logger.warning(f"Agent solve attempt {attempts} failed: {e}")
+                if attempts < self.max_fail_retries:
+                    # First retry: rebuild current; second: full recovery
+                    if attempts == 1:
+                        self._rebuild_solver_safe(False, force_defaults=False)
+                    else:
+                        recovered = self._attempt_solver_recovery(False)
+                else:
+                    break
+        if best_angles is None:
+            # Provide at least one frame (initial)
+            frame_angles = [self.initial_rotations]
+            if recovered:
+                frame_angles = [self.initial_rotations]
         else:
-            self.animation_frames_agent = self._frames_from_angles(best_angles, False)
+            self.best_angles = best_angles[-1].copy()
+            self.initial_rotations = self.best_angles.copy()
+            frame_angles = [best_angles[-1]] if last_only else best_angles
+        self.animation_frames_agent = self._frames_from_angles(frame_angles, False)
         self.agent_solve_counter += 1
+        # Circuit breaker update
+        if best_angles is None and self.agent_fail_count >= self.circuit_breaker_threshold:
+            self.circuit_open_until = time.time() + self.circuit_backoff_seconds
+        status = "ok" if best_angles is not None else ("recovered" if recovered else "failed")
         return {
+            "status":status,
             "solve_time": time.time()-start,
             "iterations": steps,
             "objective": obj_val,
             "frames": len(self.animation_frames_agent),
             "solve_id": self.agent_solve_counter,
+            "attempts": attempts,
+            "error": error_msg,
         }
 
     def solve_urdf(self, payload, last_only=False):
+        if time.time() < self.circuit_open_until:
+            return {"status":"circuit_open","error":"urdf circuit open","frames":len(self.animation_frames_urdf)}
         mand, opt, sub = self._build_urdf_objectives(payload)
-        start = time.time()
-        best_angles, obj_val, steps = self.urdf_solver.solve(
-            initial_rotations=self.urdf_initial_rotations,
-            learning_rate=self.args.learning_rate,
-            mandatory_objective_functions=tuple(mand),
-            optional_objective_functions=tuple(opt),
-            ik_points=sub,
-            verbose=False,
-        )
-        self.urdf_best_angles = best_angles[-1].copy()
-        self.urdf_initial_rotations = self.urdf_best_angles.copy()
-        if last_only:
-            self.animation_frames_urdf = self._frames_from_angles([best_angles[-1]], True)
+        lr = self._safe_float(payload.get("learning_rate", self.args.learning_rate), self.args.learning_rate, 1e-5, self.max_learning_rate)
+        attempts=0
+        best_angles=None; obj_val=float('inf'); steps=0; error_msg=None; recovered=False
+        start=time.time()
+        while attempts < self.max_fail_retries:
+            attempts += 1
+            try:
+                best_angles, obj_val, steps = self._solve_core(self.urdf_solver, self.urdf_initial_rotations, mand, opt, sub, lr)
+                break
+            except Exception as e:
+                self.urdf_fail_count += 1
+                error_msg = str(e)
+                self.last_urdf_error = error_msg
+                logger.warning(f"URDF solve attempt {attempts} failed: {e}")
+                if attempts < self.max_fail_retries:
+                    if attempts == 1:
+                        self._rebuild_solver_safe(True, force_defaults=False)
+                    else:
+                        recovered = self._attempt_solver_recovery(True)
+                else:
+                    break
+        if best_angles is None:
+            frame_angles = [self.urdf_initial_rotations]
+            if recovered:
+                frame_angles = [self.urdf_initial_rotations]
         else:
-            self.animation_frames_urdf = self._frames_from_angles(best_angles, True)
+            self.urdf_best_angles = best_angles[-1].copy()
+            self.urdf_initial_rotations = self.urdf_best_angles.copy()
+            frame_angles = [best_angles[-1]] if last_only else best_angles
+        self.animation_frames_urdf = self._frames_from_angles(frame_angles, True)
         self.urdf_solve_counter += 1
+        if best_angles is None and self.urdf_fail_count >= self.circuit_breaker_threshold:
+            self.circuit_open_until = time.time() + self.circuit_backoff_seconds
+        status = "ok" if best_angles is not None else ("recovered" if recovered else "failed")
         return {
+            "status":status,
             "solve_time": time.time()-start,
             "iterations": steps,
             "objective": obj_val,
             "frames": len(self.animation_frames_urdf),
             "solve_id": self.urdf_solve_counter,
+            "attempts": attempts,
+            "error": error_msg,
         }
 
     # ---------- Housekeeping ----------
     def _cleanup(self):
         now = time.time()
-        if now - self.last_cleanup_time < self.cleanup_interval: return
-        gc.collect()
+        if now - self.last_cleanup_time < self.cleanup_interval:
+            return
+        try:
+            gc.collect()
+            # Memory guard: if RSS > 2GB, clear caches
+            rss = self.process.memory_info().rss
+            if rss > 2 * 1024**3:
+                logger.warning("High memory usage detected; invalidating caches")
+                self._invalidate_caches()
+        except Exception:
+            pass
         self.last_cleanup_time = now
 
     # ---------- API ----------
     def _register_routes(self):
         @self.app.get("/")
-        def index(): return FileResponse("static/index.html")
+        def index():
+            return FileResponse("static/index.html")
 
         @self.app.get("/threejs_viewer")
-        def legacy(): return FileResponse("static/index.html")
+        def legacy():
+            return FileResponse("static/index.html")
 
         @self.app.get("/animation")
         def animation(request: Request):
@@ -463,8 +675,11 @@ class IKServer:
                     "end_effector_choices": self.urdf_available_bones,
                     "hand_shapes": [],
                     "hand_positions": [],
-                    "max_subpoints": 20,  # added
-                    "default_num_steps": self.urdf_current_num_steps,  # NEW
+                    "max_subpoints": 20,
+                    "default_num_steps": self.urdf_current_num_steps,
+                    "collision_default_center": getattr(self, 'urdf_collision_default_center', [0.2,0.0,0.35]),
+                    "collision_default_radius": getattr(self, 'urdf_collision_default_radius', 0.1),
+                    "collision_default_min_clearance": getattr(self, 'urdf_collision_default_min_clearance', 0.0),
                 }
             return {
                 "model":"agent",
@@ -479,48 +694,50 @@ class IKServer:
                     "Look 45° X Downwards","Look 45° X Upwards","Look X Inward","Look to Body",
                     "Arm Down","Arm 45° Down","Arm Flat"
                 ],
-                "max_subpoints": 20,  # added
-                "default_num_steps": self.current_num_steps,  # NEW
+                "max_subpoints": 20,
+                "default_num_steps": self.current_num_steps,
+                "collision_default_center": getattr(self, 'collision_default_center', [0.1,0.0,0.35]),
+                "collision_default_radius": getattr(self, 'collision_default_radius', 0.1),
+                "collision_default_min_clearance": getattr(self, 'collision_default_min_clearance', 0.0),
             }
 
         @self.app.post("/configure")
         async def configure(request: Request):
             payload = await request.json()
             model = payload.get("model","agent").lower()
-            num_steps = int(payload.get("num_steps", self.args.num_steps))
-            if model == "pepper":
-                cfg = self.configure_urdf(payload.get("controlled_bones", []), payload.get("end_effector"), num_steps=num_steps)
-            else:
-                cfg = self.configure_agent(payload.get("controlled_bones", []), payload.get("end_effector"), num_steps=num_steps)
-            return JSONResponse({"status":"ok","config":cfg})
+            num_steps = self._sanitize_num_steps(payload.get("num_steps", self.args.num_steps))
+            try:
+                if model == "pepper":
+                    cfg = self.configure_urdf(payload.get("controlled_bones", []), payload.get("end_effector"), num_steps=num_steps)
+                else:
+                    cfg = self.configure_agent(payload.get("controlled_bones", []), payload.get("end_effector"), num_steps=num_steps)
+                return JSONResponse({"status":"ok","config":cfg})
+            except Exception as e:
+                return JSONResponse({"status":"error","message":str(e)}, status_code=500)
 
         @self.app.post("/solve")
         async def solve(request: Request):
             payload = await request.json()
             model = payload.get("model","agent").lower()
             return_mode = payload.get("frames_mode", "auto")
-            num_steps = int(payload.get("num_steps", self.args.num_steps))  # NEW
-            subpoints = int(payload.get("subpoints",1))
+            num_steps = self._sanitize_num_steps(payload.get("num_steps", self.args.num_steps))
+            subpoints = self._safe_int(payload.get("subpoints",1),1,1,100)
             last_only = (return_mode != 'all' and subpoints == 1)
             self._cleanup()
             with self.solve_lock:
                 try:
                     if model == "pepper":
-                        self.configure_urdf(payload.get("controlled_bones", []),
-                                            payload.get("end_effector", self.urdf_current_end_effector),
-                                            num_steps=num_steps)
+                        self.configure_urdf(payload.get("controlled_bones", []), payload.get("end_effector", self.urdf_current_end_effector), num_steps=num_steps)
                         result = self.solve_urdf(payload, last_only=last_only); result["model"]="pepper"; result["num_steps"] = num_steps
                         if last_only:
-                            frames = list(self.animation_frames_urdf)  # only one frame
+                            frames = list(self.animation_frames_urdf)
                         elif return_mode == "all" or (return_mode == "auto" and subpoints > 1):
                             frames = list(self.animation_frames_urdf)
                         else:
                             frames = [self.animation_frames_urdf[-1]]
                         result["frames_data"] = frames
                     else:
-                        self.configure_agent(payload.get("controlled_bones", []),
-                                             payload.get("end_effector", self.current_end_effector),
-                                             num_steps=num_steps)
+                        self.configure_agent(payload.get("controlled_bones", []), payload.get("end_effector", self.current_end_effector), num_steps=num_steps)
                         result = self.solve_agent(payload, last_only=last_only); result["model"]="agent"; result["num_steps"] = num_steps
                         if last_only:
                             frames = list(self.animation_frames_agent)
@@ -529,35 +746,53 @@ class IKServer:
                         else:
                             frames = [self.animation_frames_agent[-1]]
                         result["frames_data"] = frames
-                    return JSONResponse({"status":"ok","result":result})
+                    status = result.get("status","ok")
+                    top_status = "ok" if status in ("ok","recovered") else status
+                    return JSONResponse({"status":top_status,"result":result})
                 except Exception as e:
-                    logger.exception("Solve failed")
+                    logger.exception("Solve failed unrecoverably")
                     return JSONResponse({"status":"error","message":str(e)}, status_code=500)
 
+        @self.app.post("/reset")
+        def reset():
+            with self.solve_lock:
+                self._invalidate_caches()
+                self._attempt_solver_recovery(False)
+                self._attempt_solver_recovery(True)
+                return {"status":"ok","message":"solvers & caches reset"}
+
         @self.app.get("/health")
         def health():
+            rss = self.process.memory_info().rss if self.process else 0
             return {
                 "status":"ok",
                 "agent_frames": len(self.animation_frames_agent),
                 "urdf_frames": len(self.animation_frames_urdf),
                 "cache_agent": len(self.solver_cache),
                 "cache_urdf": len(self.urdf_solver_cache),
+                "agent_fail_count": self.agent_fail_count,
+                "urdf_fail_count": self.urdf_fail_count,
+                "last_agent_error": self.last_agent_error,
+                "last_urdf_error": self.last_urdf_error,
+                "circuit_open_for": max(0, int(self.circuit_open_until - time.time())),
+                "memory_rss_mb": round(rss/1024/1024,1),
             }
 
         @self.app.get("/favicon.ico")
-        def favicon(): return Response(status_code=204)
+        def favicon():
+            return Response(status_code=204)
 
 # ---------- Main ----------
 def main():
     parser = configargparse.ArgumentParser(description="Inverse Kinematics Solver - Three.js Web UI", default_config_files=["config.ini"])
-    parser.add("--gltf_file", type=str, default="files/smplx.glb")
-    parser.add("--hand", type=str, choices=["left","right"], default="left")
-    parser.add("--threshold", type=float, default=0.0001)
-    parser.add("--num_steps", type=int, default=100)
-    parser.add("--learning_rate", type=float, default=0.2)
-    parser.add("--subpoints", type=int, default=1)
-    parser.add("--api_port", type=int, default=17861)
-    parser.add("--warmup", action='store_true', default=True, help="Enable background JIT warmup")
+    parser.add_argument("--gltf_file", type=str, default="files/smplx.glb")
+    parser.add_argument("--hand", type=str, choices=["left","right"], default="left")
+    parser.add_argument("--threshold", type=float, default=0.0001)
+    parser.add_argument("--num_steps", type=int, default=100)
+    parser.add_argument("--learning_rate", type=float, default=0.2)
+    parser.add_argument("--subpoints", type=int, default=1)
+    parser.add_argument("--api_port", type=int, default=17861)
+    parser.add_argument("--warmup", action='store_true', default=True, help="Enable background JIT warmup")
     args = parser.parse_args()
 
     download_and_setup_files()

static/index.html

@@ -37,6 +37,9 @@
   .toast.error { border-color:#b33939; }
   .toast .toast-close { float:right; cursor:pointer; color:#888; margin-left:6px; }
   .toast .toast-close:hover { color:#fff; }
+  .axis-pair { margin:4px 0; }
+  .axis-pair .flex-row { align-items:center; }
+  .axis-pair span { width:14px; display:inline-block; font-size:11px; color:#aaa; }
 </style>
 <!-- Added import map to resolve bare specifier 'three' -->
 <script type="importmap">
@@ -90,8 +93,36 @@
       <legend>Primary Objectives</legend>
       <label><input id="distance_enabled" type="checkbox" checked> Distance Objective</label>
       <label>Distance Weight <input id="distance_weight" type="number" value="1.0" step="0.1"></label>
-      <label hidden style="display:none;"><input id="collision_enabled" type="checkbox"> Collision Avoidance</label>
-      <label hidden style="display:none;">Collision Weight <input id="collision_weight" type="number" value="1.0" step="0.1"></label>
+    </fieldset>
+    <fieldset id="collisionFieldset">
+      <legend>Collision Sphere</legend>
+      <label><input id="collision_enabled" type="checkbox"> Enable Collision Sphere</label>
+      <div class="axis-pair">X
+        <div class="flex-row">
+          <input id="collision_cx" type="number" value="0.1" step="0.01">
+          <input id="collision_cx_slider" type="range" min="-1" max="1" step="0.01" value="0.1">
+        </div>
+      </div>
+      <div class="axis-pair">Y
+        <div class="flex-row">
+          <input id="collision_cy" type="number" value="0.0" step="0.01">
+          <input id="collision_cy_slider" type="range" min="-1" max="1" step="0.01" value="0.0">
+        </div>
+      </div>
+      <div class="axis-pair">Z
+        <div class="flex-row">
+          <input id="collision_cz" type="number" value="0.35" step="0.01">
+          <input id="collision_cz_slider" type="range" min="-1" max="1" step="0.01" value="0.35">
+        </div>
+      </div>
+      <label>Collision Weight <input id="collision_weight" type="number" value="1.0" step="0.1"></label>
+      <label>Sphere Radius <input id="collision_radius" type="number" value="0.1" step="0.01" min="0.01" max="1.0"></label>
+      <label>Min Clearance
+        <div class="flex-row">
+          <input id="collision_min_clearance" type="number" value="0.0" step="0.005" min="0" max="0.5">
+          <input id="collision_min_clearance_slider" type="range" min="0" max="0.5" step="0.005" value="0.0">
+        </div>
+      </label>
     </fieldset>
     <fieldset>
       <legend>Regularization</legend>
@@ -166,46 +197,28 @@ import { GLTFLoader } from 'https://unpkg.com/[email protected]/examples/jsm/loaders
   let gltfPrimaryMaterialCaptured = false;
   let fallbackMesh = null;
   let lastTime = performance.now();
-  // REMOVED: let lastFrameFetch = 0;
-  // REMOVED: const pollInterval = 1000;
-  // (ADDED) Solve ID tracking to avoid "lastSolveIdAgent / lastSolveIdPepper is not defined" errors
   let lastSolveIdAgent = 0;
   let lastSolveIdPepper = 0;
 
-  // ----- Auto-solve (moved early to avoid ReferenceError) -----
+  // ----- Auto-solve -----
   let solving = false;
-  let solvePending = false;
   let solveDebounceTimer = null;
-  const SOLVE_DEBOUNCE_MS = 25; // reduced from 40
-  let currentSolveController = null; // NEW for cancellation
-
+  const SOLVE_DEBOUNCE_MS = 25;
+  let currentSolveController = null;
   function scheduleSolve(immediate=false){
-    if (solving){
-      // Abort in-flight solve and queue new immediate solve
-      if (currentSolveController){ currentSolveController.abort(); }
-    }
-    if (immediate){
-      // Fire instantly for first / critical interactions
-      triggerSolve(true);
-      return;
-    }
+    if (solving){ if (currentSolveController) currentSolveController.abort(); }
+    if (immediate){ triggerSolve(true); return; }
     if (solveDebounceTimer) clearTimeout(solveDebounceTimer);
-    solveDebounceTimer = setTimeout(()=> { triggerSolve(false); }, SOLVE_DEBOUNCE_MS);
+    solveDebounceTimer = setTimeout(()=> triggerSolve(false), SOLVE_DEBOUNCE_MS);
   }
-
-  async function triggerSolve(isImmediate=false){
-    if (solving){
-      // In-flight already (might have just been aborted). Let doSolve handle continuation.
-    }
-    if (currentSolveController){ currentSolveController.abort(); }
+  async function triggerSolve(){
+    if (currentSolveController) currentSolveController.abort();
     currentSolveController = new AbortController();
     const controller = currentSolveController;
     solving = true;
     try { await doSolve(controller.signal); }
-    catch(e){ if (e.name !== 'AbortError') {/* logged in doSolve */} }
-    finally {
-      if (controller === currentSolveController){ solving = false; }
-    }
+    catch(e){ if (e.name !== 'AbortError') {} }
+    finally { if (controller === currentSolveController) solving = false; }
   }
 
   // Playback state
@@ -219,19 +232,14 @@ import { GLTFLoader } from 'https://unpkg.com/[email protected]/examples/jsm/loaders
   let alignAttemptCount = 0;
   const maxAlignAttempts = 120;
 
-  // --- Trajectory / spline state (ADDED) ---
+  // Trajectory / spline state
   let controlFrames = [];
   let splineMode = false;
   let splineU = 0;
-  let splineDuration = 2.0; // seconds for full path
-  // One-time initial camera framing flag (ADDED)
+  let splineDuration = 2.0;
   let initialCameraFramed = false;
 
   function cleanupVisuals(){
-    if (ikMesh && usingGLTFGeometry){
-      // keep GLTF mesh for possible reuse after reconfigure? remove anyway for clean slate
-      ikMesh = null;
-    }
     if (fallbackMesh){
       modelGroup.remove(fallbackMesh);
       if (fallbackMesh.geometry) fallbackMesh.geometry.dispose();
@@ -239,84 +247,34 @@ import { GLTFLoader } from 'https://unpkg.com/[email protected]/examples/jsm/loaders
       fallbackMesh = null;
     }
     if (gltfRoot){
-      gltfRoot.traverse(o=>{
-        if (o.isMesh){
-          if (o.geometry) o.geometry.dispose();
-          if (o.material){
-            if (Array.isArray(o.material)) o.material.forEach(m=>m.dispose());
-            else o.material.dispose();
-          }
-        }
-      });
-      modelGroup.remove(gltfRoot);
-      gltfRoot = null;
+      gltfRoot.traverse(o=>{ if (o.isMesh){ if (o.geometry) o.geometry.dispose(); if (o.material){ if (Array.isArray(o.material)) o.material.forEach(m=>m.dispose()); else o.material.dispose(); } } });
+      modelGroup.remove(gltfRoot); gltfRoot = null;
     }
-    // Remove any residual children
     while (modelGroup.children.length) modelGroup.remove(modelGroup.children[0]);
-
-    gltfPrimaryMesh = null;
-    gltfPrimaryMaterial = null;
-    gltfPrimaryMaterialCaptured = false;
-    ikMesh = null;
-    usingGLTFGeometry = false;
-    animationFrames = [];
-    frameIndex = 0;
-    baseOffsetY = 0;
-    groundAligned = false;
-    pendingAlign = false;
-    alignAttemptCount = 0;
-    log("Visuals cleaned.");
-  }
-
-  function scheduleGroundAlign(force=false){
-    if (force){
-      groundAligned = false;
-      alignAttemptCount = 0;
-    }
-    pendingAlign = true;
+    gltfPrimaryMesh = null; gltfPrimaryMaterial = null; gltfPrimaryMaterialCaptured = false; ikMesh = null; usingGLTFGeometry = false;
+    animationFrames = []; frameIndex = 0; baseOffsetY = 0; groundAligned = false; pendingAlign = false; alignAttemptCount = 0;
+    log('Visuals cleaned.');
   }
-
+  function scheduleGroundAlign(force=false){ if (force){ groundAligned=false; alignAttemptCount=0; } pendingAlign=true; }
   function performGroundAlign(){
     if (!pendingAlign) return;
-    if (!modelGroup.children.length){
-      if (++alignAttemptCount > maxAlignAttempts) pendingAlign = false;
-      return;
-    }
+    if (!modelGroup.children.length){ if (++alignAttemptCount > maxAlignAttempts) pendingAlign=false; return; }
     const box = new THREE.Box3().setFromObject(modelGroup);
-    if (box.isEmpty() || !isFinite(box.min.y)){
-      if (++alignAttemptCount > maxAlignAttempts) pendingAlign = false;
-      return;
-    }
-    const currentMin = box.min.y;
-    // Desired world shift so new min ~= 0
-    const delta = -currentMin;
-    // Avoid large downward moves; only allow raising or tiny corrections
+    if (box.isEmpty() || !isFinite(box.min.y)){ if (++alignAttemptCount > maxAlignAttempts) pendingAlign=false; return; }
+    const currentMin = box.min.y; const delta = -currentMin;
     if (delta > 0 || Math.abs(delta) < 1e-3){
-      modelGroup.position.y += delta;
-      baseOffsetY = modelGroup.position.y;
-      groundAligned = true;
-      updateTargetSphere(); // (NEW) keep target indicator vertically aligned
+      modelGroup.position.y += delta; baseOffsetY = modelGroup.position.y; groundAligned = true; updateTargetSphere(); updateCollisionSphere();
     }
-    pendingAlign = false;
+    pendingAlign=false;
   }
-
-  // (ADD) fitCamera helper (uses modelGroup if populated)
   function fitCamera(obj){
-    const targetObj = modelGroup.children.length ? modelGroup : obj;
-    if (!targetObj) return;
-    const box = new THREE.Box3().setFromObject(targetObj);
-    if (box.isEmpty()) return;
-    const size = box.getSize(new THREE.Vector3());
-    const center = box.getCenter(new THREE.Vector3());
-    controls.target.copy(center);
-    const maxDim = Math.max(size.x,size.y,size.z);
-    const dist = maxDim * 3;
-    const dir = new THREE.Vector3(0.0,0.5,1).normalize();
+    const targetObj = modelGroup.children.length ? modelGroup : obj; if (!targetObj) return;
+    const box = new THREE.Box3().setFromObject(targetObj); if (box.isEmpty()) return;
+    const size = box.getSize(new THREE.Vector3()); const center = box.getCenter(new THREE.Vector3());
+    controls.target.copy(center); const maxDim = Math.max(size.x,size.y,size.z);
+    const dist = maxDim * 3; const dir = new THREE.Vector3(0.0,0.5,1).normalize();
     camera.position.copy(center).addScaledVector(dir, dist);
-    camera.near = maxDim/100;
-    camera.far = maxDim*100;
-    camera.updateProjectionMatrix();
-    controls.update();
+    camera.near = maxDim/100; camera.far = maxDim*100; camera.updateProjectionMatrix(); controls.update();
   }
 
   // ----- DOM refs -----
@@ -327,10 +285,21 @@ import { GLTFLoader } from 'https://unpkg.com/[email protected]/examples/jsm/loaders
   const handPosSel = document.getElementById('hand_position');
   const handFieldset = document.getElementById('handFieldset');
   const modelBadge = document.getElementById('modelBadge');
-  const subpointsInput = document.getElementById('subpoints'); // ADDED
-  const numStepsInput = document.getElementById('num_steps'); // NEW
+  const subpointsInput = document.getElementById('subpoints');
+  const numStepsInput = document.getElementById('num_steps');
+  // Collision inputs
+  const collisionEnabledEl = document.getElementById('collision_enabled');
+  const collisionWeightEl = document.getElementById('collision_weight');
+  const collCx = document.getElementById('collision_cx');
+  const collCy = document.getElementById('collision_cy');
+  const collCz = document.getElementById('collision_cz');
+  const collCxSlider = document.getElementById('collision_cx_slider');
+  const collCySlider = document.getElementById('collision_cy_slider');
+  const collCzSlider = document.getElementById('collision_cz_slider');
+  const collRadiusEl = document.getElementById('collision_radius');
+  const collMinClearEl = document.getElementById('collision_min_clearance');
+  const collMinClearSlider = document.getElementById('collision_min_clearance_slider');
 
-  // Inputs
   function val(id){ return document.getElementById(id).value; }
   function num(id){ return parseFloat(val(id)); }
   function bool(id){ return document.getElementById(id).checked; }
@@ -341,743 +310,163 @@ import { GLTFLoader } from 'https://unpkg.com/[email protected]/examples/jsm/loaders
   renderer.setSize(document.getElementById('viewerPane').clientWidth, document.getElementById('viewerPane').clientHeight);
   renderer.domElement.className = 'viewer-canvas';
   document.getElementById('viewerPane').appendChild(renderer.domElement);
-
-  const scene = new THREE.Scene();
-  scene.background = new THREE.Color(0x222222);
-
+  const scene = new THREE.Scene(); scene.background = new THREE.Color(0x222222);
   const camera = new THREE.PerspectiveCamera(45, renderer.domElement.clientWidth / renderer.domElement.clientHeight, 0.01, 1000);
   camera.position.set(0,1,3);
-
-  const controls = new OrbitControls(camera, renderer.domElement);
-  controls.target.set(0,0.8,0);
-
+  const controls = new OrbitControls(camera, renderer.domElement); controls.target.set(0,0.8,0);
   scene.add(new THREE.HemisphereLight(0xffffff,0x444444,1.0));
-  const d = new THREE.DirectionalLight(0xffffff,0.8);
-  d.position.set(3,10,10); scene.add(d);
-
-  const ground = new THREE.Mesh(
-    new THREE.CircleGeometry(5,48),
-    new THREE.MeshStandardMaterial({color:0x303030, metalness:0.1, roughness:0.9})
-  );
-  ground.rotation.x = -Math.PI/2;
-  ground.receiveShadow = true;
-  scene.add(ground);
-  ground.visible = false; // DISABLED ground plane
+  const d = new THREE.DirectionalLight(0xffffff,0.8); d.position.set(3,10,10); scene.add(d);
+  const ground = new THREE.Mesh(new THREE.CircleGeometry(5,48), new THREE.MeshStandardMaterial({color:0x303030, metalness:0.1, roughness:0.9}));
+  ground.rotation.x = -Math.PI/2; ground.receiveShadow = true; scene.add(ground); ground.visible = false;
+  const modelGroup = new THREE.Group(); scene.add(modelGroup);
 
-  // (ADD) Group to apply vertical lift without altering raw vertex data
-  const modelGroup = new THREE.Group();
-  scene.add(modelGroup);
-
-  // (ADDED) Target sphere visual
+  // Target sphere
   const targetSphereGeom = new THREE.SphereGeometry(0.01, 24, 24);
   const targetSphereMat = new THREE.MeshStandardMaterial({color:0x00ff55, emissive:0x008833});
-  const targetSphere = new THREE.Mesh(targetSphereGeom, targetSphereMat);
-  scene.add(targetSphere);
+  const targetSphere = new THREE.Mesh(targetSphereGeom, targetSphereMat); scene.add(targetSphere);
+  // Collision sphere (unit scaled later)
+  const collisionSphereGeom = new THREE.SphereGeometry(1, 32, 32);
+  const collisionSphereMat = new THREE.MeshStandardMaterial({color:0xff4444, emissive:0x660000, transparent:true, opacity:0.18, wireframe:false});
+  const collisionSphere = new THREE.Mesh(collisionSphereGeom, collisionSphereMat); scene.add(collisionSphere); collisionSphere.visible = false;
 
-  // (REPLACED) updateTargetSphere: now accounts for modelGroup translation (baseOffsetY)
-  // and uniform scaling (Pepper) on all axes so sphere aligns with the visual robot pose.
   function updateTargetSphere(){
     const xRaw = parseFloat(document.getElementById('target_x').value) || 0;
     const yRaw = parseFloat(document.getElementById('target_y').value) || 0;
     const zRaw = parseFloat(document.getElementById('target_z').value) || 0;
-    // Model transform
-    const sx = modelGroup?.scale?.x ?? 1;
-    const sy = modelGroup?.scale?.y ?? 1;
-    const sz = modelGroup?.scale?.z ?? 1;
-    const tx = modelGroup?.position?.x ?? 0;
-    const ty = modelGroup?.position?.y ?? 0;  // this already equals baseOffsetY after ground align
-    const tz = modelGroup?.position?.z ?? 0;
-    // Apply uniform / per-axis scaling then translation so sphere sits where the solver
-    // target maps in the displayed (scaled & lifted) robot coordinates.
-    targetSphere.position.set(
-      xRaw * sx + tx,
-      yRaw * sy + ty,
-      zRaw * sz + tz
-    );
+    const sx = modelGroup?.scale?.x ?? 1; const sy = modelGroup?.scale?.y ?? 1; const sz = modelGroup?.scale?.z ?? 1;
+    const tx = modelGroup?.position?.x ?? 0; const ty = modelGroup?.position?.y ?? 0; const tz = modelGroup?.position?.z ?? 0;
+    targetSphere.position.set(xRaw * sx + tx, yRaw * sy + ty, zRaw * sz + tz);
   }
-
-  // Attach live update listeners
-  ['target_x','target_y','target_z'].forEach(id=>{
-    document.getElementById(id).addEventListener('input', updateTargetSphere);
-    document.getElementById(id).addEventListener('change', updateTargetSphere);
-  });
-
-  // Initial placement after DOM ready
-  updateTargetSphere();
-
-  window.addEventListener('resize', () => {
-    renderer.setSize(document.getElementById('viewerPane').clientWidth, document.getElementById('viewerPane').clientHeight);
-    camera.aspect = renderer.domElement.clientWidth / renderer.domElement.clientHeight;
-    camera.updateProjectionMatrix();
-  });
-
-  function resetCamera(){
-    camera.position.set(0,1,3);
-    controls.target.set(0,0.8,0);
-    controls.update();
+  function updateCollisionSphere(){
+    const cx = parseFloat(collCx.value) || 0; const cy = parseFloat(collCy.value) || 0; const cz = parseFloat(collCz.value) || 0;
+    const r = parseFloat(collRadiusEl.value) || 0.1; const mc = parseFloat(collMinClearEl.value) || 0.0;
+    const sx = modelGroup?.scale?.x ?? 1; const sy = modelGroup?.scale?.y ?? 1; const sz = modelGroup?.scale?.z ?? 1; const tx = modelGroup?.position?.x ?? 0; const ty = modelGroup?.position?.y ?? 0; const tz = modelGroup?.position?.z ?? 0;
+    collisionSphere.position.set(cx * sx + tx, cy * sy + ty, cz * sz + tz);
+    const uniform = (sx+sy+sz)/3; const effective = r + mc; // visualize base radius + clearance
+    collisionSphere.scale.set(effective*uniform, effective*uniform, effective*uniform);
+    collisionSphere.visible = collisionEnabledEl.checked;
   }
+  ['target_x','target_y','target_z'].forEach(id=>{ document.getElementById(id).addEventListener('input', updateTargetSphere); document.getElementById(id).addEventListener('change', updateTargetSphere); });
+  updateTargetSphere(); updateCollisionSphere();
+
+  window.addEventListener('resize', () => { renderer.setSize(document.getElementById('viewerPane').clientWidth, document.getElementById('viewerPane').clientHeight); camera.aspect = renderer.domElement.clientWidth / renderer.domElement.clientHeight; camera.updateProjectionMatrix(); });
+  function resetCamera(){ camera.position.set(0,1,3); controls.target.set(0,0.8,0); controls.update(); }
 
   // ----- Config fetch -----
-  async function loadConfig(){
-    const res = await fetch(`/config?model=${currentModel}`);
-    configData = await res.json();
-    populateUIFromConfig();
-    log(`Config loaded for ${currentModel}`);
-  }
+  async function loadConfig(){ const res = await fetch(`/config?model=${currentModel}`); configData = await res.json(); populateUIFromConfig(); log(`Config loaded for ${currentModel}`); }
 
   function populateUIFromConfig(){
-    // End effector
-    endEffectorSel.innerHTML = "";
-    configData.end_effector_choices.forEach(b=>{
-      const opt = document.createElement('option');
-      opt.value = b; opt.textContent = b;
-      if (b === configData.default_end_effector) opt.selected = true;
-      endEffectorSel.appendChild(opt);
-    });
-    // Bones
-    bonesContainer.innerHTML = "";
-    configData.selectable_bones.forEach(b=>{
-      const id = `bone_${b}`;
-      const label = document.createElement('label');
-      label.innerHTML = `<input type="checkbox" id="${id}" ${configData.default_controlled_bones.includes(b)?'checked':''}> ${b}`;
-      bonesContainer.appendChild(label);
-      // Add listener for auto-solve
-      setTimeout(()=>{ // ensure element in DOM
-        const cb = document.getElementById(id);
-        if (cb) cb.addEventListener('change', ()=> scheduleSolve());
-      },0);
-    });
-    // Hand controls (agent only)
-    if (currentModel === 'agent'){
-      handFieldset.style.display = '';
-      handShapeSel.innerHTML = "";
-      configData.hand_shapes.forEach(s=>{
-        const opt = document.createElement('option');
-        opt.value = s; opt.textContent = s;
-        handShapeSel.appendChild(opt);
-      });
-      handPosSel.innerHTML = "";
-      configData.hand_positions.forEach(s=>{
-        const opt = document.createElement('option');
-        opt.value = s; opt.textContent = s;
-        handPosSel.appendChild(opt);
-      });
-    } else {
-      handFieldset.style.display = 'none';
-    }
-    // Set subpoints max if provided
-    if (configData && typeof configData.max_subpoints !== 'undefined'){
-      subpointsInput.max = configData.max_subpoints;
-    }
-    if (configData && typeof configData.default_num_steps !== 'undefined'){
-      numStepsInput.value = configData.default_num_steps;
-    }
-    updateTargetSphere();
+    endEffectorSel.innerHTML = ""; configData.end_effector_choices.forEach(b=>{ const opt=document.createElement('option'); opt.value=b; opt.textContent=b; if (b===configData.default_end_effector) opt.selected=true; endEffectorSel.appendChild(opt); });
+    bonesContainer.innerHTML = ""; configData.selectable_bones.forEach(b=>{ const id=`bone_${b}`; const label=document.createElement('label'); label.innerHTML = `<input type="checkbox" id="${id}" ${configData.default_controlled_bones.includes(b)?'checked':''}> ${b}`; bonesContainer.appendChild(label); setTimeout(()=>{ const cb=document.getElementById(id); if (cb) cb.addEventListener('change', ()=> scheduleSolve()); },0); });
+    if (currentModel === 'agent'){ handFieldset.style.display=''; handShapeSel.innerHTML=""; configData.hand_shapes.forEach(s=>{ const o=document.createElement('option'); o.value=s; o.textContent=s; handShapeSel.appendChild(o); }); handPosSel.innerHTML=""; configData.hand_positions.forEach(s=>{ const o=document.createElement('option'); o.value=s; o.textContent=s; handPosSel.appendChild(o); }); } else { handFieldset.style.display='none'; }
+    if (configData && typeof configData.max_subpoints !== 'undefined') subpointsInput.max = configData.max_subpoints;
+    if (configData && typeof configData.default_num_steps !== 'undefined') numStepsInput.value = configData.default_num_steps;
+    if (configData && configData.collision_default_center){ const c=configData.collision_default_center; collCx.value=c[0]; collCy.value=c[1]; collCz.value=c[2]; collCxSlider.value=c[0]; collCySlider.value=c[1]; collCzSlider.value=c[2]; }
+    if (configData && typeof configData.collision_default_radius !== 'undefined'){ collRadiusEl.value = configData.collision_default_radius; }
+    if (configData && typeof configData.collision_default_min_clearance !== 'undefined'){ collMinClearEl.value = configData.collision_default_min_clearance; collMinClearSlider.value = configData.collision_default_min_clearance; }
+    updateTargetSphere(); updateCollisionSphere();
   }
 
-  // Subpoints change listener (ensure derivative toggle & autosolve) - ADDED
-  subpointsInput.addEventListener('change', ()=>{
-    const v = parseInt(subpointsInput.value,10);
-    if (v <= 1){
-      document.getElementById('derivative_enabled').checked = false;
-    }
-    scheduleSolve();
-  });
+  subpointsInput.addEventListener('change', ()=>{ const v=parseInt(subpointsInput.value,10); if (v<=1){ document.getElementById('derivative_enabled').checked=false; } scheduleSolve(); });
 
-  // Disable GLTF controls on Pepper tab (avoid SMPLX loading there)
-  function applyModelSpecificGLTFPolicy(){
-    const urlInput = document.getElementById('gltf_url');
-    const showGltfChk = document.getElementById('show_gltf');
-    if (currentModel === 'pepper'){
-      // Do NOT permanently clear stored URL; just hide GLTF reference
-      // (removed previous urlInput.value = "" to preserve texture reuse when switching back)
-      showGltfChk.disabled = true;
-      if (gltfRoot) { gltfRoot.visible = false; }
-    } else {
-      // Restore default GLTF path if empty so textures load properly after tab switching
-      if (!urlInput.value) urlInput.value = "/files/smplx.glb";
-      showGltfChk.disabled = false;
-    }
-  }
+  function applyModelSpecificGLTFPolicy(){ const urlInput=document.getElementById('gltf_url'); const showGltfChk=document.getElementById('show_gltf'); if (currentModel==='pepper'){ showGltfChk.disabled=true; if (gltfRoot) gltfRoot.visible=false; } else { if (!urlInput.value) urlInput.value="/files/smplx.glb"; showGltfChk.disabled=false; } }
 
-  // ----- GLTF Loader (reuse its primary mesh for IK if vertex counts match) -----
   const gltfLoader = new GLTFLoader();
-  function loadGLTF(url){
-    if (!url){ log("No GLTF URL (possibly Pepper) - skipping load."); return; }
-    const u = url.startsWith('http') ? url : url;
-    statusBar.textContent = `Loading GLTF: ${u}`;
-    log(`Loading GLTF ${u}`);
-    gltfLoader.load(u, gltf=>{
-      if (gltfRoot) modelGroup.remove(gltfRoot);
-      gltfRoot = gltf.scene;
-      gltfPrimaryMaterial = null;
-      gltfPrimaryMesh = null;
-      gltfPrimaryMaterialCaptured = false;
-
-      gltfRoot.traverse(o=>{
-        if (o.isMesh && o.geometry?.attributes?.position){
-            if (!gltfPrimaryMesh || o.geometry.attributes.position.count > gltfPrimaryMesh.geometry.attributes.position.count){
-              gltfPrimaryMesh = o;
-            }
-        }
-      });
-
-      if (gltfPrimaryMesh){
-        if (Array.isArray(gltfPrimaryMesh.material)){
-          const mm = gltfPrimaryMesh.material.find(m=>m.map) || gltfPrimaryMesh.material[0];
-          gltfPrimaryMaterial = mm.clone();
-        } else {
-          gltfPrimaryMaterial = gltfPrimaryMesh.material.clone();
-        }
-        gltfPrimaryMaterialCaptured = true;
-        log(`Captured GLTF primary mesh (verts=${gltfPrimaryMesh.geometry.attributes.position.count})`);
-      } else {
-        log("No primary mesh found in GLTF.");
-      }
-
-      modelGroup.add(gltfRoot); // (CHANGED) add to modelGroup instead of scene
-      gltfRoot.visible = document.getElementById('show_gltf').checked;
-
-      // Bind deformation if frames already exist
-      if (animationFrames.length){
-        ensureActiveMeshBound(animationFrames[0]);
-        applyFrame(animationFrames[Math.min(frameIndex, animationFrames.length-1)]);
-      }
-
-      fitCamera(gltfRoot);
-      // adjustModelHeight(gltfRoot); // (ADD) ensure height set even without frames yet
-      statusBar.textContent = "GLTF loaded.";
-    }, undefined, err=>{
-      statusBar.textContent = "GLTF error: " + err.message;
-      log("GLTF error: "+err.message);
-    });
-  }
-
-  // ----- Animation Frames Fetch -----
-  async function fetchFrames(force=false){
-    const now = performance.now();
-    if (!force && now - lastFrameFetch < pollInterval) return;
-    lastFrameFetch = now;
-    const url = `/animation?model=${currentModel==='pepper'?'pepper':'agent'}`;
-    try {
-      const res = await fetch(url);
-      if (!res.ok) throw new Error(res.status);
-      const data = await res.json();
-      if (force || data.length !== animationFrames.length){
-        animationFrames = data;
-        if (animationFrames.length){
-          ensureActiveMeshBound(animationFrames[0]);
-          controlFrames = [];
-          splineMode = false;
-          playbackEnabled = false;
-          const requestedSub = payload.subpoints;
-          if (animationFrames.length > 1 && requestedSub > 1){
-            setupSplinePlayback(animationFrames);
-            applyFrame(animationFrames[0]);
-          } else {
-            frameIndex = animationFrames.length - 1;
-            applyFrame(animationFrames[frameIndex]);
-          }
-          scheduleGroundAlign();
-          lastFrameFetch = performance.now();
-        }
-      }
-    } catch(e){
-      log("Animation fetch error: "+e.message);
-    }
-  }
+  function loadGLTF(url){ if (!url){ log('No GLTF URL (possibly Pepper) - skipping load.'); return; } const u=url.startsWith('http')?url:url; statusBar.textContent=`Loading GLTF: ${u}`; log(`Loading GLTF ${u}`); gltfLoader.load(u, gltf=>{ if (gltfRoot) modelGroup.remove(gltfRoot); gltfRoot=gltf.scene; gltfPrimaryMaterial=null; gltfPrimaryMesh=null; gltfPrimaryMaterialCaptured=false; gltfRoot.traverse(o=>{ if (o.isMesh && o.geometry?.attributes?.position){ if (!gltfPrimaryMesh || o.geometry.attributes.position.count > gltfPrimaryMesh.geometry.attributes.position.count){ gltfPrimaryMesh=o; } } }); if (gltfPrimaryMesh){ if (Array.isArray(gltfPrimaryMesh.material)){ const mm=gltfPrimaryMesh.material.find(m=>m.map) || gltfPrimaryMesh.material[0]; gltfPrimaryMaterial=mm.clone(); } else { gltfPrimaryMaterial=gltfPrimaryMesh.material.clone(); } gltfPrimaryMaterialCaptured=true; log(`Captured GLTF primary mesh (verts=${gltfPrimaryMesh.geometry.attributes.position.count})`); } else { log('No primary mesh found in GLTF.'); }
+      modelGroup.add(gltfRoot); gltfRoot.visible=document.getElementById('show_gltf').checked; if (animationFrames.length){ ensureActiveMeshBound(animationFrames[0]); applyFrame(animationFrames[Math.min(frameIndex, animationFrames.length-1)]); } fitCamera(gltfRoot); statusBar.textContent='GLTF loaded.'; }, undefined, err=>{ statusBar.textContent='GLTF error: '+err.message; log('GLTF error: '+err.message); }); }
 
-  // Decide how to bind deformation on first frames arrival
-  function prepareDeformationBinding(){
-    if (!animationFrames.length) return;
-    ensureActiveMeshBound(animationFrames[0]);
-    applyFrame(animationFrames[0]);
-  }
-
-  // Deprecate old createOrUpdateFallbackMesh usage by redirect (kept if referenced)
-  function createOrUpdateFallbackMesh(){
-    if (!animationFrames.length) return;
-    ensureActiveMeshBound(animationFrames[0]);
-  }
-
-  // ----- Playback control (NEW) -----
-  function updateFramePlayback(){
-    if (!animationFrames.length) return;
-    const idx = Math.min(Math.floor(frameIndex), animationFrames.length - 1); // clamp
-    applyFrame(animationFrames[idx]);
-  }
-
-  // ----- Solve -----
-  // REMOVE old binding to doSolve directly (replaced below)
-  // document.getElementById('solve_btn').onclick = () => doSolve();
-  document.getElementById('solve_btn').onclick = () => triggerSolve(true);
-
-  function collectBones(){
-    const bones = [];
-    if (!configData) return bones;
-    configData.selectable_bones.forEach(b=>{
-      const cb = document.getElementById(`bone_${b}`);
-      if (cb && cb.checked) bones.push(b);
-    });
-    return bones;
-  }
+  document.getElementById('solve_btn').onclick = ()=> triggerSolve(true);
+  function collectBones(){ const bones=[]; if (!configData) return bones; configData.selectable_bones.forEach(b=>{ const cb=document.getElementById(`bone_${b}`); if (cb && cb.checked) bones.push(b); }); return bones; }
 
   async function doSolve(abortSignal){
-    statusBar.textContent = "Solving...";
-    log("Solve request started");
-    const subpointsVal = parseInt(val('subpoints'),10);
-    const payload = {
+    statusBar.textContent='Solving...'; log('Solve request started');
+    const subpointsVal=parseInt(val('subpoints'),10);
+    const payload={
       model: currentModel,
       target: [num('target_x'), num('target_y'), num('target_z')],
       subpoints: subpointsVal,
       num_steps: parseInt(numStepsInput.value,10) || 100,
       distance_enabled: bool('distance_enabled'),
       distance_weight: num('distance_weight'),
-      collision_enabled: bool('collision_enabled'),
-      collision_weight: num('collision_weight'),
+      collision_enabled: collisionEnabledEl.checked,
+      collision_weight: parseFloat(collisionWeightEl.value)||1.0,
+      collision_center: [parseFloat(collCx.value)||0, parseFloat(collCy.value)||0, parseFloat(collCz.value)||0],
+      collision_radius: parseFloat(collRadiusEl.value)||0.1,
       bone_zero_enabled: bool('bone_zero_enabled'),
       bone_zero_weight: num('bone_zero_weight'),
       derivative_enabled: bool('derivative_enabled'),
       derivative_weight: num('derivative_weight'),
       controlled_bones: collectBones(),
       end_effector: endEffectorSel.value,
-      hand_shape: currentModel==='agent'? handShapeSel.value : "None",
-      hand_position: currentModel==='agent'? handPosSel.value : "None",
-      // Request only last frame unless we actually need a trajectory
-      frames_mode: subpointsVal > 1 ? 'auto' : 'last'
+      hand_shape: currentModel==='agent'? handShapeSel.value : 'None',
+      hand_position: currentModel==='agent'? handPosSel.value : 'None',
+      frames_mode: subpointsVal > 1 ? 'auto' : 'last',
+      collision_min_clearance: parseFloat(collMinClearEl.value)||0.0,
     };
-    try {
-      const t0 = performance.now();
-      // Single request (backend configure occurs inside /solve)
-      const res = await fetch("/solve", {
-        method:"POST",
-        headers:{'Content-Type':'application/json'},
-        body: JSON.stringify(payload),
-        signal: abortSignal
-      });
-      if (!res.ok) throw new Error(res.status+" "+res.statusText);
-      const json = await res.json();
-      if (json.status !== 'ok') throw new Error(json.message || 'Solve failed');
-
-      // Stale solve guard
-      const sid = json.result.solve_id || 0;
-      if (payload.model === 'agent'){
-        if (sid <= lastSolveIdAgent){ log(`Stale agent solve ignored (sid=${sid} <= ${lastSolveIdAgent})`); return; }
-        lastSolveIdAgent = sid;
-      } else {
-        if (sid <= lastSolveIdPepper){ log(`Stale pepper solve ignored (sid=${sid} <= ${lastSolveIdPepper})`); return; }
-        lastSolveIdPepper = sid;
-      }
-
-      const server = json.result.solve_time;
-      const total = (performance.now()-t0)/1000;
-      statusBar.textContent =
-        `Solved: server=${server.toFixed(2)}s total=${total.toFixed(2)}s it=${json.result.iterations} obj=${json.result.objective.toFixed(6)} frames=${json.result.frames}`;
-      log(`Solve completed (frames received=${json.result.frames})`);
-      if (json.result.frames_data && json.result.frames_data.length){
-        animationFrames = json.result.frames_data;
-        ensureActiveMeshBound(animationFrames[0]);
-        controlFrames = [];
-        splineMode = false;
-        playbackEnabled = false;
-        if (animationFrames.length > 1 && subpointsVal > 1){
-          setupSplinePlayback(animationFrames);
-          applyFrame(animationFrames[0]);
-        } else {
-          frameIndex = animationFrames.length - 1;
-          applyFrame(animationFrames[frameIndex]);
-        }
-        scheduleGroundAlign();
-      }
-      updateTargetSphere();
-      showToast(
-        `Solve OK • it=${json.result.iterations} • t=${json.result.solve_time.toFixed(2)}s • err=${json.result.objective.toExponential(2)}`,
-        'success',
-        4000
-      );
-    } catch(e){
-      if (e.name === 'AbortError'){ log('Solve aborted (superseded by new request)'); return; }
-      statusBar.textContent = "Solve error: " + e.message;
-      log("Solve error: "+e.message);
-      showToast(`Solve ERROR: ${e.message}`, 'error', 6000);
-    }
-  }
-
-  // ----- Toast System (ADDED) -----
-  const toastContainer = document.getElementById('toastContainer');
-  function showToast(message, type='success', ttl=4000){
-    const el = document.createElement('div');
-    el.className = `toast ${type}`;
-    const close = document.createElement('span');
-    close.className = 'toast-close';
-    close.textContent = '✕';
-    close.onclick = (e)=>{ e.stopPropagation(); remove(); };
-    const content = document.createElement('div');
-    content.textContent = message;
-    el.appendChild(close);
-    el.appendChild(content);
-    toastContainer.appendChild(el);
-    requestAnimationFrame(()=> el.classList.add('show'));
-    function remove(){
-      el.classList.remove('show');
-      setTimeout(()=> el.remove(), 180);
-    }
-    setTimeout(remove, ttl);
-  }
-
-  // ----- Target sliders (NEW) -----
-  const xNum = document.getElementById('target_x');
-  const yNum = document.getElementById('target_y');
-  const zNum = document.getElementById('target_z');
-  const xSlider = document.getElementById('target_x_slider');
-  const ySlider = document.getElementById('target_y_slider');
-
-  function syncSliderToNum(axis){
-    if (axis==='x') xSlider.value = xNum.value;
-    else if (axis==='y') ySlider.value = yNum.value;
-  }
-  function syncNumToSlider(axis){
-    if (axis==='x') xNum.value = xSlider.value;
-    else if (axis==='y') yNum.value = ySlider.value;
+    try { const t0=performance.now(); const res=await fetch('/solve',{method:'POST', headers:{'Content-Type':'application/json'}, body:JSON.stringify(payload), signal:abortSignal}); if (!res.ok) throw new Error(res.status+' '+res.statusText); const json=await res.json(); if (json.status!=='ok') throw new Error(json.message || 'Solve failed'); const sid=json.result.solve_id||0; if (payload.model==='agent'){ if (sid <= lastSolveIdAgent){ log(`Stale agent solve ignored (sid=${sid} <= ${lastSolveIdAgent})`); return; } lastSolveIdAgent=sid; } else { if (sid <= lastSolveIdPepper){ log(`Stale pepper solve ignored (sid=${sid} <= ${lastSolveIdPepper})`); return; } lastSolveIdPepper=sid; }
+      const server=json.result.solve_time; const total=(performance.now()-t0)/1000; statusBar.textContent=`Solved: server=${server.toFixed(2)}s total=${total.toFixed(2)}s it=${json.result.iterations} obj=${json.result.objective.toFixed(6)} frames=${json.result.frames}`; log(`Solve completed (frames received=${json.result.frames})`); if (json.result.frames_data && json.result.frames_data.length){ animationFrames=json.result.frames_data; ensureActiveMeshBound(animationFrames[0]); controlFrames=[]; splineMode=false; playbackEnabled=false; if (animationFrames.length>1 && subpointsVal>1){ setupSplinePlayback(animationFrames); applyFrame(animationFrames[0]); } else { frameIndex=animationFrames.length-1; applyFrame(animationFrames[frameIndex]); } scheduleGroundAlign(); }
+      updateTargetSphere(); updateCollisionSphere(); showToast(`Solve OK • it=${json.result.iterations} • t=${json.result.solve_time.toFixed(2)}s • err=${json.result.objective.toExponential(2)}`,'success',4000); }
+    catch(e){ if (e.name==='AbortError'){ log('Solve aborted (superseded)'); return; } statusBar.textContent='Solve error: '+e.message; log('Solve error: '+e.message); showToast(`Solve ERROR: ${e.message}`,'error',6000); }
   }
 
-  // Slider live update (no solve while dragging)
-  xSlider.addEventListener('input', ()=>{ syncNumToSlider('x'); updateTargetSphere(); });
-  ySlider.addEventListener('input', ()=>{ syncNumToSlider('y'); updateTargetSphere(); });
-
-  // Solve only after release (change event)
-  xSlider.addEventListener('change', ()=>{ syncNumToSlider('x'); updateTargetSphere(); scheduleSolve(); });
-  ySlider.addEventListener('change', ()=>{ syncNumToSlider('y'); updateTargetSphere(); scheduleSolve(); });
-
-  // Numeric fields: update slider & sphere; solve on change (debounced)
-  xNum.addEventListener('change', ()=>{ syncSliderToNum('x'); updateTargetSphere(); scheduleSolve(); });
-  yNum.addEventListener('change', ()=>{ syncSliderToNum('y'); updateTargetSphere(); scheduleSolve(); });
-  zNum.addEventListener('change', ()=>{ updateTargetSphere(); scheduleSolve(); });
-
-  // Existing listeners for target sphere input (remove duplicate if present)
-  // ...existing code...
-
-  // ----- Hook other auto-solve controls (NEW) -----
-  const autoSolveSelectors = [
+  const toastContainer=document.getElementById('toastContainer');
+  function showToast(message, type='success', ttl=4000){ const el=document.createElement('div'); el.className=`toast ${type}`; const close=document.createElement('span'); close.className='toast-close'; close.textContent='✕'; close.onclick=e=>{ e.stopPropagation(); remove(); }; const content=document.createElement('div'); content.textContent=message; el.appendChild(close); el.appendChild(content); toastContainer.appendChild(el); requestAnimationFrame(()=> el.classList.add('show')); function remove(){ el.classList.remove('show'); setTimeout(()=> el.remove(), 180); } setTimeout(remove, ttl); }
+
+  const xNum=document.getElementById('target_x'); const yNum=document.getElementById('target_y'); const zNum=document.getElementById('target_z'); const xSlider=document.getElementById('target_x_slider'); const ySlider=document.getElementById('target_y_slider');
+  function syncSliderToNum(axis){ if (axis==='x') xSlider.value=xNum.value; else if (axis==='y') ySlider.value=yNum.value; }
+  function syncNumToSlider(axis){ if (axis==='x') xNum.value=xSlider.value; else if (axis==='y') yNum.value=ySlider.value; }
+  xSlider.addEventListener('input', ()=>{ syncNumToSlider('x'); updateTargetSphere(); }); ySlider.addEventListener('input', ()=>{ syncNumToSlider('y'); updateTargetSphere(); });
+  xSlider.addEventListener('change', ()=>{ syncNumToSlider('x'); updateTargetSphere(); scheduleSolve(); }); ySlider.addEventListener('change', ()=>{ syncNumToSlider('y'); updateTargetSphere(); scheduleSolve(); });
+  xNum.addEventListener('change', ()=>{ syncSliderToNum('x'); updateTargetSphere(); scheduleSolve(); }); yNum.addEventListener('change', ()=>{ syncSliderToNum('y'); updateTargetSphere(); scheduleSolve(); }); zNum.addEventListener('change', ()=>{ updateTargetSphere(); scheduleSolve(); });
+
+  // Collision sliders
+  function syncCollSliderToNum(axis){ if (axis==='x') collCxSlider.value=collCx.value; else if (axis==='y') collCySlider.value=collCy.value; else if (axis==='z') collCzSlider.value=collCz.value; else if (axis==='mc') collMinClearSlider.value=collMinClearEl.value; }
+  function syncCollNumToSlider(axis){ if (axis==='x') collCx.value=collCxSlider.value; else if (axis==='y') collCy.value=collCySlider.value; else if (axis==='z') collCz.value=collCzSlider.value; else if (axis==='mc') collMinClearEl.value=collMinClearSlider.value; }
+  ['x','y','z'].forEach(a=>{ const slider = a==='x'?collCxSlider: a==='y'?collCySlider:collCzSlider; slider.addEventListener('input', ()=>{ syncCollNumToSlider(a); updateCollisionSphere(); }); slider.addEventListener('change', ()=>{ syncCollNumToSlider(a); updateCollisionSphere(); scheduleSolve(); }); });
+  [collCx, collCy, collCz].forEach((el,i)=> el.addEventListener('change', ()=>{ syncCollSliderToNum(i===0?'x':i===1?'y':'z'); updateCollisionSphere(); scheduleSolve(); }));
+  collRadiusEl.addEventListener('change', ()=>{ updateCollisionSphere(); scheduleSolve(); });
+  collisionEnabledEl.addEventListener('change', ()=>{ updateCollisionSphere(); scheduleSolve(); });
+  collMinClearSlider.addEventListener('input', ()=>{ syncCollNumToSlider('mc'); updateCollisionSphere(); });
+  collMinClearSlider.addEventListener('change', ()=>{ syncCollNumToSlider('mc'); updateCollisionSphere(); scheduleSolve(); });
+  collMinClearEl.addEventListener('change', ()=>{ syncCollSliderToNum('mc'); updateCollisionSphere(); scheduleSolve(); });
+
+  const autoSolveSelectors=[
     '#distance_enabled','#distance_weight',
-    '#collision_enabled','#collision_weight',
+    '#collision_enabled','#collision_weight', '#collision_radius', '#collision_cx', '#collision_cy', '#collision_cz', '#collision_min_clearance',
     '#bone_zero_enabled','#bone_zero_weight',
     '#derivative_enabled','#derivative_weight',
     '#hand_shape','#hand_position',
     '#end_effector','#wireframe','#show_gltf','#show_ikmesh',
-    '#play_fps','#num_steps' // ADDED num_steps
+    '#play_fps','#num_steps'
   ];
-  autoSolveSelectors.forEach(sel=>{
-    const el = document.querySelector(sel);
-    if (el){
-      const evt = (el.type === 'checkbox' || el.tagName === 'SELECT') ? 'change' : 'input';
-      el.addEventListener(evt, ()=> scheduleSolve());
-    }
-  });
-
-  // Wireframe / visibility still immediate visual update (keep old logic if any)
-  document.getElementById('wireframe').addEventListener('change', ()=>{
-    if (ikMesh && ikMesh.material){
-      ikMesh.material.wireframe = document.getElementById('wireframe').checked;
-      ikMesh.material.needsUpdate = true;
-    }
-    if (gltfPrimaryMesh && gltfPrimaryMesh.material){
-      if (Array.isArray(gltfPrimaryMesh.material)) gltfPrimaryMesh.material.forEach(m=>{m.wireframe=document.getElementById('wireframe').checked; m.needsUpdate=true;});
-      else { gltfPrimaryMesh.material.wireframe = document.getElementById('wireframe').checked; gltfPrimaryMesh.material.needsUpdate=true; }
-    }
-  });
+  autoSolveSelectors.forEach(sel=>{ const el=document.querySelector(sel); if (el){ const evt=(el.type==='checkbox'||el.tagName==='SELECT')?'change':'input'; el.addEventListener(evt, ()=> scheduleSolve()); }});
 
-  document.getElementById('show_gltf').addEventListener('change', ()=>{
-    if (gltfRoot) gltfRoot.visible = document.getElementById('show_gltf').checked;
-  });
-  document.getElementById('show_ikmesh').addEventListener('change', ()=>{
-    if (ikMesh) ikMesh.visible = document.getElementById('show_ikmesh').checked;
-  });
+  document.getElementById('wireframe').addEventListener('change', ()=>{ if (ikMesh && ikMesh.material){ ikMesh.material.wireframe=document.getElementById('wireframe').checked; ikMesh.material.needsUpdate=true; } if (gltfPrimaryMesh && gltfPrimaryMesh.material){ if (Array.isArray(gltfPrimaryMesh.material)) gltfPrimaryMesh.material.forEach(m=>{m.wireframe=document.getElementById('wireframe').checked; m.needsUpdate=true;}); else { gltfPrimaryMesh.material.wireframe=document.getElementById('wireframe').checked; gltfPrimaryMesh.material.needsUpdate=true; } } });
+  document.getElementById('show_gltf').addEventListener('change', ()=>{ if (gltfRoot) gltfRoot.visible=document.getElementById('show_gltf').checked; });
+  document.getElementById('show_ikmesh').addEventListener('change', ()=>{ if (ikMesh) ikMesh.visible=document.getElementById('show_ikmesh').checked; });
 
-  // Update bones listener attachment inside populateUIFromConfig (MODIFIED)
-  function populateUIFromConfig(){
-    // End effector
-    endEffectorSel.innerHTML = "";
-    configData.end_effector_choices.forEach(b=>{
-      const opt = document.createElement('option');
-      opt.value = b; opt.textContent = b;
-      if (b === configData.default_end_effector) opt.selected = true;
-      endEffectorSel.appendChild(opt);
-    });
-    // Bones
-    bonesContainer.innerHTML = "";
-    configData.selectable_bones.forEach(b=>{
-      const id = `bone_${b}`;
-      const label = document.createElement('label');
-      label.innerHTML = `<input type="checkbox" id="${id}" ${configData.default_controlled_bones.includes(b)?'checked':''}> ${b}`;
-      bonesContainer.appendChild(label);
-      // Add listener for auto-solve
-      setTimeout(()=>{ // ensure element in DOM
-        const cb = document.getElementById(id);
-        if (cb) cb.addEventListener('change', ()=> scheduleSolve());
-      },0);
-    });
-    // Hand controls (agent only)
-    if (currentModel === 'agent'){
-      handFieldset.style.display = '';
-      handShapeSel.innerHTML = "";
-      configData.hand_shapes.forEach(s=>{
-        const opt = document.createElement('option');
-        opt.value = s; opt.textContent = s;
-        handShapeSel.appendChild(opt);
-      });
-      handPosSel.innerHTML = "";
-      configData.hand_positions.forEach(s=>{
-        const opt = document.createElement('option');
-        opt.value = s;
-        opt.textContent = s;
-        handPosSel.appendChild(opt);
-      });
-    } else {
-      handFieldset.style.display = 'none';
-    }
-    // Set subpoints max if provided
-    if (configData && typeof configData.max_subpoints !== 'undefined'){
-      subpointsInput.max = configData.max_subpoints;
-    }
-    if (configData && typeof configData.default_num_steps !== 'undefined'){
-      numStepsInput.value = configData.default_num_steps;
-    }
-    updateTargetSphere();
-  }
+  function ensureActiveMeshBound(firstFrame){ if (!firstFrame) return; const verts=firstFrame.vertices; let bound=false; if (gltfPrimaryMesh && gltfPrimaryMesh.geometry?.attributes?.position && gltfPrimaryMesh.geometry.attributes.position.count === verts.length){ if (fallbackMesh){ modelGroup.remove(fallbackMesh); if (fallbackMesh.geometry) fallbackMesh.geometry.dispose(); if (fallbackMesh.material && !Array.isArray(fallbackMesh.material)) fallbackMesh.material.dispose(); fallbackMesh=null; } ikMesh=gltfPrimaryMesh; usingGLTFGeometry=true; if (gltfPrimaryMaterial) ikMesh.material=gltfPrimaryMaterial; bound=true; log('ensureActiveMeshBound: using GLTF primary mesh'); }
+    if (!bound){ if (!fallbackMesh || !fallbackMesh.geometry?.getAttribute('position') || fallbackMesh.geometry.getAttribute('position').count !== verts.length){ if (fallbackMesh){ modelGroup.remove(fallbackMesh); if (fallbackMesh.geometry) fallbackMesh.geometry.dispose(); if (fallbackMesh.material && !Array.isArray(fallbackMesh.material)) fallbackMesh.material.dispose(); }
+        const geom=new THREE.BufferGeometry(); const posArr=new Float32Array(verts.length*3); for (let i=0;i<verts.length;i++){ const v=verts[i]; posArr[i*3]=v[0]; posArr[i*3+1]=v[1]; posArr[i*3+2]=v[2]; } geom.setAttribute('position', new THREE.BufferAttribute(posArr,3)); geom.getAttribute('position').setUsage(THREE.DynamicDrawUsage); const faces=firstFrame.faces; const idx=new Uint32Array(faces.length*3); for (let i=0;i<faces.length;i++){ const f=faces[i]; idx[i*3]=f[0]; idx[i*3+1]=f[1]; idx[i*3+2]=f[2]; } geom.setIndex(new THREE.BufferAttribute(idx,1)); geom.computeVertexNormals(); const mat=gltfPrimaryMaterial?gltfPrimaryMaterial.clone(): new THREE.MeshStandardMaterial({color:0x6699ff, metalness:0.2, roughness:0.6}); fallbackMesh=new THREE.Mesh(geom, mat); fallbackMesh.visible=document.getElementById('show_ikmesh').checked; modelGroup.add(fallbackMesh); ikMesh=fallbackMesh; usingGLTFGeometry=false; log('ensureActiveMeshBound: created fallback mesh'); } else { ikMesh=fallbackMesh; usingGLTFGeometry=false; log('ensureActiveMeshBound: reused fallback mesh'); } }
+    if (currentModel==='pepper' && ikMesh){ const box=new THREE.Box3().setFromObject(modelGroup); const h=box.max.y - box.min.y; const targetH=1.2; if (h>0 && (h<0.6 || h>2.0)){ const s=targetH / h; modelGroup.scale.set(s,s,s); log(`Pepper scaled: origH=${h.toFixed(3)} scale=${s.toFixed(3)}`); scheduleGroundAlign(true); } }
+    if (gltfRoot) gltfRoot.visible=document.getElementById('show_gltf').checked; if (ikMesh) ikMesh.visible=document.getElementById('show_ikmesh').checked; updateTargetSphere(); updateCollisionSphere(); }
 
-  // Solve button untouched; user can still manually force
-  // document.getElementById('solve_btn').onclick = ()=> triggerSolve();
+  function applyFrame(frame){ if (!ikMesh || !frame) return; const posAttr=ikMesh.geometry.getAttribute('position'); if (!posAttr || posAttr.count !== frame.vertices.length){ log('applyFrame: vertex count mismatch'); return; } const arr=posAttr.array; const verts=frame.vertices; for (let i=0;i<verts.length;i++){ const v=verts[i]; arr[i*3]=v[0]; arr[i*3+1]=v[1]; arr[i*3+2]=v[2]; } posAttr.needsUpdate=true; if (!groundAligned) scheduleGroundAlign(); }
+  function bsplineBasis(t){ const t2=t*t, t3=t2*t; return [(1 - 3*t + 3*t2 - t3)/6,(4 - 6*t2 + 3*t3)/6,(1 + 3*t + 3*t2 - 3*t3)/6,t3/6]; }
+  function getControlFrame(i){ if (i<0) return controlFrames[0]; if (i>=controlFrames.length) return controlFrames[controlFrames.length-1]; return controlFrames[i]; }
+  function evalSplineVertices(u){ if (!ikMesh || controlFrames.length<2) return; const n=controlFrames.length; const seg=Math.min(Math.floor(u), n-2); const t=u - seg; const [w0,w1,w2,w3]=bsplineBasis(t); const f0=getControlFrame(seg-1); const f1=getControlFrame(seg); const f2=getControlFrame(seg+1); const f3=getControlFrame(seg+2); const posAttr=ikMesh.geometry.getAttribute('position'); if (!posAttr) return; const arr=posAttr.array; const v0=f0.vertices, v1=f1.vertices, v2=f2.vertices, v3=f3.vertices; const count=posAttr.count; for (let i=0;i<count;i++){ const a0=v0[i], a1=v1[i], a2=v2[i], a3=v3[i]; arr[i*3]=w0*a0[0]+w1*a1[0]+w2*a2[0]+w3*a3[0]; arr[i*3+1]=w0*a0[1]+w1*a1[1]+w2*a2[1]+w3*a3[1]; arr[i*3+2]=w0*a0[2]+w1*a1[2]+w2*a2[2]+w3*a3[2]; } posAttr.needsUpdate=true; if (!groundAligned) scheduleGroundAlign(); }
+  function setupSplinePlayback(frames){ controlFrames=frames; splineMode=true; splineU=0; const segments=frames.length - 1; splineDuration=Math.min(segments * 0.6, 8.0); playbackEnabled=true; }
+  function updateFramePlayback(){ if (!animationFrames.length) return; const idx=Math.min(Math.floor(frameIndex), animationFrames.length - 1); applyFrame(animationFrames[idx]); }
 
-  // During tab switch, avoid duplicate immediate solve until config/frames loaded
-  // (MODIFY tab switch handler to schedule solve after frames)
-  document.querySelectorAll('.tab-btn').forEach(btn=>{
-    btn.onclick = () => {
-      if (btn.classList.contains('active')) return;
-      document.querySelectorAll('.tab-btn').forEach(b=>b.classList.remove('active'));
-      btn.classList.add('active');
-      currentModel = btn.getAttribute('data-model');
-      modelBadge.textContent = currentModel === 'pepper' ? "Pepper Robot" : "Agent";
-      cleanupVisuals();
-      // Ensure camera will refit once new geometry (fallback mesh or GLTF) is created
-      initialCameraFramed = false; // ADDED
-      applyModelSpecificGLTFPolicy();
-      loadConfig().then(()=>{
-        if (currentModel === 'agent') {
-          const url = document.getElementById('gltf_url').value;
-          if (url) loadGLTF(url);
-        }
-        scheduleSolve(true); // immediate first solve on tab switch
-        updateTargetSphere();
-      });
-    };
-  });
+  document.querySelectorAll('.tab-btn').forEach(btn=>{ btn.onclick=()=>{ if (btn.classList.contains('active')) return; document.querySelectorAll('.tab-btn').forEach(b=>b.classList.remove('active')); btn.classList.add('active'); currentModel=btn.getAttribute('data-model'); modelBadge.textContent=currentModel==='pepper'? 'Pepper Robot':'Agent'; cleanupVisuals(); initialCameraFramed=false; applyModelSpecificGLTFPolicy(); loadConfig().then(()=>{ if (currentModel==='agent'){ const url=document.getElementById('gltf_url').value; if (url) loadGLTF(url); } scheduleSolve(true); updateTargetSphere(); updateCollisionSphere(); }); }; });
 
-  // ===== Unified Mesh Binding (ADDED) =====
-  function ensureActiveMeshBound(firstFrame){
-    if (!firstFrame) return;
-    const verts = firstFrame.vertices;
-    let bound = false;
-
-    // Try GLTF primary mesh first
-    if (gltfPrimaryMesh &&
-        gltfPrimaryMesh.geometry?.attributes?.position &&
-        gltfPrimaryMesh.geometry.attributes.position.count === verts.length){
-      // Remove fallback if exists
-      if (fallbackMesh){
-        modelGroup.remove(fallbackMesh);
-        if (fallbackMesh.geometry) fallbackMesh.geometry.dispose();
-        if (fallbackMesh.material && !Array.isArray(fallbackMesh.material)) fallbackMesh.material.dispose();
-        fallbackMesh = null;
-      }
-      ikMesh = gltfPrimaryMesh;
-      usingGLTFGeometry = true;
-      if (gltfPrimaryMaterial) ikMesh.material = gltfPrimaryMaterial;
-      bound = true;
-      log("ensureActiveMeshBound: using GLTF primary mesh");
-    }
-
-    // Fallback creation/update
-    if (!bound){
-      if (!fallbackMesh ||
-          !fallbackMesh.geometry?.getAttribute('position') ||
-          fallbackMesh.geometry.getAttribute('position').count !== verts.length){
-        if (fallbackMesh){
-          modelGroup.remove(fallbackMesh);
-          if (fallbackMesh.geometry) fallbackMesh.geometry.dispose();
-          if (fallbackMesh.material && !Array.isArray(fallbackMesh.material)) fallbackMesh.material.dispose();
-        }
-        const geom = new THREE.BufferGeometry();
-        const posArr = new Float32Array(verts.length * 3);
-        for (let i=0;i<verts.length;i++){
-          const v = verts[i];
-          posArr[i*3]=v[0]; posArr[i*3+1]=v[1]; posArr[i*3+2]=v[2];
-        }
-        geom.setAttribute('position', new THREE.BufferAttribute(posArr,3));
-        geom.getAttribute('position').setUsage(THREE.DynamicDrawUsage);
-        const faces = firstFrame.faces;
-        const idx = new Uint32Array(faces.length*3);
-        for (let i=0;i<faces.length;i++){
-          const f = faces[i];
-          idx[i*3]=f[0]; idx[i*3+1]=f[1]; idx[i*3+2]=f[2];
-        }
-        geom.setIndex(new THREE.BufferAttribute(idx,1));
-        geom.computeVertexNormals();
-        const mat = gltfPrimaryMaterial ? gltfPrimaryMaterial.clone()
-                                        : new THREE.MeshStandardMaterial({color:0x6699ff, metalness:0.2, roughness:0.6});
-        fallbackMesh = new THREE.Mesh(geom, mat);
-        fallbackMesh.visible = document.getElementById('show_ikmesh').checked;
-        modelGroup.add(fallbackMesh);
-        ikMesh = fallbackMesh;
-        usingGLTFGeometry = false;
-        log("ensureActiveMeshBound: created fallback mesh");
-      } else {
-        ikMesh = fallbackMesh;
-        usingGLTFGeometry = false;
-        log("ensureActiveMeshBound: reused fallback mesh");
-      }
-    }
-
-    // Pepper scaling (visual only)
-    if (currentModel === 'pepper' && ikMesh){
-      const box = new THREE.Box3().setFromObject(modelGroup);
-      const h = box.max.y - box.min.y;
-      const targetH = 1.2;
-      if (h > 0 && (h < 0.6 || h > 2.0)){
-        const s = targetH / h;
-        modelGroup.scale.set(s,s,s);
-        log(`Pepper scaled: origH=${h.toFixed(3)} scale=${s.toFixed(3)}`);
-        scheduleGroundAlign(true);
-      }
-    }
-
-    if (gltfRoot) gltfRoot.visible = document.getElementById('show_gltf').checked;
-    if (ikMesh) ikMesh.visible = document.getElementById('show_ikmesh').checked;
-
-    updateTargetSphere();
-  }
-
-  // ===== Frame Application (ADDED) =====
-  function applyFrame(frame){
-    if (!ikMesh || !frame) return;
-    const posAttr = ikMesh.geometry.getAttribute('position');
-    if (!posAttr || posAttr.count !== frame.vertices.length){
-      log("applyFrame: vertex count mismatch");
-      return;
-    }
-    const arr = posAttr.array;
-    const verts = frame.vertices;
-    for (let i=0;i<verts.length;i++){
-      const v = verts[i];
-      arr[i*3]=v[0]; arr[i*3+1]=v[1]; arr[i*3+2]=v[2];
-    }
-    posAttr.needsUpdate = true;
-    if (!groundAligned) scheduleGroundAlign();
-  }
-
-  // ----- B-spline helpers (ADDED) ---
-  function bsplineBasis(t){
-    const t2=t*t, t3=t2*t;
-    return [
-      (1 - 3*t + 3*t2 - t3)/6,
-      (4 - 6*t2 + 3*t3)/6,
-      (1 + 3*t + 3*t2 - 3*t3)/6,
-      t3/6
-    ];
-  }
-  function getControlFrame(i){
-    if (i < 0) return controlFrames[0];
-    if (i >= controlFrames.length) return controlFrames[controlFrames.length-1];
-    return controlFrames[i];
-  }
-  function evalSplineVertices(u){
-    if (!ikMesh || controlFrames.length < 2) return;
-    const n = controlFrames.length;
-    const seg = Math.min(Math.floor(u), n-2);
-    const t = u - seg;
-    const [w0,w1,w2,w3] = bsplineBasis(t);
-    const f0 = getControlFrame(seg-1);
-    const f1 = getControlFrame(seg);
-    const f2 = getControlFrame(seg+1);
-    const f3 = getControlFrame(seg+2);
-    const posAttr = ikMesh.geometry.getAttribute('position');
-    if (!posAttr) return;
-    const arr = posAttr.array;
-    const v0=f0.vertices, v1=f1.vertices, v2=f2.vertices, v3=f3.vertices;
-    const count = posAttr.count;
-    for (let i=0;i<count;i++){
-      const a0=v0[i], a1=v1[i], a2=v2[i], a3=v3[i];
-      arr[i*3]   = w0*a0[0]+w1*a1[0]+w2*a2[0]+w3*a3[0];
-      arr[i*3+1] = w0*a0[1]+w1*a1[1]+w2*a2[1]+w3*a3[1];
-      arr[i*3+2] = w0*a0[2]+w1*a1[2]+w2*a2[2]+w3*a3[2];
-    }
-    posAttr.needsUpdate = true;
-    if (!groundAligned) scheduleGroundAlign();
-  }
-  function setupSplinePlayback(frames){
-    controlFrames = frames;
-    splineMode = true;
-    splineU = 0;
-    const segments = frames.length - 1;
-    splineDuration = Math.min(segments * 0.6, 8.0); // heuristic
-    playbackEnabled = true;
-  }
-
-  // ----- Render Loop -----
-  function animate(now){
-    requestAnimationFrame(animate);
-    const dt = (now - lastTime)/1000;
-    lastTime = now;
-    if (splineMode && playbackEnabled){
-      const n = controlFrames.length;
-      if (n > 1){
-        splineU += (dt / splineDuration) * (n - 1);
-        if (splineU >= (n - 1)){
-          splineU = n - 1;
-          playbackEnabled = false; // stop at end
-        }
-        evalSplineVertices(splineU);
-      }
-    } else if (animationFrames.length > 1 && playbackEnabled){
-      const fps = parseInt(document.getElementById('play_fps').value,10) || 24;
-      frameIndex += dt * fps;
-      if (frameIndex >= animationFrames.length){
-        if (allowLoopPlayback){
-          frameIndex = 0;
-        } else {
-          frameIndex = animationFrames.length - 1;
-          playbackEnabled = false; // stop at final frame
-        }
-      }
-      updateFramePlayback();
-    }
-    // If not animating but we just solved / loaded, ensure final frame rendered
-    if (!playbackEnabled && animationFrames.length && frameIndex === animationFrames.length - 1){
-      // Single application safeguard (light cost)
-      // applyFrame(animationFrames[frameIndex]); // already applied; can omit
-    }
-    if (pendingAlign) performGroundAlign();
-
-    // (ADDED) One-time initial camera framing after first geometry appears
-    if (!initialCameraFramed && modelGroup.children.length){
-      fitCamera(modelGroup);
-      initialCameraFramed = true;
-    }
-
-    renderer.render(scene, camera);
-  }
+  function animate(now){ requestAnimationFrame(animate); const dt=(now - lastTime)/1000; lastTime=now; if (splineMode && playbackEnabled){ const n=controlFrames.length; if (n>1){ splineU += (dt / splineDuration) * (n - 1); if (splineU >= (n - 1)){ splineU = n - 1; playbackEnabled=false; } evalSplineVertices(splineU); } } else if (animationFrames.length>1 && playbackEnabled){ const fps=parseInt(document.getElementById('play_fps').value,10) || 24; frameIndex += dt * fps; if (frameIndex >= animationFrames.length){ if (allowLoopPlayback){ frameIndex=0; } else { frameIndex=animationFrames.length - 1; playbackEnabled=false; } } updateFramePlayback(); } if (pendingAlign) performGroundAlign(); if (!initialCameraFramed && modelGroup.children.length){ fitCamera(modelGroup); initialCameraFramed=true; } renderer.render(scene, camera); }
   requestAnimationFrame(animate);
 
-  // ----- Init -----
-  applyModelSpecificGLTFPolicy();
-  loadConfig().then(()=>{
-    const url = document.getElementById('gltf_url').value;
-    if (url) loadGLTF(url);
-    scheduleGroundAlign(true);
-    scheduleSolve(true);
-    updateTargetSphere();
-  });
+  document.getElementById('reset_cam').addEventListener('click', ()=>{ fitCamera(modelGroup); });
 
+  applyModelSpecificGLTFPolicy();
+  loadConfig().then(()=>{ const url=document.getElementById('gltf_url').value; if (url) loadGLTF(url); scheduleGroundAlign(true); scheduleSolve(true); updateTargetSphere(); updateCollisionSphere(); });
 })();
 </script>
 </body>