multi-axes jogging

MUJOCO_LOG.TXT

@@ -0,0 +1,9 @@
+Thu Jan 29 17:06:20 2026
+WARNING: ARB_clip_control unavailable while mjDEPTH_ZEROFAR requested, depth accuracy will be limited
+
+Thu Jan 29 17:13:30 2026
+WARNING: ARB_clip_control unavailable while mjDEPTH_ZEROFAR requested, depth accuracy will be limited
+
+Fri Jan 30 16:28:31 2026
+WARNING: ARB_clip_control unavailable while mjDEPTH_ZEROFAR requested, depth accuracy will be limited
+

README.md

@@ -432,8 +432,26 @@ Nova-Sim provides a minimal HTTP API for static information:
   "action_space": {...},
   "observation_space": {...},
   "camera_feeds": [
-    {"name": "main", "label": "Main Camera", "url": "/nova-sim/api/v1/video_feed"},
-    {"name": "aux_top", "label": "Top View", "url": "/nova-sim/api/v1/camera/aux_top/video_feed"}
+    {
+      "name": "main",
+      "label": "Main Camera",
+      "url": "/nova-sim/api/v1/video_feed",
+      "pose": {
+        "position": {"x": 0.75, "y": -0.75, "z": 0.9},
+        "orientation": {"w": 0.92, "x": -0.16, "y": -0.36, "z": 0.04}
+      },
+      "intrinsics": {"fx": 869.1, "fy": 869.1, "cx": 640.0, "cy": 360.0, "width": 1280, "height": 720, "fovy_degrees": 45.0}
+    },
+    {
+      "name": "aux_top",
+      "label": "Top View",
+      "url": "/nova-sim/api/v1/camera/aux_top/video_feed",
+      "pose": {
+        "position": {"x": 0.5, "y": 0.2, "z": 0.9},
+        "orientation": {"w": 0.99, "x": -0.08, "y": 0.02, "z": 0.02}
+      },
+      "intrinsics": {"fx": 193.1, "fy": 193.1, "cx": 120.0, "cy": 80.0, "width": 240, "height": 160, "fovy_degrees": 45.0}
+    }
   ],
   "home_pose": [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0]
 }

frontend/index.html

@@ -885,38 +885,38 @@
                             <div class="jog-row">
                                 <label style="color: #ff6b6b; font-weight: bold;">X</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'x', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'x', '-')"
                                     ontouchstart="startJog('cartesian_translation', 'x', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_translation', 'x', '-')">−</button>
                                 <span class="val-display" id="pos_x_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'x', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'x', '+')"
                                     ontouchstart="startJog('cartesian_translation', 'x', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_translation', 'x', '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label style="color: #51cf66; font-weight: bold;">Y</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'y', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'y', '-')"
                                     ontouchstart="startJog('cartesian_translation', 'y', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_translation', 'y', '-')">−</button>
                                 <span class="val-display" id="pos_y_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'y', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'y', '+')"
                                     ontouchstart="startJog('cartesian_translation', 'y', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_translation', 'y', '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label style="color: #339af0; font-weight: bold;">Z</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'z', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'z', '-')"
                                     ontouchstart="startJog('cartesian_translation', 'z', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_translation', 'z', '-')">−</button>
                                 <span class="val-display" id="pos_z_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'z', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_translation', 'z', '+')"
                                     ontouchstart="startJog('cartesian_translation', 'z', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_translation', 'z', '+')">+</button>
                             </div>
                         </div>
                         <div style="margin-top: 8px;">
@@ -931,38 +931,38 @@
                             <div class="jog-row">
                                 <label style="color: #ff6b6b; font-weight: bold;">Rx</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'x', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'x', '-')"
                                     ontouchstart="startJog('cartesian_rotation', 'x', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'x', '-')">−</button>
                                 <span class="val-display" id="rot_x_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'x', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'x', '+')"
                                     ontouchstart="startJog('cartesian_rotation', 'x', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'x', '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label style="color: #51cf66; font-weight: bold;">Ry</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'y', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'y', '-')"
                                     ontouchstart="startJog('cartesian_rotation', 'y', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'y', '-')">−</button>
                                 <span class="val-display" id="rot_y_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'y', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'y', '+')"
                                     ontouchstart="startJog('cartesian_rotation', 'y', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'y', '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label style="color: #339af0; font-weight: bold;">Rz</label>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'z', '-')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'z', '-')"
                                     ontouchstart="startJog('cartesian_rotation', 'z', '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'z', '-')">−</button>
                                 <span class="val-display" id="rot_z_val">0.00</span>
                                 <button class="jog-btn" onmousedown="startJog('cartesian_rotation', 'z', '+')"
