Update mujoco_server.py and ur5_env.py for improved teleoperation and target positioning

- Adjusted the velocity range in the UI for cartesian translation, changing the default from 50 mm/s to 5 mm/s and modifying the input range for finer control.
- Implemented a repeat mechanism for teleoperation commands to enhance responsiveness during key presses.
- Updated the target positioning logic in the UR5 environment to consistently raise the target position by 0.3 meters, ensuring better alignment with the end effector's height.

mujoco_server.py

@@ -1560,10 +1560,10 @@ def index():
                                 <button class="jog-btn" onmousedown="startJog('cartesian_translation', 'z', '+')" onmouseup="stopJog()" ontouchstart="startJog('cartesian_translation', 'z', '+'); event.preventDefault()" ontouchend="stopJog()">+</button>
                             </div>
                         </div>
-                        <div style="margin-top: 8px;">
-                            <label style="font-size: 0.85em;">Velocity: <span id="trans_vel_val">50</span> mm/s</label>
-                            <input type="range" id="trans_velocity" min="10" max="200" step="10" value="50" oninput="updateTransVelocity()" style="width: 100%;">
-                        </div>
+                <div style="margin-top: 8px;">
+                    <label style="font-size: 0.85em;">Velocity: <span id="trans_vel_val">5</span> mm/s</label>
+                    <input type="range" id="trans_velocity" min="1" max="100" step="1" value="5" oninput="updateTransVelocity()" style="width: 100%;">
+                </div>
                     </div>
                     <div class="control-group">
                         <label>Rotation (RPY)</label>
@@ -1720,8 +1720,10 @@ def index():
                 'KeyW', 'KeyA', 'KeyS', 'KeyD', 'KeyQ', 'KeyE',
                 'ArrowUp', 'ArrowDown', 'ArrowLeft', 'ArrowRight'
             ]);
-            let teleopTranslationStep = 0.05; // meters per keyboard nudge
+            let teleopTranslationStep = 0.005; // meters per keyboard nudge
             let teleopVerticalStep = 0.01;
+            const TELEOP_REPEAT_INTERVAL_MS = 80;
+            let teleopRepeatTimer = null;
             let lastTeleopCommand = {dx: 0, dy: 0, dz: 0};
 
             function humanizeScene(scene) {
@@ -2317,7 +2319,31 @@ def index():
                 return armRobotTypes.has(active.robot) ? armTeleopKeys.has(code) : locomotionKeys.has(code);
             }
 
-            function updateArmTeleopFromKeys() {
+            function hasActiveTeleopKeys() {
+                for (const key of armTeleopKeys) {
+                    if (keysPressed.has(key)) {
+                        return true;
+                    }
+                }
+                return false;
+            }
+
+            function startTeleopRepeat() {
+                if (teleopRepeatTimer) {
+                    return;
+                }
+                teleopRepeatTimer = setInterval(() => updateArmTeleopFromKeys(true), TELEOP_REPEAT_INTERVAL_MS);
+            }
+
+            function stopTeleopRepeat() {
+                if (!teleopRepeatTimer) {
+                    return;
+                }
+                clearInterval(teleopRepeatTimer);
+                teleopRepeatTimer = null;
+            }
+
+            function updateArmTeleopFromKeys(force = false) {
                 let dx = 0, dy = 0, dz = 0;
                 if (keysPressed.has('KeyW')) dx += teleopTranslationStep;
                 if (keysPressed.has('KeyS')) dx -= teleopTranslationStep;
@@ -2331,7 +2357,7 @@ def index():
                     Math.abs(dy - lastTeleopCommand.dy) < 1e-6 &&
                     Math.abs(dz - lastTeleopCommand.dz) < 1e-6;
 
-                if (unchanged) {
+                if (unchanged && !force) {
                     return;
                 }
 
@@ -2344,8 +2370,15 @@ def index():
 
             function handleKeyStateChange() {
                 if (isArmRobot()) {
-                    updateArmTeleopFromKeys();
+                    const active = hasActiveTeleopKeys();
+                    updateArmTeleopFromKeys(active);
+                    if (active) {
+                        startTeleopRepeat();
+                    } else {
+                        stopTeleopRepeat();
+                    }
                 } else {
+                    stopTeleopRepeat();
                     updateCmdFromKeys();
                     if (teleopStatus) {
                         teleopStatus.innerText = 'UI teleop idle';

robots/ur5/ur5_env.py

@@ -159,6 +159,7 @@ class UR5Env(gym.Env):
         ee_pos = self.get_end_effector_pos()
         ee_quat = self.get_end_effector_quat()
         self._target_pos = ee_pos.astype(np.float32)
+        self._target_pos[2] += 0.3
         roll, pitch, yaw = IKController.quat_to_euler(ee_quat)
         self._target_euler = np.array([roll, pitch, yaw], dtype=np.float32)
 
@@ -570,10 +571,11 @@ class UR5Env(gym.Env):
         ee_pos = self.get_end_effector_pos()
         ee_quat = self.get_end_effector_quat()
         if self.scene_name == "scene_t_push" and self.t_target_body_id != -1:
-            target_pos = self.data.xpos[self.t_target_body_id]
+            target_pos = self.data.xpos[self.t_target_body_id].astype(np.float32)
         else:
-            target_pos = ee_pos
-        self._target_pos = target_pos.astype(np.float32)
+            target_pos = ee_pos.astype(np.float32)
+        target_pos[2] += 0.3
+        self._target_pos = target_pos
 
         # Convert EE quaternion to euler for target orientation
         roll, pitch, yaw = IKController.quat_to_euler(ee_quat)