chore: release v0.3.0
This commit is contained in:
motphys-developers
257
docs/source/en/user_guide/demo/rm65_open_cabinet.md
Normal file
257
docs/source/en/user_guide/demo/rm65_open_cabinet.md
Normal file
@@ -0,0 +1,257 @@
|
||||
# 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
|
||||
```
|
||||
Reference in New Issue
Block a user