Enhance UR5e robot control with orientation tracking and UI updates

- Added support for end-effector orientation control in the UR5e robot, allowing for 6-DOF inverse kinematics.
- Implemented new WebSocket messages for setting target orientation and toggling orientation usage.
- Updated mujoco_server.py to broadcast end-effector orientation and target orientation in state messages.
- Enhanced the UI to include orientation sliders and display for end-effector orientation.
- Refined README.md to document new features, including orientation control and updated JSON message structures.

README.md

@@ -20,9 +20,10 @@ A unified MuJoCo-based robot simulation platform with web interface for multiple
 - 6 degrees of freedom (6-axis industrial arm)
 - Robotiq 2F-85 gripper
 - Two control modes:
-  - **IK Mode**: Set target XYZ position, inverse kinematics computes joint angles
+  - **IK Mode**: Set target XYZ position and orientation (Roll/Pitch/Yaw), inverse kinematics computes joint angles using damped least squares with 6-DOF Jacobian
   - **Joint Mode**: Direct control of individual joint positions
 - End-effector position and orientation tracking
+- Full 6-DOF IK with orientation control (can be toggled on/off)
 
 ## Features
 
@@ -159,7 +160,8 @@ docker run --gpus all -p 3004:3004 \
 - **E**: Strafe right
 
 ### Robot Arm (UR5)
-- **IK Mode**: Use XYZ sliders to set end-effector target position
+- **IK Mode**: Use XYZ sliders to set end-effector target position, RPY sliders for orientation
+- **Orientation Control**: Toggle checkbox to enable/disable 6-DOF orientation tracking
 - **Joint Mode**: Use J1-J6 sliders to control individual joints
 - **Gripper**: Open/Close buttons for Robotiq gripper
 - **Home**: Return to home position (joint mode)
@@ -399,12 +401,25 @@ All messages are JSON with `{type, data}` structure:
 
 #### UR5-Specific Messages
 
-**`arm_target`** (IK mode - set end-effector target):
+**`arm_target`** (IK mode - set end-effector target position):
 ```json
 {"type": "arm_target", "data": {"x": 0.4, "y": 0.0, "z": 0.6}}
 ```
 - `x`, `y`, `z`: Target position in meters
 
+**`arm_orientation`** (IK mode - set end-effector target orientation):
+```json
+{"type": "arm_orientation", "data": {"roll": 0.0, "pitch": 1.57, "yaw": 0.0}}
+```
+- `roll`, `pitch`, `yaw`: Target orientation in radians (XYZ Euler convention)
+- Default pointing down: `pitch = 1.57` (π/2)
+
+**`use_orientation`** (Toggle orientation control):
+```json
+{"type": "use_orientation", "data": {"enabled": true}}
+```
+- `enabled`: `true` for 6-DOF IK (position + orientation), `false` for 3-DOF IK (position only)
+
 **`joint_positions`** (Joint mode - direct joint control):
 ```json
 {"type": "joint_positions", "data": {"positions": [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0]}}
@@ -450,14 +465,20 @@ For robot arm (UR5):
   "data": {
     "robot": "ur5",
     "end_effector": {"x": 0.4, "y": 0.0, "z": 0.6},
+    "ee_orientation": {"w": 0.5, "x": 0.5, "y": 0.5, "z": 0.5},
     "target": {"x": 0.4, "y": 0.0, "z": 0.6},
+    "target_orientation": {"roll": 0.0, "pitch": 1.57, "yaw": 0.0},
     "gripper": 128,
     "joint_positions": [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0],
     "control_mode": "ik",
+    "use_orientation": true,
     "steps": 1234
   }
 }
 ```
+- `ee_orientation`: Current end-effector orientation as quaternion [w, x, y, z]
+- `target_orientation`: Target orientation as Euler angles (radians)
+- `use_orientation`: Whether 6-DOF orientation control is enabled
 
 ### HTTP Endpoints
 

mujoco_server.py

@@ -168,20 +168,26 @@ def broadcast_state():
         # UR5 has different state structure
         if current_robot == "ur5":
             ee_pos = env.get_end_effector_pos()
+            ee_quat = env.get_end_effector_quat()
             target = env.get_target()
+            target_euler = env.get_target_orientation()
             gripper = env.get_gripper()
             joint_pos = env.get_joint_positions()
             control_mode = env.get_control_mode()
+            use_orientation = env.get_use_orientation()
 
             state_msg = json.dumps({
                 'type': 'state',
                 'data': {
                     'robot': current_robot,
                     'end_effector': {'x': float(ee_pos[0]), 'y': float(ee_pos[1]), 'z': float(ee_pos[2])},
+                    'ee_orientation': {'w': float(ee_quat[0]), 'x': float(ee_quat[1]), 'y': float(ee_quat[2]), 'z': float(ee_quat[3])},
                     'target': {'x': float(target[0]), 'y': float(target[1]), 'z': float(target[2])},
+                    'target_orientation': {'roll': float(target_euler[0]), 'pitch': float(target_euler[1]), 'yaw': float(target_euler[2])},
                     'gripper': float(gripper),
                     'joint_positions': [float(j) for j in joint_pos],
                     'control_mode': control_mode,
+                    'use_orientation': use_orientation,
                     'steps': int(steps)
                 }
             })
@@ -379,10 +385,12 @@ def handle_ws_message(data):
     elif msg_type == 'gripper':
         payload = data.get('data', {})
         action = payload.get('action', 'open')
+        # Robotiq 2F-85: higher control value = more open (joint angle increases)
+        # Control range 0-255 maps to joint angle 0-0.8 rad
         if action == 'open':
-            value = 0  # 0 = open
+            value = 255  # 255 = fully open
         elif action == 'close':
-            value = 255  # 255 = closed
+            value = 0  # 0 = fully closed
         else:
             value = payload.get('value', 128)
         with mujoco_lock:
@@ -404,6 +412,22 @@ def handle_ws_message(data):
                 if env is not None and current_robot == "ur5":
                     env.set_joint_positions(positions)
 
+    elif msg_type == 'arm_orientation':
+        payload = data.get('data', {})
+        roll = payload.get('roll', 0.0)
+        pitch = payload.get('pitch', np.pi/2)
+        yaw = payload.get('yaw', 0.0)
+        with mujoco_lock:
+            if env is not None and current_robot == "ur5":
+                env.set_target_orientation(roll, pitch, yaw)
+
+    elif msg_type == 'use_orientation':
+        payload = data.get('data', {})
+        use = payload.get('enabled', True)
+        with mujoco_lock:
+            if env is not None and current_robot == "ur5":
+                env.set_use_orientation(use)
+
 
 @sock.route(f'{API_PREFIX}/ws')
 def websocket_handler(ws):
@@ -465,12 +489,27 @@ def index():
     <!DOCTYPE html>
     <html>
     <head>
-        <title>Nova Sim - Robot Simulator</title>
+        <title>Nova Sim - Wandelbots Robot Simulator</title>
         <style>