-                                    onmouseup="stopJog()"
+                                    onmouseup="stopJog('cartesian_rotation', 'z', '+')"
                                     ontouchstart="startJog('cartesian_rotation', 'z', '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('cartesian_rotation', 'z', '+')">+</button>
                             </div>
                         </div>
                         <div style="margin-top: 8px;">
@@ -980,63 +980,63 @@
                         <div class="jog-controls">
                             <div class="jog-row">
                                 <label>J1</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 1, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 1, '-')" onmouseup="stopJog('joint', 1, '-')"
                                     ontouchstart="startJog('joint', 1, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 1, '-')">−</button>
                                 <span class="val-display" id="joint_0_val">-1.57</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 1, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 1, '+')" onmouseup="stopJog('joint', 1, '+')"
                                     ontouchstart="startJog('joint', 1, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 1, '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label>J2</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 2, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 2, '-')" onmouseup="stopJog('joint', 2, '-')"
                                     ontouchstart="startJog('joint', 2, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 2, '-')">−</button>
                                 <span class="val-display" id="joint_1_val">-1.57</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 2, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 2, '+')" onmouseup="stopJog('joint', 2, '+')"
                                     ontouchstart="startJog('joint', 2, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 2, '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label>J3</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 3, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 3, '-')" onmouseup="stopJog('joint', 3, '-')"
                                     ontouchstart="startJog('joint', 3, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 3, '-')">−</button>
                                 <span class="val-display" id="joint_2_val">1.57</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 3, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 3, '+')" onmouseup="stopJog('joint', 3, '+')"
                                     ontouchstart="startJog('joint', 3, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 3, '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label>J4</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 4, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 4, '-')" onmouseup="stopJog('joint', 4, '-')"
                                     ontouchstart="startJog('joint', 4, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 4, '-')">−</button>
                                 <span class="val-display" id="joint_3_val">-1.57</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 4, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 4, '+')" onmouseup="stopJog('joint', 4, '+')"
                                     ontouchstart="startJog('joint', 4, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 4, '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label>J5</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 5, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 5, '-')" onmouseup="stopJog('joint', 5, '-')"
                                     ontouchstart="startJog('joint', 5, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 5, '-')">−</button>
                                 <span class="val-display" id="joint_4_val">-1.57</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 5, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 5, '+')" onmouseup="stopJog('joint', 5, '+')"
                                     ontouchstart="startJog('joint', 5, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 5, '+')">+</button>
                             </div>
                             <div class="jog-row">
                                 <label>J6</label>
-                                <button class="jog-btn" onmousedown="startJog('joint', 6, '-')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 6, '-')" onmouseup="stopJog('joint', 6, '-')"
                                     ontouchstart="startJog('joint', 6, '-'); event.preventDefault()"
-                                    ontouchend="stopJog()">−</button>
+                                    ontouchend="stopJog('joint', 6, '-')">−</button>
                                 <span class="val-display" id="joint_5_val">0.00</span>
-                                <button class="jog-btn" onmousedown="startJog('joint', 6, '+')" onmouseup="stopJog()"
+                                <button class="jog-btn" onmousedown="startJog('joint', 6, '+')" onmouseup="stopJog('joint', 6, '+')"
                                     ontouchstart="startJog('joint', 6, '+'); event.preventDefault()"
-                                    ontouchend="stopJog()">+</button>
+                                    ontouchend="stopJog('joint', 6, '+')">+</button>
                             </div>
                         </div>
                         <div style="margin-top: 8px;">
@@ -2187,29 +2187,72 @@
             document.getElementById('joint_vel_val').innerText = jointVelocity.toFixed(1);
         }
 
