# RM65 Open Cabinet ## Overview This document describes the `rm65-open-cabinet` manipulation task environment. The environment uses an RM65 6-DOF robotic arm with a parallel gripper. The goal is to approach the bottom drawer handle, establish a stable grasp, and pull the drawer open. ```{video} /_static/videos/rm65_open_cabinet.mp4 :poster: _static/images/poster/rm65_open_cabinet.png :nocontrols: :autoplay: :playsinline: :muted: :loop: :width: 100% ``` --- ## Environment Description This task is built on an RM65 arm and a cabinet drawer scene. Although the registered environment name is `rm65-open-cabinet`, the current implementation actually targets the bottom drawer, using `drawer_bottom_handle` and `drawer_bottom_joint` in code. ### Robot Structure The RM65 robot in this environment contains the following major components: - **Base (`base_link`)**: Fixed in front of the workspace - **6 arm joints**: `joint_1` to `joint_6` - **Parallel gripper**: The main driven gripper joint is `gripper_Left_1_Joint`, while the other 5 gripper joints follow through mimic linkage - **End effector (TCP)**: The `gripper` site, used to compute the relative pose between the tool center point and the drawer handle - **Finger contact sites**: `left_finger_pad` and `right_finger_pad`, used to evaluate handle alignment and penetration ### Scene Objects - **Cabinet**: Contains multiple doors and drawers - **Target handle**: `drawer_bottom_handle` - **Target joint**: `drawer_bottom_joint`, with a sliding range of `0.0 ~ 0.4 m` ### Task Objective The robot is expected to complete the following stages: 1. **Approach the handle**: Move the TCP close to the drawer handle 2. **Align the pose**: Match the gripper pose to the handle pose 3. **Secure the grasp**: Close the gripper and maintain a stable grasp 4. **Open the drawer**: Pull the bottom drawer along its sliding direction --- ## Action Space The action space is `Box(-inf, inf, (7,), float32)`. The first 6 dimensions control arm joint targets, and the last dimension controls gripper opening and closing. ### Control Mode - **Arm**: Uses `joint_target` mode by default, with normalized target actions Arm actions are clipped to `[-1, 1]` first, then linearly mapped to each joint control range - **Gripper**: Uses `binary` mode by default The raw action is converted to a closing probability through a Sigmoid function, then turned into a binary open/close command with hysteresis ### Action Dimension Details | Index | Action Description | Raw Input Range | Controlled Target | | ----- | ------------------ | --------------- | ---------------------- | | 0 | Joint 1 target | `(-inf, inf)` | `joint_1` | | 1 | Joint 2 target | `(-inf, inf)` | `joint_2` | | 2 | Joint 3 target | `(-inf, inf)` | `joint_3` | | 3 | Joint 4 target | `(-inf, inf)` | `joint_4` | | 4 | Joint 5 target | `(-inf, inf)` | `joint_5` | | 5 | Joint 6 target | `(-inf, inf)` | `joint_6` | | 6 | Gripper open/close | `(-inf, inf)` | `gripper_Left_1_Joint` | ### Control Constraints - Control period: `ctrl_dt = 0.025s`, corresponding to 40 Hz - The arm uses speed limits, acceleration limits, action delay, and first-order actuator lag by default - During training, arm delay, lag, speed limits, and acceleration limits are randomized per episode to improve sim-to-real robustness - The gripper uses hysteresis thresholds: - Open-to-close threshold: `0.78` - Close-to-open threshold: `0.62` - Minimum switching interval: `0.25s` --- ## Observation Space The observation space is `Box(-inf, inf, (84,), float32)`, and the final observation is clipped to `[-5, 5]`. ### Observation Components The observation is composed of the following 4 parts: 1. **Joint positions (7 dimensions)** - 6 arm joints - 1 primary gripper joint - All normalized to `[-1, 1]` 2. **Joint velocities (7 dimensions)** - Estimated by finite differences between consecutive joint positions - Then divided by `2` for scaling 3. **Target relative pose (7 dimensions)** - Relative position from TCP to handle - Relative orientation from TCP to handle in quaternion form 4. **Action history (63 dimensions)** - Raw actions from the most recent `9` steps - `7` values per step, for `9 × 7 = 63` dimensions ### Observation Dimension Details | Index Range | Description | Dimension | | ----------- | ------------------------------------ | --------- | | 0-6 | Normalized joint positions | 7 | | 7-13 | Joint velocities | 7 | | 14-16 | Relative position from TCP to handle | 3 | | 17-20 | Relative orientation quaternion | 4 | | 21-83 | Recent 9-step action history | 63 | ### Observation Noise Sim-to-real observation perturbations are enabled by default, including: - Joint position and velocity noise - Handle position and orientation noise - Persistent handle observation bias - Random handle observation dropout, optionally holding the previous observation on dropout --- ## Reward Function The reward is a staged composite design that encourages approach, alignment, stable grasping, and continuous drawer opening. ### Main Reward Terms 1. **Distance reward** ```python dist_reward = 15.0 * (1 - tanh(distance / 0.4)) ``` Encourages the TCP to stay close to the handle. 2. **Orientation reward** Computed from quaternion similarity between the TCP pose and the handle pose, and only applied when the TCP is sufficiently close to the handle. 3. **Gripper closing reward** When the TCP is within `0.035m` and the two fingers are vertically aligned around the handle, closing the gripper is rewarded; otherwise it is penalized. This term is also scaled by the gripper closing amount. 4. **Drawer opening reward** ```python open_reward = (exp(open_dist) - 1.0) * 420.0 ``` This reward is only active when the robot has already grasped the handle, or has entered the grasp-maintenance phase while still staying near the handle. 5. **Open-distance delta reward** Provides extra reward for newly gained drawer displacement at the current step, encouraging stable and continuous pulling. 6. **Stable grasp reward** When the TCP is within `0.03m`, the gripper close ratio exceeds `0.7`, and this condition is maintained for `6` consecutive steps, the environment considers the handle grasped and provides persistent reward. 7. **Milestone rewards** - Reward `35` when drawer opening exceeds `0.15m` - Additional reward `70` when drawer opening exceeds `0.22m` ### Penalty Terms 1. **Slip penalty** If the environment has entered the grasping phase but the robot later loses the grasp while the drawer is already open, an additional penalty is applied. 2. **Finger penetration penalty** Applied when the finger contact points cross the upper or lower handle boundary, discouraging unrealistic penetration. 3. **Gripper switching penalty** Penalizes frequent gripper toggling when the TCP is near the handle. 4. **Action-change penalty** Penalizes the squared difference between consecutive actions. 5. **Joint-velocity penalty** Penalizes the squared sum of joint velocities, with a larger weight in later training. 6. **Termination penalty** Applies an additional `-10.0` penalty when a termination condition is triggered. --- ## Initial State ### Robot Initialization - The arm starts from its default zero pose - The gripper starts in the open state - No joint-position reset noise is added by default - All joint velocities are initialized to zero ### Scene Initialization - The target drawer starts fully closed - The drawer and the rest of the cabinet remain at their default scene poses ### Randomized Factors At reset, the environment re-samples a subset of sim-to-real parameters, including: - Arm action delay - Arm actuator lag - Arm speed and acceleration limits - Handle observation bias --- ## Episode Termination Conditions The episode terminates early if any of the following conditions is met: 1. **TCP moves too far behind the handle** - Threshold: `tcp_x - handle_x < -0.02` 2. **Joint velocity becomes too large** - Any robot joint velocity magnitude exceeds `3.93 rad/s` In addition, the maximum episode length is `30s`. --- ## Usage ### Training ```bash uv run scripts/train.py --env rm65-open-cabinet --train-backend torch ``` ### Policy Evaluation ```bash uv run scripts/play.py --env rm65-open-cabinet ``` ### TensorBoard ```bash uv run tensorboard --logdir runs/rm65_open_cabinet ```