+            /* Wandelbots Corporate Design Colors:
+               Primary Dark: #01040f (Blue Charcoal)
+               Light/Secondary: #bcbeec (Spindle - lavender)
+               Accent: #211c44 (Port Gore - deep purple)
+               Logo Dark: #181838
+            */
+            :root {
+                --wb-primary: #01040f;
+                --wb-secondary: #bcbeec;
+                --wb-accent: #211c44;
+                --wb-logo: #181838;
+                --wb-highlight: #8b7fef;
+                --wb-success: #7c6bef;
+                --wb-danger: #ef6b6b;
+            }
             body, html {
                 margin: 0; padding: 0; width: 100%; height: 100%;
-                overflow: hidden; background: black;
-                font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif;
+                overflow: hidden; background: var(--wb-primary);
+                font-family: Arial, Helvetica, sans-serif;
                 user-select: none;
             }
             .video-container {
@@ -481,71 +520,118 @@ def index():
             .video-container img { width: 100%; height: 100%; object-fit: cover; }
             .overlay {
                 position: absolute; top: 20px; left: 20px;
-                background: rgba(255, 255, 255, 0.1);
+                background: rgba(33, 28, 68, 0.85);
                 backdrop-filter: blur(15px);
-                padding: 25px; border-radius: 15px;
-                box-shadow: 0 8px 32px rgba(0,0,0,0.5);
-                color: white; border: 1px solid rgba(255, 255, 255, 0.1);
+                padding: 25px; border-radius: 12px;
+                box-shadow: 0 8px 32px rgba(1, 4, 15, 0.6);
+                color: var(--wb-secondary); border: 1px solid rgba(188, 190, 236, 0.15);
                 z-index: 100; width: 320px;
             }
-            .overlay h2 { margin: 0 0 15px 0; font-size: 1.1em; font-weight: 300; letter-spacing: 1px; }
+            .overlay h2 { margin: 0 0 15px 0; font-size: 1.1em; font-weight: 600; letter-spacing: 0.5px; color: #fff; }
             .control-group { margin-bottom: 15px; }
-            .control-group label { display: block; margin-bottom: 5px; font-size: 0.8em; opacity: 0.7; }
+            .control-group label { display: block; margin-bottom: 5px; font-size: 0.8em; opacity: 0.8; color: var(--wb-secondary); }
             .slider-row { display: flex; align-items: center; gap: 12px; }
-            input[type=range] { flex-grow: 1; cursor: pointer; opacity: 0.8; }
-            .val-display { width: 60px; font-family: monospace; font-size: 0.9em; text-align: right; }
+            input[type=range] {
+                flex-grow: 1; cursor: pointer;
+                accent-color: var(--wb-highlight);
+            }
+            .val-display { width: 60px; font-family: monospace; font-size: 0.9em; text-align: right; color: #fff; }
             button {
                 flex: 1; padding: 10px;
-                background: rgba(255, 255, 255, 0.1);
-                color: white; border: 1px solid rgba(255, 255, 255, 0.2);
+                background: rgba(188, 190, 236, 0.1);
+                color: var(--wb-secondary); border: 1px solid rgba(188, 190, 236, 0.25);
                 border-radius: 6px; cursor: pointer; transition: all 0.2s;
                 font-size: 0.85em;
             }
-            button:hover { background: rgba(255, 255, 255, 0.2); }
-            button.danger { background: rgba(231, 76, 60, 0.5); border-color: rgba(231, 76, 60, 0.3); }
-            button.danger:hover { background: rgba(231, 76, 60, 0.7); }
-            .stats { margin-top: 15px; font-size: 0.8em; opacity: 0.8; line-height: 1.6; }
-            .hint { position: absolute; bottom: 20px; right: 20px; color: rgba(255,255,255,0.5); font-size: 0.8em; pointer-events: none; text-align: right; }
+            button:hover { background: rgba(188, 190, 236, 0.2); border-color: rgba(188, 190, 236, 0.4); }
+            button.danger { background: rgba(239, 107, 107, 0.4); border-color: rgba(239, 107, 107, 0.5); }
+            button.danger:hover { background: rgba(239, 107, 107, 0.6); }
+            .stats { margin-top: 15px; font-size: 0.8em; opacity: 0.9; line-height: 1.6; color: var(--wb-secondary); }
+            .hint { position: absolute; bottom: 20px; right: 20px; color: rgba(188, 190, 236, 0.5); font-size: 0.8em; pointer-events: none; text-align: right; }
+            /* State info panel - top right */
+            .state-panel {
+                position: absolute; top: 60px; right: 20px;
+                background: rgba(33, 28, 68, 0.85);
+                backdrop-filter: blur(15px);
+                padding: 15px 20px; border-radius: 10px;
+                box-shadow: 0 4px 20px rgba(1, 4, 15, 0.5);
+                color: var(--wb-secondary); border: 1px solid rgba(188, 190, 236, 0.15);
+                z-index: 100; min-width: 200px;
+                font-size: 0.8em; line-height: 1.5;
+            }
+            .state-panel strong { color: #fff; }
+            /* Camera controls - bottom left */
+            .camera-panel {
+                position: absolute; bottom: 20px; left: 20px;
+                background: rgba(33, 28, 68, 0.85);
+                backdrop-filter: blur(15px);
+                padding: 15px 20px; border-radius: 10px;
+                box-shadow: 0 4px 20px rgba(1, 4, 15, 0.5);
+                color: var(--wb-secondary); border: 1px solid rgba(188, 190, 236, 0.15);
+                z-index: 100; width: 280px;
+            }
+            .camera-panel .control-group { margin-bottom: 10px; }
+            .camera-panel .control-group:last-child { margin-bottom: 0; }
+            /* Collapsible panel header */
+            .panel-header {
+                display: flex; justify-content: space-between; align-items: center;
+                cursor: pointer; margin-bottom: 15px;
+            }
+            .panel-header h2 { margin: 0; }
+            .collapse-btn {
+                background: rgba(188, 190, 236, 0.2);
+                border: 1px solid rgba(188, 190, 236, 0.3);
+                color: var(--wb-secondary);
+                width: 28px; height: 28px;
+                border-radius: 6px; cursor: pointer;
+                display: flex; align-items: center; justify-content: center;
+                font-size: 1.1em; transition: all 0.2s;
+            }
+            .collapse-btn:hover { background: rgba(188, 190, 236, 0.3); }
+            .panel-content { transition: all 0.3s ease; overflow: hidden; }
+            .panel-content.collapsed { max-height: 0; opacity: 0; margin: 0; padding: 0; }
+            .overlay.collapsed { width: auto; min-width: 200px; }
             .rl-buttons { display: flex; flex-direction: column; gap: 5px; margin: 10px 0; }
             .rl-row { display: flex; justify-content: center; gap: 5px; }
             .rl-btn {
                 padding: 12px 16px; min-width: 80px;
-                background: rgba(46, 204, 113, 0.4); border: 1px solid rgba(46, 204, 113, 0.5);
-                color: white; border-radius: 6px; cursor: pointer; transition: all 0.15s;
+                background: rgba(124, 107, 239, 0.4); border: 1px solid rgba(124, 107, 239, 0.5);
+                color: #fff; border-radius: 6px; cursor: pointer; transition: all 0.15s;
                 font-size: 0.85em; font-weight: 500;
             }
-            .rl-btn:hover { background: rgba(46, 204, 113, 0.6); }
-            .rl-btn:active { background: rgba(46, 204, 113, 0.8); transform: scale(0.95); }
-            .rl-btn.stop { background: rgba(231, 76, 60, 0.4); border-color: rgba(231, 76, 60, 0.5); }
-            .rl-btn.stop:hover { background: rgba(231, 76, 60, 0.6); }
-            .cmd-display { font-family: monospace; font-size: 0.8em; opacity: 0.8; margin-top: 8px; }
+            .rl-btn:hover { background: rgba(124, 107, 239, 0.6); }
+            .rl-btn:active { background: rgba(124, 107, 239, 0.8); transform: scale(0.95); }
+            .rl-btn.stop { background: rgba(239, 107, 107, 0.4); border-color: rgba(239, 107, 107, 0.5); }
+            .rl-btn.stop:hover { background: rgba(239, 107, 107, 0.6); }
+            .cmd-display { font-family: monospace; font-size: 0.8em; opacity: 0.8; margin-top: 8px; color: var(--wb-secondary); }
             .connection-status {
                 position: absolute; top: 20px; right: 20px;
                 padding: 8px 15px; border-radius: 20px;
                 font-size: 0.75em; font-weight: 500;
-                background: rgba(46, 204, 113, 0.4);
-                color: white; border: 1px solid rgba(46, 204, 113, 0.5);
+                background: rgba(124, 107, 239, 0.5);
+                color: #fff; border: 1px solid rgba(124, 107, 239, 0.6);
             }
             .connection-status.disconnected {
-                background: rgba(231, 76, 60, 0.4);
-                border-color: rgba(231, 76, 60, 0.5);
+                background: rgba(239, 107, 107, 0.5);
+                border-color: rgba(239, 107, 107, 0.6);
             }
             .camera-controls { margin-top: 15px; }
             select {
                 width: 100%; padding: 10px;
-                background: rgba(255, 255, 255, 0.1);
-                color: white; border: 1px solid rgba(255, 255, 255, 0.2);
+                background: rgba(188, 190, 236, 0.1);
+                color: #fff; border: 1px solid rgba(188, 190, 236, 0.25);
                 border-radius: 6px; cursor: pointer;
                 font-size: 0.9em;
             }
-            select option { background: #333; color: white; }
+            select option { background: var(--wb-accent); color: #fff; }
             .robot-selector { margin-bottom: 20px; }
             .robot-info {
                 padding: 10px;
-                background: rgba(52, 152, 219, 0.3);
+                background: rgba(139, 127, 239, 0.2);
                 border-radius: 6px;
                 margin-top: 10px;
                 font-size: 0.8em;
+                border: 1px solid rgba(139, 127, 239, 0.3);
             }
             .arm-controls { display: none; }
             .arm-controls.active { display: block; }
@@ -559,12 +645,12 @@ def index():
             .mode-toggle { display: flex; gap: 5px; margin-bottom: 15px; }
             .mode-toggle button {
                 flex: 1; padding: 8px;
-                background: rgba(255, 255, 255, 0.1);
-                border: 1px solid rgba(255, 255, 255, 0.2);
+                background: rgba(188, 190, 236, 0.1);
+                border: 1px solid rgba(188, 190, 236, 0.2);
             }
             .mode-toggle button.active {
-                background: rgba(52, 152, 219, 0.5);
-                border-color: rgba(52, 152, 219, 0.7);
+                background: rgba(139, 127, 239, 0.5);
+                border-color: rgba(139, 127, 239, 0.7);
             }
             .ik-controls, .joint-controls { display: none; }
             .ik-controls.active, .joint-controls.active { display: block; }
@@ -579,52 +665,81 @@ def index():
 
         <div class="connection-status" id="conn_status">Connecting...</div>
 
-        <div class="overlay">
-            <h2 id="robot_title">Unitree G1 Humanoid</h2>
-
-            <div class="robot-selector">
-                <label>Select Robot</label>
-                <select id="robot_select" onchange="switchRobot()">
-                    <option value="g1">Unitree G1 (Humanoid)</option>
-                    <option value="spot">Boston Dynamics Spot (Quadruped)</option>
-                    <option value="ur5">Universal Robots UR5e (Arm)</option>
-                </select>
-                <div class="robot-info" id="robot_info">
-                    29 DOF humanoid with RL walking policy
+        <!-- State info panel - top right -->
+        <div class="state-panel" id="state_panel">
+            <div id="locomotion_state">
+                <strong>Robot State</strong><br>
+                Base Height: <span id="height_val">0.00</span> m<br>
+                Upright: <span id="upright_val">0.00</span><br>
+                Steps: <span id="step_val">0</span><br>
+                Cmd: <span id="cmd_vx">0.0</span>, <span id="cmd_vy">0.0</span>, <span id="cmd_yaw">0.0</span>
+            </div>
+            <div id="arm_state" style="display: none;">
+                <strong>Arm State</strong><br>
+                EE Pos: <span id="ee_pos">0.00, 0.00, 0.00</span><br>
+                EE Ori: <span id="ee_ori">0.00, 0.00, 0.00</span><br>
+                Gripper: <span id="gripper_val">50%</span><br>
+                Mode: <span id="control_mode_display">IK</span> | Steps: <span id="arm_step_val">0</span>
+            </div>
+        </div>
+
+        <!-- Camera controls - bottom left -->
+        <div class="camera-panel">
+            <div class="control-group">
+                <label>Camera Distance</label>
+                <div class="slider-row">
+                    <input type="range" id="cam_dist" min="1" max="10" step="0.1" value="3.0">
+                    <span class="val-display" id="cam_dist_val">3.0</span>
                 </div>
             </div>
+            <div class="control-group">
+                <label style="display: flex; align-items: center; gap: 8px; cursor: pointer;">
+                    <input type="checkbox" id="cam_follow" checked onchange="setCameraFollow()">
+                    Camera Follow Robot
+                </label>
+            </div>
+        </div>
 
-            <!-- Locomotion controls (G1, Spot) -->
-            <div class="locomotion-controls" id="locomotion_controls">
-                <div class="control-group">
-                    <label>Walking Controls (WASD or buttons)</label>
-                    <div class="rl-buttons">
-                        <div class="rl-row">
-                            <button class="rl-btn" onmousedown="setCmd(0.8, 0, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0.8, 0, 0)" ontouchend="setCmd(0,0,0)">W Forward</button>
-                        </div>
-                        <div class="rl-row">
-                            <button class="rl-btn" onmousedown="setCmd(0, 0, 1.2)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0, 1.2)" ontouchend="setCmd(0,0,0)">A Turn L</button>
-                            <button class="rl-btn" onmousedown="setCmd(-0.5, 0, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(-0.5, 0, 0)" ontouchend="setCmd(0,0,0)">S Back</button>
-                            <button class="rl-btn" onmousedown="setCmd(0, 0, -1.2)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0, -1.2)" ontouchend="setCmd(0,0,0)">D Turn R</button>
-                        </div>
-                        <div class="rl-row">
-                            <button class="rl-btn" onmousedown="setCmd(0, 0.4, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0.4, 0)" ontouchend="setCmd(0,0,0)">Q Strafe L</button>
-                            <button class="rl-btn stop" onclick="setCmd(0, 0, 0)">Stop</button>
-                            <button class="rl-btn" onmousedown="setCmd(0, -0.4, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, -0.4, 0)" ontouchend="setCmd(0,0,0)">E Strafe R</button>
-                        </div>
-                    </div>
-                    <div class="cmd-display">
-                        Cmd: vx=<span id="cmd_vx">0.0</span> vy=<span id="cmd_vy">0.0</span> yaw=<span id="cmd_yaw">0.0</span>
+        <div class="overlay" id="control_panel">
+            <div class="panel-header" onclick="togglePanel()">
+                <h2 id="robot_title">Unitree G1 Humanoid</h2>
+                <button class="collapse-btn" id="collapse_btn">-</button>
+            </div>
+
+            <div class="panel-content" id="panel_content">
+                <div class="robot-selector">
+                    <label>Select Robot</label>
+                    <select id="robot_select" onchange="switchRobot()">
+                        <option value="g1">Unitree G1 (Humanoid)</option>
+                        <option value="spot">Boston Dynamics Spot (Quadruped)</option>
+                        <option value="ur5">Universal Robots UR5e (Arm)</option>
+                    </select>
+                    <div class="robot-info" id="robot_info">
+                        29 DOF humanoid with RL walking policy
                     </div>
                 </div>
 
-                <div class="stats" id="locomotion_stats">
-                    <strong>Robot State:</strong><br>
-                    Base Height: <span id="height_val">0.00</span> m<br>
-                    Upright Score: <span id="upright_val">0.00</span><br>
-                    Step Count: <span id="step_val">0</span>
+                <!-- Locomotion controls (G1, Spot) -->
+                <div class="locomotion-controls" id="locomotion_controls">
+                    <div class="control-group">
+                        <label>Walking Controls (WASD or buttons)</label>
+                        <div class="rl-buttons">
+                            <div class="rl-row">
+                                <button class="rl-btn" onmousedown="setCmd(0.8, 0, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0.8, 0, 0)" ontouchend="setCmd(0,0,0)">W Forward</button>
+                            </div>
+                            <div class="rl-row">
+                                <button class="rl-btn" onmousedown="setCmd(0, 0, 1.2)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0, 1.2)" ontouchend="setCmd(0,0,0)">A Turn L</button>
+                                <button class="rl-btn" onmousedown="setCmd(-0.5, 0, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(-0.5, 0, 0)" ontouchend="setCmd(0,0,0)">S Back</button>
+                                <button class="rl-btn" onmousedown="setCmd(0, 0, -1.2)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0, -1.2)" ontouchend="setCmd(0,0,0)">D Turn R</button>
+                            </div>
+                            <div class="rl-row">
+                                <button class="rl-btn" onmousedown="setCmd(0, 0.4, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, 0.4, 0)" ontouchend="setCmd(0,0,0)">Q Strafe L</button>
+                                <button class="rl-btn stop" onclick="setCmd(0, 0, 0)">Stop</button>
+                                <button class="rl-btn" onmousedown="setCmd(0, -0.4, 0)" onmouseup="setCmd(0,0,0)" ontouchstart="setCmd(0, -0.4, 0)" ontouchend="setCmd(0,0,0)">E Strafe R</button>
+                            </div>
+                        </div>
+                    </div>
                 </div>
-            </div>
 
             <!-- Arm controls (UR5) -->
             <div class="arm-controls" id="arm_controls">
@@ -653,8 +768,32 @@ def index():
                             </div>
                             <div class="slider-row">
                                 <label>Z</label>
-                                <input type="range" id="target_z" min="0.3" max="1.0" step="0.01" value="0.6" oninput="updateTarget()">
-                                <span class="val-display" id="target_z_val">0.60</span>
+                                <input type="range" id="target_z" min="0.1" max="1.0" step="0.01" value="0.4" oninput="updateTarget()">
+                                <span class="val-display" id="target_z_val">0.40</span>
+                            </div>
+                        </div>
+                    </div>
+                    <div class="control-group">
+                        <label style="display: flex; align-items: center; gap: 8px;">
+                            Target Orientation (RPY)
+                            <input type="checkbox" id="use_orientation" checked onchange="setUseOrientation()">
+                            <span style="font-size: 0.75em; opacity: 0.7;">Enable</span>
+                        </label>
+                        <div class="target-sliders" id="orientation_sliders">
+                            <div class="slider-row">
+                                <label>R</label>
+                                <input type="range" id="target_roll" min="-3.14" max="3.14" step="0.05" value="0.0" oninput="updateOrientation()">
+                                <span class="val-display" id="target_roll_val">0.00</span>
+                            </div>
+                            <div class="slider-row">
+                                <label>P</label>
+                                <input type="range" id="target_pitch" min="-3.14" max="3.14" step="0.05" value="1.57" oninput="updateOrientation()">
+                                <span class="val-display" id="target_pitch_val">1.57</span>
+                            </div>
+                            <div class="slider-row">
+                                <label>Y</label>
+                                <input type="range" id="target_yaw" min="-3.14" max="3.14" step="0.05" value="0.0" oninput="updateOrientation()">
+                                <span class="val-display" id="target_yaw_val">0.00</span>
                             </div>
                         </div>
                     </div>
@@ -667,18 +806,18 @@ def index():
                         <div class="joint-sliders">
                             <div class="slider-row">
                                 <label>J1</label>
-                                <input type="range" id="joint_0" min="-6.28" max="6.28" step="0.01" value="-1.57" oninput="updateJoints()">
-                                <span class="val-display" id="joint_0_val">-1.57</span>
+                                <input type="range" id="joint_0" min="-6.28" max="6.28" step="0.01" value="0.0" oninput="updateJoints()">
+                                <span class="val-display" id="joint_0_val">0.00</span>
                             </div>
                             <div class="slider-row">
                                 <label>J2</label>
-                                <input type="range" id="joint_1" min="-6.28" max="6.28" step="0.01" value="-1.57" oninput="updateJoints()">
-                                <span class="val-display" id="joint_1_val">-1.57</span>
+                                <input type="range" id="joint_1" min="-6.28" max="6.28" step="0.01" value="-2.0" oninput="updateJoints()">
+                                <span class="val-display" id="joint_1_val">-2.00</span>
                             </div>
                             <div class="slider-row">
                                 <label>J3</label>
-                                <input type="range" id="joint_2" min="-3.14" max="3.14" step="0.01" value="1.57" oninput="updateJoints()">
-                                <span class="val-display" id="joint_2_val">1.57</span>
+                                <input type="range" id="joint_2" min="-3.14" max="3.14" step="0.01" value="2.0" oninput="updateJoints()">
+                                <span class="val-display" id="joint_2_val">2.00</span>
                             </div>
                             <div class="slider-row">
                                 <label>J4</label>
@@ -707,32 +846,9 @@ def index():
                         <button class="rl-btn" onclick="setGripper('close')">Close</button>
                     </div>
                 </div>
-
-                <div class="stats" id="arm_stats">
-                    <strong>Arm State:</strong><br>
-                    End Effector: <span id="ee_pos">0.00, 0.00, 0.00</span><br>
-                    Joints: <span id="joint_pos_display">0.00, 0.00, 0.00, 0.00, 0.00, 0.00</span><br>
-                    Mode: <span id="control_mode_display">IK</span> | Gripper: <span id="gripper_val">50%</span><br>
-                    Step Count: <span id="arm_step_val">0</span>
-                </div>
             </div>
 
-            <button class="danger" onclick="resetEnv()">Reset Environment</button>
-
-            <div class="camera-controls">
-                <div class="control-group">
-                    <label>Camera Distance</label>
-                    <div class="slider-row">
-                        <input type="range" id="cam_dist" min="1" max="10" step="0.1" value="3.0">
-                        <span class="val-display" id="cam_dist_val">3.0</span>
-                    </div>
-                </div>
-                <div class="control-group" style="margin-top: 10px;">
-                    <label style="display: flex; align-items: center; gap: 8px; cursor: pointer;">
-                        <input type="checkbox" id="cam_follow" checked onchange="setCameraFollow()">
-                        Camera Follow Robot
-                    </label>
-                </div>
+                <button class="danger" style="width: 100%; margin-top: 15px;" onclick="resetEnv()">Reset Environment</button>
             </div>
         </div>
 
@@ -808,8 +924,18 @@ def index():
                                 const ee = data.end_effector;
                                 document.getElementById('ee_pos').innerText =
                                     ee.x.toFixed(2) + ', ' + ee.y.toFixed(2) + ', ' + ee.z.toFixed(2);
+
+                                // EE Orientation - convert quaternion to euler for display
+                                if (data.ee_orientation) {
+                                    const q = data.ee_orientation;
+                                    const euler = quatToEuler(q.w, q.x, q.y, q.z);
+                                    document.getElementById('ee_ori').innerText =
+                                        euler[0].toFixed(2) + ', ' + euler[1].toFixed(2) + ', ' + euler[2].toFixed(2);
+                                }
+
+                                // Gripper: 0=closed, 255=open
                                 document.getElementById('gripper_val').innerText =
-                                    ((255 - data.gripper) / 255 * 100).toFixed(0) + '%';
+                                    (data.gripper / 255 * 100).toFixed(0) + '% open';
                                 document.getElementById('arm_step_val').innerText = data.steps;
 
                                 // Update joint position display
@@ -828,6 +954,15 @@ def index():
                                         setControlMode(data.control_mode);
                                     }
                                 }
+
+                                // Sync use_orientation checkbox
+                                if (data.use_orientation !== undefined) {
+                                    const checkbox = document.getElementById('use_orientation');
+                                    if (checkbox.checked !== data.use_orientation) {
+                                        checkbox.checked = data.use_orientation;
+                                        document.getElementById('orientation_sliders').style.opacity = data.use_orientation ? '1' : '0.4';
+                                    }
+                                }
                             } else {
                                 // Locomotion state
                                 heightVal.innerText = data.base_height.toFixed(2);
@@ -855,6 +990,30 @@ def index():
                 }
             }
 
+            // Convert quaternion to euler angles (XYZ convention)
+            function quatToEuler(w, x, y, z) {
+                // Roll (x-axis rotation)
+                const sinr_cosp = 2 * (w * x + y * z);
+                const cosr_cosp = 1 - 2 * (x * x + y * y);
+                const roll = Math.atan2(sinr_cosp, cosr_cosp);
+
+                // Pitch (y-axis rotation)
+                const sinp = 2 * (w * y - z * x);
+                let pitch;
+                if (Math.abs(sinp) >= 1) {
+                    pitch = Math.sign(sinp) * Math.PI / 2;
+                } else {
+                    pitch = Math.asin(sinp);
+                }
+
+                // Yaw (z-axis rotation)
+                const siny_cosp = 2 * (w * z + x * y);
+                const cosy_cosp = 1 - 2 * (y * y + z * z);
+                const yaw = Math.atan2(siny_cosp, cosy_cosp);
+
+                return [roll, pitch, yaw];
+            }
+
             function updateRobotUI(robot) {
                 robotTitle.innerText = robotTitles[robot] || robot;
                 robotInfo.innerText = robotInfoText[robot] || '';
@@ -863,9 +1022,30 @@ def index():
                 if (robot === 'ur5') {
                     locomotionControls.classList.add('hidden');
                     armControls.classList.add('active');
+                    document.getElementById('locomotion_state').style.display = 'none';
+                    document.getElementById('arm_state').style.display = 'block';
                 } else {
                     locomotionControls.classList.remove('hidden');
                     armControls.classList.remove('active');
+                    document.getElementById('locomotion_state').style.display = 'block';
+                    document.getElementById('arm_state').style.display = 'none';
+                }
+            }
+
+            let panelCollapsed = false;
+            function togglePanel() {
+                panelCollapsed = !panelCollapsed;
+                const content = document.getElementById('panel_content');
+                const btn = document.getElementById('collapse_btn');
+                const panel = document.getElementById('control_panel');
+                if (panelCollapsed) {
+                    content.classList.add('collapsed');
+                    panel.classList.add('collapsed');
+                    btn.innerText = '+';
+                } else {
+                    content.classList.remove('collapsed');
+                    panel.classList.remove('collapsed');
+                    btn.innerText = '-';
                 }
             }
 
@@ -924,6 +1104,22 @@ def index():
                 send('arm_target', {x, y, z});
             }
 
+            function updateOrientation() {
+                const roll = parseFloat(document.getElementById('target_roll').value);
+                const pitch = parseFloat(document.getElementById('target_pitch').value);
+                const yaw = parseFloat(document.getElementById('target_yaw').value);
+                document.getElementById('target_roll_val').innerText = roll.toFixed(2);
+                document.getElementById('target_pitch_val').innerText = pitch.toFixed(2);
+                document.getElementById('target_yaw_val').innerText = yaw.toFixed(2);
+                send('arm_orientation', {roll, pitch, yaw});
+            }
+
+            function setUseOrientation() {
+                const enabled = document.getElementById('use_orientation').checked;
+                document.getElementById('orientation_sliders').style.opacity = enabled ? '1' : '0.4';
+                send('use_orientation', {enabled});
+            }
+
             function updateJoints() {
                 const positions = [];
                 for (let i = 0; i < 6; i++) {
@@ -935,8 +1131,8 @@ def index():
             }
 
             function goToHome() {
-                // UR5 home position
-                const home = [-1.57, -1.57, 1.57, -1.57, -1.57, 0.0];
+                // UR5 home position - reaching forward with gripper down
+                const home = [0.0, -2.0, 2.0, -1.57, -1.57, 0.0];
                 for (let i = 0; i < 6; i++) {
                     document.getElementById('joint_' + i).value = home[i];
                     document.getElementById('joint_' + i + '_val').innerText = home[i].toFixed(2);

robots/g1/scene.xml

@@ -4,17 +4,25 @@
 
   <statistic center="0 0 0.5" extent="2.0"/>
 
+  <!-- Wandelbots Corporate Design Colors:
+       Primary Dark: #01040f (0.004, 0.016, 0.059)
+       Light/Secondary: #bcbeec (0.737, 0.745, 0.925)
+       Accent: #211c44 (0.129, 0.110, 0.267)
+       Highlight: #8b7fef (0.545, 0.498, 0.937)
+  -->
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
-    <rgba haze="0.15 0.25 0.35 1"/>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.35 0.35 0.4" specular="0 0 0"/>
+    <rgba haze="0.02 0.04 0.12 1"/>
     <global azimuth="-130" elevation="-20" offwidth="1280" offheight="720"/>
   </visual>
 
   <asset>
-    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8" width="300" height="300"/>
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+    <!-- Wandelbots gradient skybox - deep purple to near black -->
+    <texture type="skybox" builtin="gradient" rgb1="0.13 0.11 0.27" rgb2="0.004 0.016 0.059" width="512" height="3072"/>
+    <!-- Ground with Wandelbots purple accent -->
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.08 0.07 0.15" rgb2="0.04 0.04 0.08"
+      markrgb="0.55 0.5 0.94" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.15"/>
   </asset>
 
   <worldbody>

robots/spot/model/scene.xml

@@ -3,18 +3,26 @@
 
   <statistic center="0.15 0.1 0.38" extent=".8" meansize="0.05"/>
 
+  <!-- Wandelbots Corporate Design Colors:
+       Primary Dark: #01040f (0.004, 0.016, 0.059)
+       Light/Secondary: #bcbeec (0.737, 0.745, 0.925)
+       Accent: #211c44 (0.129, 0.110, 0.267)
+       Highlight: #8b7fef (0.545, 0.498, 0.937)
+  -->
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
-    <rgba haze="0.15 0.25 0.35 1"/>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.35 0.35 0.4" specular="0 0 0"/>
+    <rgba haze="0.02 0.04 0.12 1"/>
     <global azimuth="220" elevation="-10"/>
     <quality shadowsize="8192"/>
   </visual>
 
   <asset>
-    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8" width="300" height="300"/>
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+    <!-- Wandelbots gradient skybox - deep purple to near black -->
+    <texture type="skybox" builtin="gradient" rgb1="0.13 0.11 0.27" rgb2="0.004 0.016 0.059" width="512" height="3072"/>
+    <!-- Ground with Wandelbots purple accent -->
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.08 0.07 0.15" rgb2="0.04 0.04 0.08"
+      markrgb="0.55 0.5 0.94" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.15"/>
   </asset>
 
   <worldbody>

robots/ur5/controllers/ik_controller.py

@@ -73,8 +73,27 @@ class IKController:
         """Get current end-effector position."""
         return self.data.site_xpos[self.ee_site_id].copy()
 
-    def compute_jacobian(self):
-        """Compute the Jacobian for the end-effector site."""
+    def get_ee_orientation(self):
+        """Get current end-effector orientation as rotation matrix."""
+        return self.data.site_xmat[self.ee_site_id].reshape(3, 3).copy()
+
+    def get_ee_quat(self):
+        """Get current end-effector orientation as quaternion [w, x, y, z]."""
+        xmat = self.get_ee_orientation()
+        quat = np.zeros(4)
+        mujoco.mju_mat2Quat(quat, xmat.flatten())
+        return quat
+
+    def compute_jacobian(self, include_rotation=False):
+        """Compute the Jacobian for the end-effector site.
+
+        Args:
+            include_rotation: If True, return full 6x6 Jacobian (position + rotation)
+
+        Returns:
+            If include_rotation: 6xN Jacobian (position rows, then rotation rows)
+            Otherwise: 3xN position-only Jacobian
+        """
         # Zero out arrays
         self.jacp.fill(0)
         self.jacr.fill(0)
@@ -82,12 +101,19 @@ class IKController:
         # Compute Jacobian using MuJoCo
         mujoco.mj_jacSite(self.model, self.data, self.jacp, self.jacr, self.ee_site_id)
 
-        # Extract columns corresponding to controlled joints
-        jac = np.zeros((3, self.num_joints))
-        for i, dof_id in enumerate(self.dof_ids):
-            jac[:, i] = self.jacp[:, dof_id]
-
-        return jac
+        if include_rotation:
+            # Full 6-DOF Jacobian
+            jac = np.zeros((6, self.num_joints))
+            for i, dof_id in enumerate(self.dof_ids):
+                jac[:3, i] = self.jacp[:, dof_id]  # Position
+                jac[3:, i] = self.jacr[:, dof_id]  # Rotation
+            return jac
+        else:
+            # Position-only Jacobian
+            jac = np.zeros((3, self.num_joints))
+            for i, dof_id in enumerate(self.dof_ids):
+                jac[:, i] = self.jacp[:, dof_id]
+            return jac
 
     def compute_ik(self, target_pos, current_qpos=None):
         """
@@ -140,7 +166,46 @@ class IKController:
 
         return qpos
 
-    def compute_ik_with_orientation(self, target_pos, target_quat=None, current_qpos=None):
+    def quat_error(self, quat_current, quat_target):
+        """Compute orientation error as axis-angle representation.
+
+        Args:
+            quat_current: Current quaternion [w, x, y, z]
+            quat_target: Target quaternion [w, x, y, z]
+
+        Returns:
+            Orientation error as 3D vector (axis * angle)
+        """
+        # Compute quaternion difference: q_err = q_target * q_current^(-1)
+        # For unit quaternions, inverse is conjugate: [w, -x, -y, -z]
+        qc = quat_current
+        qt = quat_target
+
+        # Quaternion conjugate of current
+        qc_conj = np.array([qc[0], -qc[1], -qc[2], -qc[3]])
+
+        # Quaternion multiplication: q_err = qt * qc_conj
+        q_err = np.zeros(4)
+        q_err[0] = qt[0]*qc_conj[0] - qt[1]*qc_conj[1] - qt[2]*qc_conj[2] - qt[3]*qc_conj[3]
+        q_err[1] = qt[0]*qc_conj[1] + qt[1]*qc_conj[0] + qt[2]*qc_conj[3] - qt[3]*qc_conj[2]
+        q_err[2] = qt[0]*qc_conj[2] - qt[1]*qc_conj[3] + qt[2]*qc_conj[0] + qt[3]*qc_conj[1]
+        q_err[3] = qt[0]*qc_conj[3] + qt[1]*qc_conj[2] - qt[2]*qc_conj[1] + qt[3]*qc_conj[0]
+
+        # Ensure we take the shortest path (w >= 0)
+        if q_err[0] < 0:
+            q_err = -q_err
+
+        # Convert to axis-angle: angle = 2 * acos(w), axis = [x,y,z] / sin(angle/2)
+        # For small angles, error ≈ 2 * [x, y, z]
+        angle = 2.0 * np.arccos(np.clip(q_err[0], -1.0, 1.0))
+        if angle < 1e-6:
+            return np.zeros(3)
+
+        axis = q_err[1:4] / np.sin(angle / 2.0)
+        return axis * angle
+
+    def compute_ik_with_orientation(self, target_pos, target_quat=None, current_qpos=None,
+                                     position_weight=1.0, orientation_weight=0.5):
         """
         Compute joint positions to reach target pose (position + orientation).
 
@@ -148,13 +213,135 @@ class IKController:
             target_pos: Target position [x, y, z]
             target_quat: Target orientation quaternion [w, x, y, z] (optional)
             current_qpos: Starting joint positions (uses current if None)
+            position_weight: Weight for position error (default 1.0)
+            orientation_weight: Weight for orientation error (default 0.5)
 
         Returns:
             Joint positions to reach target
         """
-        # For now, just use position-only IK
-        # Full 6-DOF IK would need to include orientation in the error
-        return self.compute_ik(target_pos, current_qpos)
+        # If no orientation specified, fall back to position-only IK
+        if target_quat is None:
+            return self.compute_ik(target_pos, current_qpos)
+
+        if current_qpos is None:
+            qpos = self.get_current_joint_positions()
+        else:
+            qpos = current_qpos.copy()
+
+        target_p = np.array(target_pos)
+        target_q = np.array(target_quat)
+
+        # Normalize target quaternion
+        target_q = target_q / np.linalg.norm(target_q)
+
+        # Tolerance for orientation (radians)
+        orientation_tolerance = 0.01
+
+        for _ in range(self.max_iterations):
+            # Update forward kinematics
+            for i, jid in enumerate(self.joint_ids):
+                self.data.qpos[self.model.jnt_qposadr[jid]] = qpos[i]
+            mujoco.mj_forward(self.model, self.data)
+
+            # Get current end-effector pose
+            ee_pos = self.get_ee_position()
+            ee_quat = self.get_ee_quat()
+
+            # Compute position error
+            pos_error = target_p - ee_pos
+            pos_error_norm = np.linalg.norm(pos_error)
+
+            # Compute orientation error
+            ori_error = self.quat_error(ee_quat, target_q)
+            ori_error_norm = np.linalg.norm(ori_error)
+
+            # Check convergence
+            if pos_error_norm < self.tolerance and ori_error_norm < orientation_tolerance:
+                break
+
+            # Compute full 6-DOF Jacobian
+            jac = self.compute_jacobian(include_rotation=True)
+
+            # Construct weighted error vector [position; orientation]
+            error = np.concatenate([
+                position_weight * pos_error,
+                orientation_weight * ori_error
+            ])
+
+            # Apply weights to Jacobian rows
+            jac_weighted = jac.copy()
+            jac_weighted[:3, :] *= position_weight
+            jac_weighted[3:, :] *= orientation_weight
+
+            # Damped least squares: dq = J^T * (J * J^T + lambda^2 * I)^-1 * error
+            jjt = jac_weighted @ jac_weighted.T
+            damped = jjt + self.damping**2 * np.eye(6)
+            dq = jac_weighted.T @ np.linalg.solve(damped, error)
+
+            # Apply step
+            qpos = qpos + self.step_size * dq
+
+            # Clamp to joint limits
+            qpos = np.clip(qpos, self.joint_limits_low, self.joint_limits_high)
+
+        return qpos
+
+    @staticmethod
+    def euler_to_quat(roll, pitch, yaw):
+        """Convert Euler angles (XYZ convention) to quaternion [w, x, y, z].
+
+        Args:
+            roll: Rotation around X axis (radians)
+            pitch: Rotation around Y axis (radians)
+            yaw: Rotation around Z axis (radians)
+
+        Returns:
+            Quaternion [w, x, y, z]
+        """
+        cy = np.cos(yaw * 0.5)
+        sy = np.sin(yaw * 0.5)
+        cp = np.cos(pitch * 0.5)
+        sp = np.sin(pitch * 0.5)
+        cr = np.cos(roll * 0.5)
+        sr = np.sin(roll * 0.5)
+
+        w = cr * cp * cy + sr * sp * sy
+        x = sr * cp * cy - cr * sp * sy
+        y = cr * sp * cy + sr * cp * sy
+        z = cr * cp * sy - sr * sp * cy
+
+        return np.array([w, x, y, z])
+
+    @staticmethod
+    def quat_to_euler(quat):
+        """Convert quaternion [w, x, y, z] to Euler angles (XYZ convention).
+
+        Args:
+            quat: Quaternion [w, x, y, z]
+
+        Returns:
+            (roll, pitch, yaw) in radians
+        """
+        w, x, y, z = quat
+
+        # Roll (x-axis rotation)
+        sinr_cosp = 2 * (w * x + y * z)
+        cosr_cosp = 1 - 2 * (x * x + y * y)
+        roll = np.arctan2(sinr_cosp, cosr_cosp)
+
+        # Pitch (y-axis rotation)
+        sinp = 2 * (w * y - z * x)
+        if abs(sinp) >= 1:
+            pitch = np.copysign(np.pi / 2, sinp)  # Gimbal lock
+        else:
+            pitch = np.arcsin(sinp)
+
+        # Yaw (z-axis rotation)
+        siny_cosp = 2 * (w * z + x * y)
+        cosy_cosp = 1 - 2 * (y * y + z * z)
+        yaw = np.arctan2(siny_cosp, cosy_cosp)
+
+        return roll, pitch, yaw
 
     def reset(self):
         """Reset controller state."""

robots/ur5/model/scene.xml

@@ -3,25 +3,36 @@
 
   <option integrator="implicitfast" cone="elliptic" impratio="10"/>
 
+  <!-- Wandelbots Corporate Design Colors:
+       Primary Dark: #01040f (0.004, 0.016, 0.059)
+       Light/Secondary: #bcbeec (0.737, 0.745, 0.925)
+       Accent: #211c44 (0.129, 0.110, 0.267)
+       Highlight: #8b7fef (0.545, 0.498, 0.937)
+  -->
   <visual>
-    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
-    <rgba haze="0.15 0.25 0.35 1"/>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.35 0.35 0.4" specular="0 0 0"/>
+    <rgba haze="0.02 0.04 0.12 1"/>
     <global azimuth="120" elevation="-20"/>
   </visual>
 
   <asset>
-    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
-    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
-      markrgb="0.8 0.8 0.8" width="300" height="300"/>
-    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
-    <material name="table" rgba="0.6 0.5 0.4 1" specular="0.3" shininess="0.3"/>
-    <material name="target_mat" rgba="0.2 0.8 0.2 0.5" specular="0.5" shininess="0.5"/>
+    <!-- Wandelbots gradient skybox - deep purple to near black -->
+    <texture type="skybox" builtin="gradient" rgb1="0.13 0.11 0.27" rgb2="0.004 0.016 0.059" width="512" height="3072"/>
+    <!-- Ground with Wandelbots purple accent -->
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.08 0.07 0.15" rgb2="0.04 0.04 0.08"
+      markrgb="0.55 0.5 0.94" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.15"/>
+    <!-- Table with subtle purple tint -->
+    <material name="table" rgba="0.18 0.16 0.25 1" specular="0.4" shininess="0.4"/>
+    <!-- Target marker with Wandelbots highlight color -->
+    <material name="target_mat" rgba="0.55 0.5 0.94 0.6" specular="0.5" shininess="0.5"/>
 
-    <!-- UR5e materials -->
-    <material name="black" rgba="0.033 0.033 0.033 1" specular="0.5" shininess="0.25"/>
-    <material name="jointgray" rgba="0.278 0.278 0.278 1" specular="0.5" shininess="0.25"/>
-    <material name="linkgray" rgba="0.82 0.82 0.82 1" specular="0.5" shininess="0.25"/>
-    <material name="urblue" rgba="0.49 0.678 0.8 1" specular="0.5" shininess="0.25"/>
+    <!-- UR5e materials - with Wandelbots accent colors -->
+    <material name="black" rgba="0.02 0.02 0.04 1" specular="0.5" shininess="0.25"/>
+    <material name="jointgray" rgba="0.22 0.22 0.26 1" specular="0.5" shininess="0.25"/>
+    <material name="linkgray" rgba="0.74 0.75 0.82 1" specular="0.5" shininess="0.25"/>
+    <!-- Wandelbots purple accent instead of UR blue -->
+    <material name="urblue" rgba="0.55 0.5 0.94 1" specular="0.6" shininess="0.35"/>
 
     <!-- Gripper materials -->
     <material name="metal" rgba="0.58 0.58 0.58 1"/>
@@ -292,10 +303,10 @@
       </body>
     </body>
 
-    <!-- Optional: object to manipulate -->
+    <!-- Optional: object to manipulate - Wandelbots lavender accent -->
     <body name="box" pos="0.5 0.2 0.45">
       <freejoint name="box_joint"/>
-      <geom name="box_geom" type="box" size="0.025 0.025 0.025" rgba="1 0.3 0.3 1" mass="0.1"/>
+      <geom name="box_geom" type="box" size="0.025 0.025 0.025" rgba="0.74 0.75 0.93 1" mass="0.1"/>
     </body>
   </worldbody>
 
@@ -334,7 +345,7 @@
   </actuator>
 
   <keyframe>
-    <key name="home" qpos="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 0 0 0 0 0 0 0 0 0.5 0.2 0.45 1 0 0 0"
-         ctrl="-1.5708 -1.5708 1.5708 -1.5708 -1.5708 0 128"/>
+    <key name="home" qpos="0 -2.0 2.0 -1.5708 -1.5708 0 0 0 0 0 0 0 0 0 0.5 0.2 0.45 1 0 0 0"
+         ctrl="0 -2.0 2.0 -1.5708 -1.5708 0 128"/>
   </keyframe>
 </mujoco>

robots/ur5/ur5_env.py

@@ -32,14 +32,15 @@ class UR5Env(gym.Env):
         "wrist_3_joint",
     ]
 
-    # Home position (arm pointing up and forward)
+    # Home position - arm reaching forward with gripper pointing down
+    # This is a typical "ready to pick" pose for manipulation tasks
     DEFAULT_HOME_POSE = np.array([
-        -1.5708,  # shoulder_pan
-        -1.5708,  # shoulder_lift
-        1.5708,   # elbow
-        -1.5708,  # wrist_1
-        -1.5708,  # wrist_2
-        0.0,      # wrist_3
+        0.0,      # shoulder_pan - facing forward
+        -2.0,     # shoulder_lift - angled down
+        2.0,      # elbow - bent
+        -1.57,    # wrist_1 - aligned
+        -1.57,    # wrist_2 - gripper pointing down
+        0.0,      # wrist_3 - no rotation
     ], dtype=np.float32)
 
     def __init__(self, render_mode=None, width=1280, height=720):
@@ -109,8 +110,13 @@ class UR5Env(gym.Env):
             arm_joint_names=self.JOINT_NAMES
         )
 
-        # Target position for IK
-        self._target_pos = np.array([0.4, 0.0, 0.6], dtype=np.float32)
+        # Target position for IK - matching the new home pose
+        self._target_pos = np.array([0.4, 0.0, 0.4], dtype=np.float32)
+        # Target orientation as Euler angles (roll, pitch, yaw) in radians
+        # Default: gripper pointing down (-pi/2 pitch)
+        self._target_euler = np.array([0.0, np.pi/2, 0.0], dtype=np.float32)
+        # Whether to use orientation in IK (user can toggle this)
+        self._use_orientation = True
         # Gripper target (0=closed, 255=open)
         self._gripper_target = 128.0
 
@@ -129,6 +135,22 @@ class UR5Env(gym.Env):
         """Get current target position."""
         return self._target_pos.copy()
 
+    def set_target_orientation(self, roll: float, pitch: float, yaw: float):
+        """Set target orientation as Euler angles (radians)."""
+        self._target_euler = np.array([roll, pitch, yaw], dtype=np.float32)
+
+    def get_target_orientation(self):
+        """Get target orientation as Euler angles (radians)."""
+        return self._target_euler.copy()
+
+    def set_use_orientation(self, use: bool):
+        """Enable/disable orientation control in IK mode."""
+        self._use_orientation = use
+
+    def get_use_orientation(self):
+        """Check if orientation control is enabled."""
+        return self._use_orientation
+
     def set_gripper(self, value: float):
         """Set gripper target (0=closed, 255=open)."""
         self._gripper_target = np.clip(value, 0, 255)
@@ -229,7 +251,8 @@ class UR5Env(gym.Env):
         self.data.qpos[box_qpos_start+3:box_qpos_start+7] = [1, 0, 0, 0]
 
         # Reset target
-        self._target_pos = np.array([0.4, 0.0, 0.6], dtype=np.float32)
+        self._target_pos = np.array([0.4, 0.0, 0.4], dtype=np.float32)
+        self._target_euler = np.array([0.0, np.pi/2, 0.0], dtype=np.float32)  # Gripper pointing down
         self.data.mocap_pos[0] = self._target_pos
         self._gripper_target = 128.0
 
@@ -270,8 +293,19 @@ class UR5Env(gym.Env):
     def step_with_controller(self, dt: float = 0.002):
         """Step using controller (IK or direct joint control)."""
         if self._control_mode == 'ik':
-            # Compute IK to reach target position
-            joint_targets = self.controller.compute_ik(self._target_pos)
+            if self._use_orientation:
+                # Compute IK with position and orientation
+                target_quat = IKController.euler_to_quat(
+                    self._target_euler[0],
+                    self._target_euler[1],
+                    self._target_euler[2]
+                )
+                joint_targets = self.controller.compute_ik_with_orientation(
+                    self._target_pos, target_quat
+                )
+            else:
+                # Compute IK with position only
+                joint_targets = self.controller.compute_ik(self._target_pos)
         else:
             # Use direct joint targets
             joint_targets = self._joint_targets