-        function startJog(jogType, axisOrJoint, direction) {
+        const activeJogButtons = new Map();
+
+        function _applyKeyboardJog(translation) {
+            if (keysPressed.has('KeyW')) translation.x += transVelocity / 1000.0;
+            if (keysPressed.has('KeyS')) translation.x -= transVelocity / 1000.0;
+            if (keysPressed.has('KeyA')) translation.y += transVelocity / 1000.0;
+            if (keysPressed.has('KeyD')) translation.y -= transVelocity / 1000.0;
+            if (keysPressed.has('KeyR')) translation.z += transVelocity / 1000.0;
+            if (keysPressed.has('KeyF')) translation.z -= transVelocity / 1000.0;
+        }
+
+        function _updateJogAction() {
             const actionData = {};
+            const translation = { x: 0.0, y: 0.0, z: 0.0 };
+            const rotation = { x: 0.0, y: 0.0, z: 0.0 };
+            const joint = { 1: 0.0, 2: 0.0, 3: 0.0, 4: 0.0, 5: 0.0, 6: 0.0 };
+
+            for (const entry of activeJogButtons.values()) {
+                const sign = entry.direction === '+' ? 1 : -1;
+                if (entry.jogType === 'cartesian_translation') {
+                    translation[entry.axisOrJoint] += sign * (transVelocity / 1000.0);
+                } else if (entry.jogType === 'cartesian_rotation') {
+                    rotation[entry.axisOrJoint] += sign * rotVelocity;
+                } else if (entry.jogType === 'joint') {
+                    const jointKey = Number(entry.axisOrJoint);
+                    if (jointKey >= 1 && jointKey <= 6) {
+                        joint[jointKey] += sign * jointVelocity;
+                    }
+                }
+            }
+
+            _applyKeyboardJog(translation);
 
-            if (jogType === 'cartesian_translation') {
-                const velocity = (direction === '+' ? 1 : -1) * (transVelocity / 1000.0);  // Convert mm/s to m/s
-                if (axisOrJoint === 'x') actionData.vx = velocity;
-                else if (axisOrJoint === 'y') actionData.vy = velocity;
-                else if (axisOrJoint === 'z') actionData.vz = velocity;
-            } else if (jogType === 'cartesian_rotation') {
-                const velocity = (direction === '+' ? 1 : -1) * rotVelocity;
-                if (axisOrJoint === 'x') actionData.vrx = velocity;
-                else if (axisOrJoint === 'y') actionData.vry = velocity;
-                else if (axisOrJoint === 'z') actionData.vrz = velocity;
-            } else if (jogType === 'joint') {
-                const velocity = (direction === '+' ? 1 : -1) * jointVelocity;
-                actionData['j' + axisOrJoint] = velocity;  // j1-j6
+            if (translation.x || translation.y || translation.z) {
+                if (translation.x) actionData.vx = translation.x;
+                if (translation.y) actionData.vy = translation.y;
+                if (translation.z) actionData.vz = translation.z;
+            }
+            if (rotation.x || rotation.y || rotation.z) {
+                if (rotation.x) actionData.vrx = rotation.x;
+                if (rotation.y) actionData.vry = rotation.y;
+                if (rotation.z) actionData.vrz = rotation.z;
+            }
+            for (let j = 1; j <= 6; j += 1) {
+                if (joint[j]) {
+                    actionData['j' + j] = joint[j];
+                }
             }
 
-            send('action', actionData);
+            if (Object.keys(actionData).length) {
+                send('action', actionData);
+            } else {
+                send('action', {});  // Send zero velocities
+            }
+        }
+
+        function startJog(jogType, axisOrJoint, direction) {
+            const key = `${jogType}:${axisOrJoint}:${direction}`;
+            activeJogButtons.set(key, { jogType, axisOrJoint, direction });
+            _updateJogAction();
         }
 
-        function stopJog() {
-            send('action', {});  // Send zero velocities
+        function stopJog(jogType, axisOrJoint, direction) {
+            const key = `${jogType}:${axisOrJoint}:${direction}`;
+            activeJogButtons.delete(key);
+            _updateJogAction();
         }
 
         function setGripper(action) {
@@ -2290,28 +2333,18 @@
             send('teleop_action', { dx, dy, dz });
         }
 
-        function startArmJogForKey(code) {
-            const entry = armJogKeyMap[code];
-            if (!entry) {
+        function handleArmKeyDown(code) {
+            if (!armJogKeyMap[code]) {
                 return;
             }
-            startJog(entry.jogType, entry.axis, entry.direction);
-
-        }
-
-        function handleArmKeyDown(code) {
-            startArmJogForKey(code);
+            _updateJogAction();
         }
 
         function handleArmKeyUp(code) {
             if (!armJogKeyMap[code]) {
                 return;
             }
-            stopJog();
-            const remaining = [...keysPressed].filter((key) => armJogKeyMap[key]);
-            if (remaining.length > 0) {
-                startArmJogForKey(remaining[remaining.length - 1]);
-            }
+            _updateJogAction();
         }
 
         function handleKeyStateChange() {
@@ -2463,4 +2496,4 @@
     </script>
 </body>
 
-</html>
+</html>

mujoco_server.py

@@ -6,6 +6,7 @@ import json
 import base64
 import math
 import traceback
+import io
 import cv2
 import numpy as np
 import mujoco
@@ -300,6 +301,104 @@ def _make_mjv_camera(config: dict[str, float]) -> mujoco.MjvCamera:
     return cam_obj
 
 
+def _normalize_vec(vec: np.ndarray) -> np.ndarray:
+    norm = np.linalg.norm(vec)
+    if norm < 1e-8:
+        return vec
+    return vec / norm
+
+
+def _rotation_matrix_to_quaternion(mat: np.ndarray) -> dict[str, float]:
+    """Convert a 3x3 rotation matrix to quaternion (w, x, y, z)."""
+    m00, m01, m02 = mat[0, 0], mat[0, 1], mat[0, 2]
+    m10, m11, m12 = mat[1, 0], mat[1, 1], mat[1, 2]
+    m20, m21, m22 = mat[2, 0], mat[2, 1], mat[2, 2]
+
+    trace = m00 + m11 + m22
+    if trace > 0.0:
+        s = math.sqrt(trace + 1.0) * 2.0
+        w = 0.25 * s
+        x = (m21 - m12) / s
+        y = (m02 - m20) / s
+        z = (m10 - m01) / s
+    elif m00 > m11 and m00 > m22:
+        s = math.sqrt(1.0 + m00 - m11 - m22) * 2.0
+        w = (m21 - m12) / s
+        x = 0.25 * s
+        y = (m01 + m10) / s
+        z = (m02 + m20) / s
+    elif m11 > m22:
+        s = math.sqrt(1.0 + m11 - m00 - m22) * 2.0
+        w = (m02 - m20) / s
+        x = (m01 + m10) / s
+        y = 0.25 * s
+        z = (m12 + m21) / s
+    else:
+        s = math.sqrt(1.0 + m22 - m00 - m11) * 2.0
+        w = (m10 - m01) / s
+        x = (m02 + m20) / s
+        y = (m12 + m21) / s
+        z = 0.25 * s
+
+    return {"w": float(w), "x": float(x), "y": float(y), "z": float(z)}
+
+
+def _camera_axes_from_orbit(cam_obj: mujoco.MjvCamera) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """Return (right, up, forward) axes for an orbit camera in world coordinates."""
+    azimuth = math.radians(float(cam_obj.azimuth))
+    elevation = math.radians(float(cam_obj.elevation))
+
+    right = np.array([math.sin(azimuth), -math.cos(azimuth), 0.0], dtype=np.float32)
+    up = np.array([
+        -math.cos(azimuth) * math.sin(elevation),
+        -math.sin(azimuth) * math.sin(elevation),
+        math.cos(elevation),
+    ], dtype=np.float32)
+    forward = np.array([
+        -math.cos(azimuth) * math.cos(elevation),
+        -math.sin(azimuth) * math.cos(elevation),
+        -math.sin(elevation),
+    ], dtype=np.float32)
+
+    return _normalize_vec(right), _normalize_vec(up), _normalize_vec(forward)
+
+
+def _camera_pose(cam_obj: mujoco.MjvCamera) -> dict[str, dict[str, float]]:
+    """Compute camera pose (position + orientation) in world coordinates."""
+    lookat = np.array(cam_obj.lookat, dtype=np.float32)
+    right, up, forward = _camera_axes_from_orbit(cam_obj)
+    position = lookat - forward * float(cam_obj.distance)
+    rotation = np.column_stack((right, up, forward))
+    quat = _rotation_matrix_to_quaternion(rotation)
+    return {
+        "position": {"x": float(position[0]), "y": float(position[1]), "z": float(position[2])},
+        "orientation": quat,
+    }
+
+
+def _camera_fovy_degrees(cam_obj: mujoco.MjvCamera, env_obj) -> float:
+    fovy = float(getattr(cam_obj, "fovy", 0.0) or 0.0)
+    if fovy <= 0.0 and env_obj is not None:
+        fovy = float(env_obj.model.vis.global_.fovy)
+    return float(fovy) if fovy > 0.0 else 45.0
+
+
+def _camera_intrinsics(width: int, height: int, fovy_degrees: float) -> dict[str, float]:
+    """Compute pinhole intrinsics from image size and vertical field-of-view."""
+    fovy_rad = math.radians(float(fovy_degrees))
+    fy = (float(height) / 2.0) / math.tan(fovy_rad / 2.0)
+    fx = fy
+    return {
+        "fx": float(fx),
+        "fy": float(fy),
+        "cx": float(width) / 2.0,
+        "cy": float(height) / 2.0,
+        "width": int(width),
+        "height": int(height),
+        "fovy_degrees": float(fovy_degrees),
+    }
+
+
 def _get_site_forward(env_obj, site_id: int) -> np.ndarray:
     """Compute the forward (X axis) vector for a site."""
     default = np.array([0.0, 0.0, 1.0], dtype=np.float32)
@@ -1255,47 +1354,27 @@ def handle_ws_message(ws, data):
                     if gripper is not None:
                         env.set_gripper(float(gripper))
 
-                    # Check which type of velocity is active (mutually exclusive)
-                    joint_velocities = [j1, j2, j3, j4, j5, j6]
-                    cartesian_translation = [vx, vy, vz]
-                    cartesian_rotation = [vrx, vry, vrz]
-
-                    # Find which velocity mode is active
-                    active_joint = None
-                    for i, vel in enumerate(joint_velocities):
-                        if abs(vel) > 0.001:  # Threshold to ignore noise
-                            active_joint = (i + 1, vel)
-                            break
-
-                    active_translation = None
-                    for i, (axis, vel) in enumerate(zip(['x', 'y', 'z'], cartesian_translation)):
-                        if abs(vel) > 0.001:
-                            active_translation = (axis, vel)
-                            break
-
-                    active_rotation = None
-                    for i, (axis, vel) in enumerate(zip(['x', 'y', 'z'], cartesian_rotation)):
-                        if abs(vel) > 0.001:
-                            active_rotation = (axis, vel)
-                            break
+                    # Check which type of velocity is active (joint vs cartesian)
+                    joint_velocities = [float(j1), float(j2), float(j3), float(j4), float(j5), float(j6)]
+                    cartesian_translation = [float(vx), float(vy), float(vz)]
+                    cartesian_rotation = [float(vrx), float(vry), float(vrz)]
+
+                    any_joint = any(abs(v) > 0.001 for v in joint_velocities)
+                    any_cartesian = any(abs(v) > 0.001 for v in cartesian_translation + cartesian_rotation)
 
                     # Apply the appropriate jogging mode
-                    if active_joint:
-                        joint, velocity = active_joint
-                        direction = '+' if velocity > 0 else '-'
-                        env.start_jog('joint', joint=joint, direction=direction, velocity=abs(velocity))
-                    elif active_translation:
-                        axis, velocity = active_translation
-                        direction = '+' if velocity > 0 else '-'
-                        # Convert m/s to mm/s
-                        velocity_mm_s = abs(velocity) * 1000.0
-                        env.start_jog('cartesian_translation', axis=axis, direction=direction,
-                                     velocity=velocity_mm_s, tcp_id='Flange', coord_system_id='world')
-                    elif active_rotation:
-                        axis, velocity = active_rotation
-                        direction = '+' if velocity > 0 else '-'
-                        env.start_jog('cartesian_rotation', axis=axis, direction=direction,
-                                     velocity=abs(velocity), tcp_id='Flange', coord_system_id='world')
+                    if any_joint:
+                        env.start_multi_joint_jog(joint_velocities)
+                    elif any_cartesian:
+                        # Convert m/s to mm/s for translations
+                        translation_mm_s = [v * 1000.0 for v in cartesian_translation]
+                        env.start_jog(
+                            'cartesian_velocity',
+                            translation=translation_mm_s,
+                            rotation=cartesian_rotation,
+                            tcp_id='Flange',
+                            coord_system_id='world'
+                        )
                     else:
                         # No active velocity - stop jogging
                         env.stop_jog()
@@ -1828,6 +1907,93 @@ def camera_feed(name):
                     mimetype='multipart/x-mixed-replace; boundary=frame')
 
 
+def capture_depth_snapshot(camera_name: str) -> Optional[bytes]:
+    """Capture a depth image from the specified camera and return as PNG bytes.
+
+    Args:
+        camera_name: Name of the camera ("main" or overlay camera name)
+
+    Returns:
+        PNG-encoded depth image as bytes, or None if capture fails
+    """
+    global env, cam, overlay_camera_states
+
+    if env is None:
+        return None
+
+    # Determine which camera to use
+    camera_obj = None
+    if camera_name == "main":
+        camera_obj = cam
+    elif camera_name in overlay_camera_states:
+        camera_obj = overlay_camera_states[camera_name].get("camera")
+
+    if camera_obj is None:
+        return None
+
+    # Create a temporary renderer for depth capture
+    with mujoco_lock:
+        try:
+            depth_renderer = mujoco.Renderer(
+                env.model,
+                height=env.height,
+                width=env.width
+            )
+
+            # Enable depth rendering
+            depth_renderer.enable_depth_rendering()
+
+            # Update scene and render depth
+            depth_renderer.update_scene(env.data, camera=camera_obj)
+            depth_array = depth_renderer.render()  # Returns (height, width) float32
+
+            # Close the temporary renderer
+            depth_renderer.close()
+
+            # Convert depth to 16-bit PNG for precision
+            # Normalize to 0-65535 range
+            # Handle invalid/infinite values
+            depth_min = np.min(depth_array[np.isfinite(depth_array)])
+            depth_max = np.max(depth_array[np.isfinite(depth_array)])
+
+            if depth_max > depth_min:
+                depth_normalized = np.clip(
+                    (depth_array - depth_min) / (depth_max - depth_min),
+                    0, 1
+                )
+            else:
+                depth_normalized = np.zeros_like(depth_array)
+
+            # Convert to 16-bit
+            depth_16bit = (depth_normalized * 65535).astype(np.uint16)
+
+            # Encode as PNG
+            ret, buffer = cv2.imencode('.png', depth_16bit)
+            if not ret:
+                return None
+
+            return buffer.tobytes()
+
+        except Exception as e:
+            print(f"Error capturing depth snapshot: {e}")
+            traceback.print_exc()
+            return None
+
+
+@app.route(f'{API_PREFIX}/camera/<name>/depth_snapshot')
+def camera_depth_snapshot(name):
+    """Endpoint to capture a single depth image as PNG."""
+    depth_png = capture_depth_snapshot(name)
+
+    if depth_png is None:
+        return jsonify({"error": "Failed to capture depth image"}), 500
+
+    response = make_response(depth_png)
+    response.headers['Content-Type'] = 'image/png'
+    response.headers['Content-Disposition'] = f'inline; filename="depth_{name}.png"'
+    return response
+
+
 @app.route(f'{API_PREFIX}/metadata')
 def metadata():
     robots_meta = {}
@@ -1891,21 +2057,39 @@ def get_env_info():
         scene_name = getattr(env, 'scene_name', None)
         control_mode = getattr(env, 'get_control_mode', lambda: None)()
 
-        # Build camera feed URLs
+        # Build camera feed URLs with pose + intrinsics
         camera_feeds = []
+        main_fovy = _camera_fovy_degrees(cam, env)
+        main_intrinsics = _camera_intrinsics(env.width, env.height, main_fovy)
         camera_feeds.append({
             "name": "main",
             "label": "Main Camera",
-            "url": f"{API_PREFIX}/video_feed"
+            "url": f"{API_PREFIX}/video_feed",
+            "pose": _camera_pose(cam),
+            "intrinsics": main_intrinsics,
         })
 
         # Check for overlay cameras based on scene
         configs = (scene_name and OVERLAY_CAMERA_PRESETS.get(scene_name)) or OVERLAY_CAMERA_PRESETS.get(current_robot) or []
         for feed in configs:
+            feed_name = feed.get('name', 'aux')
+            cam_state = overlay_camera_states.get(feed_name, {})
+            cam_obj = cam_state.get("camera")
+            feed_intrinsics = _camera_intrinsics(
+                OVERLAY_RENDER_WIDTH,
+                OVERLAY_RENDER_HEIGHT,
+                _camera_fovy_degrees(cam_obj or cam, env),
+            )
+            feed_payload = {
+                "name": feed_name,
+                "label": feed.get('label', feed_name),
+                "url": f"{API_PREFIX}/camera/{feed_name}/video_feed"
+            }
+            if cam_obj is not None:
+                feed_payload["pose"] = _camera_pose(cam_obj)
+            feed_payload["intrinsics"] = feed_intrinsics
             camera_feeds.append({
-                "name": feed.get('name', 'aux'),
-                "label": feed.get('label', feed.get('name', 'Aux')),
-                "url": f"{API_PREFIX}/camera/{feed.get('name', 'aux')}/video_feed"
+                **feed_payload
             })
 
         # Get home pose for current robot

protocol_types.py

@@ -400,11 +400,30 @@ class ClientNotificationBroadcast(TypedDict):
 # HTTP Response Types
 # ============================================================================
 
-class CameraFeed(TypedDict):
+class CameraIntrinsics(TypedDict):
+    """Camera intrinsics for pinhole camera model."""
+    fx: float
+    fy: float
+    cx: float
+    cy: float
+    width: int
+    height: int
+    fovy_degrees: float
+
+
+class CameraPose(TypedDict):
+    """Camera pose in world coordinates."""
+    position: Position
+    orientation: Quaternion
+
+
+class CameraFeed(TypedDict, total=False):
     """Camera feed information."""
     name: str
     label: str
     url: str
+    pose: CameraPose
+    intrinsics: CameraIntrinsics
 
 
 class EnvResponse(TypedDict, total=False):

robots/ur5/nova_jogger.py

@@ -322,6 +322,38 @@ class NovaJogger:
         if not self._ensure_websocket_for_mode("cartesian", tcp_id, coord_system_id):
             return False
 
+    def start_cartesian_velocity(self, translation_mm_per_sec: list[float] | tuple[float, float, float],
+                                 rotation_rads_per_sec: list[float] | tuple[float, float, float] = (0.0, 0.0, 0.0),
+                                 tcp_id: str = "Flange",
+                                 coord_system_id: str = "world") -> bool:
+        """Start jogging in cartesian space with full translation/rotation vectors."""
+        if not self._ensure_websocket_for_mode("cartesian", tcp_id, coord_system_id):
+            return False
+
+        try:
+            if len(translation_mm_per_sec) != 3 or len(rotation_rads_per_sec) != 3:
+                print("[Nova Jogger] Error: translation/rotation vectors must be length 3")
+                return False
+
+            command = {
+                "message_type": "TcpVelocityRequest",
+                "translation": [float(v) for v in translation_mm_per_sec],
+                "rotation": [float(v) for v in rotation_rads_per_sec],
+                "use_tool_coordinate_system": False
+            }
+            with self._lock:
+                self._current_command = command
+
+            print(
+                "[Nova Jogger] Cartesian velocity translation="
+                f"{command['translation']} mm/s rotation={command['rotation']} rad/s (continuous)"
+            )
+            return True
+
+        except Exception as e:
+            print(f"[Nova Jogger] Failed to start cartesian velocity: {e}")
+            return False
+
         try:
             # Create translation velocity array [x, y, z] in mm/s
             translation = [0.0, 0.0, 0.0]

robots/ur5/ur5_env.py

@@ -519,7 +519,7 @@ class UR5Env(gym.Env):
         """Start jogging command (Nova or internal IK fallback).
 
         Args:
-            jog_type: 'joint', 'cartesian_translation', or 'cartesian_rotation'
+            jog_type: 'joint', 'cartesian_translation', 'cartesian_rotation', or 'cartesian_velocity'
             **kwargs: Parameters for the jog command (joint, axis, direction, velocity, etc.)
 
         Returns:
@@ -569,6 +569,13 @@ class UR5Env(gym.Env):
                         tcp_id=kwargs.get('tcp_id', 'Flange'),
                         coord_system_id=kwargs.get('coord_system_id', 'world')
                     )
+                elif jog_type == 'cartesian_velocity':
+                    return self._nova_jogger.start_cartesian_velocity(
+                        translation_mm_per_sec=kwargs.get('translation', (0.0, 0.0, 0.0)),
+                        rotation_rads_per_sec=kwargs.get('rotation', (0.0, 0.0, 0.0)),
+                        tcp_id=kwargs.get('tcp_id', 'Flange'),
+                        coord_system_id=kwargs.get('coord_system_id', 'world')
+                    )
                 else:
                     print(f"[Nova] Unknown jog type: {jog_type}")
                     return False
@@ -578,7 +585,25 @@ class UR5Env(gym.Env):
 
         # Fallback to internal IK for cartesian movements
         print(f"[Internal] Using internal IK for jogging: {jog_type}")
-        if jog_type in ('cartesian_translation', 'cartesian_rotation'):
+        if jog_type in ('cartesian_translation', 'cartesian_rotation', 'cartesian_velocity'):
+            if jog_type == 'cartesian_velocity':
+                translation = kwargs.get('translation', (0.0, 0.0, 0.0))
+                rotation = kwargs.get('rotation', (0.0, 0.0, 0.0))
+                if len(translation) != 3 or len(rotation) != 3:
+                    print("[Internal] Invalid cartesian_velocity vectors")
+                    return False
+
+                # Translation is mm/s; convert to m/s and apply timestep.
+                translation_step = [(float(v) / 1000.0) * 0.02 for v in translation]
+                rotation_step = [float(v) * 0.02 for v in rotation]
+
+                self._active_jog = {
+                    'type': 'cartesian_velocity',
+                    'translation_step': translation_step,
+                    'rotation_step': rotation_step
+                }
+                return True
+
             axis = kwargs.get('axis', 'x')
             direction = kwargs.get('direction', '+')
 
@@ -913,6 +938,10 @@ class UR5Env(gym.Env):
                 # Update target position along specified axis
                 axis_idx = {'x': 0, 'y': 1, 'z': 2}[jog['axis']]
                 self._target_pos[axis_idx] += jog['step_size']
+            elif jog['type'] == 'cartesian_velocity':
+                # Update target position and orientation using vector steps
+                self._target_pos += np.array(jog['translation_step'], dtype=np.float32)
+                self._target_euler += np.array(jog['rotation_step'], dtype=np.float32)
             elif jog['type'] == 'cartesian_rotation':
                 # Update target orientation around specified axis
                 axis_idx = {'x': 0, 'y': 1, 'z': 2}[jog['axis']]
@@ -926,6 +955,16 @@ class UR5Env(gym.Env):
                     self.action_space.low[:6],
                     self.action_space.high[:6]
                 )
+            elif jog['type'] == 'multi_joint':
+                velocities = jog.get('velocities', [0.0] * 6)
+                if len(velocities) == 6:
+                    step = np.array(velocities, dtype=np.float32) * 0.02
+                    self._joint_targets += step
+                    self._joint_targets = np.clip(
+                        self._joint_targets,
+                        self.action_space.low[:6],
+                        self.action_space.high[:6]
+                    )
 
         if self._control_mode == 'ik':
             joint_targets = self._compute_joint_targets()