cyy/Motrixlab
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: release v0.3.0

Browse Source
This commit is contained in:
motphys-developers
2026-04-02 03:45:10 +00:00
parent c84d382b8c
commit e1421d1055
232 changed files with 20258 additions and 2004 deletions
Download Patch File Download Diff File Expand all files Collapse all files

199
docs/source/en/user_guide/demo/dm_lqr.md Normal file
View File

@@ -0,0 +1,199 @@
# Linear Quadratic Regulator
LQR (Linear Quadratic Regulator) is a classic continuous control and stabilization task. This repository currently provides two variants:
- `dm-lqr-2-1`: two masses connected by a rope, with only the last mass actuated
- `dm-lqr-6-2`: six masses connected as a chain, with only the last two masses actuated
The goal is to drive the whole system back to the center and keep it near equilibrium with minimal control effort.
```{video} /_static/videos/dm_lqr_2_1.mp4
:poster: _static/images/poster/dm_lqr_2_1.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
```{video} /_static/videos/dm_lqr_6_2.mp4
:poster: _static/images/poster/dm_lqr_6_2.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Task Description
Both tasks can be viewed as one-dimensional spring-damper chain stabilization problems. Each mass has a single translational degree of freedom along the x-axis. Neighboring masses are coupled by rope-like spring forces, and the system is affected by:
- body damping on each mass
- spring forces and relative damping between neighboring masses
- a center-restoring force pulling the system toward the origin
- control inputs applied only to the actuated terminal degrees of freedom
In practice:
- `dm-lqr-2-1` is the simpler version and is useful for verifying whether the policy can learn a stable equilibrium
- `dm-lqr-6-2` is more difficult because the controller must propagate its effect through a longer chain
---
## Action Space
### dm-lqr-2-1
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (1,), float32)` |
| **Dimension** | 1 |
| Index | Action Description | Min | Max | XML Joint |
| ----- | -------------------------------------- | ---- | --- | --------- |
| 0 | Control input applied to the last mass | -1.0 | 1.0 | `q1` |
### dm-lqr-6-2
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (2,), float32)` |
| **Dimension** | 2 |
| Index | Action Description | Min | Max | XML Joint |
| ----- | --------------------------------------------- | ---- | --- | --------- |
| 0 | Control input applied to the second-last mass | -1.0 | 1.0 | `q4` |
| 1 | Control input applied to the last mass | -1.0 | 1.0 | `q5` |
---
## Observation Space
The observation is formed by concatenating all positions `qpos` and velocities `qvel`.
### dm-lqr-2-1
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (4,), float32)` |
| **Dimension** | 4 |
| Index | Observation | Meaning |
| ----- | ----------- | --------------------------- |
| 0 | `q0` | Position of the first mass |
| 1 | `q1` | Position of the second mass |
| 2 | `dq0` | Velocity of the first mass |
| 3 | `dq1` | Velocity of the second mass |
### dm-lqr-6-2
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (12,), float32)` |
| **Dimension** | 12 |
The first 6 dimensions are `q0 ~ q5`, and the last 6 dimensions are `dq0 ~ dq5`.
---
## Reward Function Design
The current reward is composed of state cost, velocity cost, control cost, success bonus, and out-of-bounds penalty:
```python
state_cost = 0.5 * sum(qpos ** 2)
velocity_cost = 0.5 * velocity_cost_coef * sum(qvel ** 2)
control_cost = 0.5 * control_cost_coef * sum(action ** 2)
reward = 1.0 - (state_cost + velocity_cost + control_cost)
reward += success_bonus
reward -= out_of_bounds_penalty
```
Intuitively:
- the farther the system is from the origin, the lower the reward
- larger velocities reduce the reward
- aggressive control inputs reduce the reward
- entering a small stable region around the origin yields a success bonus
- leaving the valid state boundary triggers an additional penalty
---
## Initial State
At reset:
- the position vector is sampled in a random direction and normalized to a fixed norm
- all initial velocities are set to zero
With the current configuration:
- `dm-lqr-2-1` starts with position norm around `0.8`
- `dm-lqr-6-2` starts with position norm around `1.0`
---
## Episode Termination Conditions
An episode terminates and resets when any of the following conditions is met:
- success condition is reached:
the position norm is below the success distance threshold and the velocity norm is below the success velocity threshold
- out-of-bounds condition is reached:
any position exceeds the position boundary or any velocity exceeds the velocity boundary
- the full state is sufficiently close to zero
- `NaN` appears in the observation or action
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-lqr-2-1
uv run scripts/view.py --env dm-lqr-6-2
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-lqr-2-1
uv run scripts/train.py --env dm-lqr-6-2
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-lqr-2-1
uv run tensorboard --logdir runs/dm-lqr-6-2
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-lqr-2-1
uv run scripts/play.py --env dm-lqr-6-2
```
---
## Expected Training Results
### dm-lqr-2-1
1. The actuated mass pulls the unactuated mass back toward the center.
2. Both positions and velocities converge to a small neighborhood of zero.
3. The learned policy does not settle at a biased off-center equilibrium.
### dm-lqr-6-2
1. The last two actuated masses gradually pull the entire chain back toward the center.
2. The chain remains stable without obvious divergence or persistent oscillation.
3. Success rate increases during training while the out-of-bounds rate decreases.

159
docs/source/en/user_guide/demo/dm_point_mass.md Normal file
View File

@@ -0,0 +1,159 @@
# Point Mass Environment
The Point Mass environment is a simple yet fundamental 2D navigation task where an agent controls a point mass to reach a target position. This environment serves as an excellent introduction to reinforcement learning concepts and continuous action spaces.
```{video} /_static/videos/point_mass.mp4
:poster: _static/images/poster/point_mass.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
The Point Mass environment is a 2D navigation task. The agent needs to control a point mass by applying forces to move it to a randomly generated target position. This task requires the agent to learn efficient navigation strategies to reach the target with minimal control cost.
---
## Action Space (Action Space)
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (2,), float32)` |
| **Dimension** | 2 |
Actions correspond to:
| Index | Action Meaning (Applied Force) | Min | Max | XML Name |
| ----: | ------------------------------ | :-: | :-: | :-------: |
| 0 | x-direction force | -1 | 1 | `x_force` |
| 1 | y-direction force | -1 | 1 | `y_force` |
---
## Observation Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (9,), float32)` |
| **Dimension** | 9 |
The observation space of the Point Mass environment consists of the following components (in order):
| Component | Description | Dimension | Notes |
| ------------ | ---------------------------- | --------- | ----- |
| **Position** | Point mass x, y coordinates | 2 | |
| **Velocity** | Point mass x, y velocities | 2 | |
| **Target** | Target x, y coordinates | 2 | |
| **Distance** | Distance vector to target | 2 | |
| **Distance** | Euclidean distance to target | 1 | |
---
## Reward Function Design
The Point Mass environment's reward function consists of the following components:
### Distance Reward
```python
# Exponential distance reward - stronger as agent gets closer
distance_reward = np.exp(-10 * dist_to_target)
```
### Target Arrival and Stay Reward
```python
# Large bonus for reaching target
target_bonus = 100.0 * in_target
# Continuous reward for staying in target
continuous_reward = 30.0 * in_target
```
### Control and Path Optimization
```python
# Penalty for distance from target center when inside target
center_penalty = np.where(in_target, 10.0 * dist_to_target, 0.0)
# Control penalty to encourage smooth movement
control_penalty = 0.1 * vel_magnitude
# Path optimization reward for straight-line movement
path_reward = 0.5 * direction_alignment
```
### Total Reward Calculation
```python
# Combine all reward components
rwd = distance_reward + target_bonus + continuous_reward + path_reward - center_penalty - control_penalty
```
---
## Initial State
- Point mass position randomly initialized within [-1.0, 1.0]
- Target position randomly initialized within [-1.5, 1.5]
- Point mass velocity initialized to 0
## Episode Termination Conditions
- Point mass reaches target and stays for 0.5 seconds
- Simulation time reaches 10 seconds
- Observation contains abnormal values (NaN)
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env point_mass
```
### 2. Start Training
```bash
uv run scripts/train.py --env point_mass
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/point_mass
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env point_mass
```
---
## Expected Training Results
### Navigation Performance
1. Agent learns to move directly towards the target
2. Smooth movement with minimal control effort
3. Consistent target reaching within episode duration
### Learning Progress
1. Rapid initial learning phase as agent discovers basic navigation
2. Gradual refinement of control strategy
3. Stable performance across different target positions
### Behavior Characteristics
1. Efficient path planning towards target
2. Smooth approach to target center
3. Minimal overshooting or oscillatory behavior

290
docs/source/en/user_guide/demo/dm_quadruped.md Normal file
View File

@@ -0,0 +1,290 @@
# Quadruped Robot
The Quadruped robot is a classic continuous control task in the DeepMind Control Suite. In MotrixLab, the `motrix_envs/src/motrix_envs/basic/quadruped` directory currently registers four directly trainable tasks: flat-ground walking `dm-quadruped-walk`, flat-ground running `dm-quadruped-run`, rough-terrain escape `dm-quadruped-escape`, and flat-ground ball pushing `dm-quadruped-fetch`.
## Task Preview
### Walk
```{video} /_static/videos/dm_quadruped_walk.mp4
:poster: _static/images/poster/dm_quadruped_walk.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
### Run
```{video} /_static/videos/dm_quadruped_run.mp4
:poster: _static/images/poster/dm_quadruped_run.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
### Escape
```{video} /_static/videos/dm_quadruped_escape.mp4
:poster: _static/images/poster/dm_quadruped_escape.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
### Fetch
```{video} /_static/videos/dm_quadruped_fetch.mp4
:poster: _static/images/poster/dm_quadruped_fetch.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Overview
| Environment ID | Task Goal | Model File | Target Speed | Observation Dimension |
| --------------------- | ---------------------------------------------------------------------------- | ---------------------- | ------------ | --------------------- |
| `dm-quadruped-walk` | Walk forward stably on flat ground while maintaining heading | `quadruped_walk.xml` | 0.5 m/s | 54 |
| `dm-quadruped-run` | Run at high speed on flat ground while maintaining stable posture | `quadruped_walk.xml` | 5.0 m/s | 54 |
| `dm-quadruped-escape` | Escape outward from the origin area as quickly as possible on uneven terrain | `quadruped_escape.xml` | 3.0 m/s | 57 |
| `dm-quadruped-fetch` | Push a ball into the target area on flat ground | `quadruped_fetch.xml` | 2.0 m/s | 66 |
## Task Description
Quadruped is a 3D quadruped robot task. The robot consists of one torso and four legs, and each leg has control dimensions related to yaw, lift, and extension. The underlying XML defines the hip, knee, and ankle joint structure, while the action layer uses a coupled actuator design with 3 actuators per leg:
- `yaw`: controls leg yaw
- `lift`: controls leg lifting through tendon coupling
- `extend`: controls leg extension/retraction through tendon coupling
`walk` and `run` use the same flat-ground model, with the main difference being the target speed. `escape` uses `quadruped_escape.xml` with a heightfield terrain and requires the robot to move away from the world origin quickly while maintaining an upright torso and stable locomotion. `fetch` uses `quadruped_fetch.xml`, which adds a free ball and a target region to the scene, requiring the robot to first approach a suitable position and then push the ball toward the goal.
---
## Action Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(low, high, (12,), float32)` |
| **Dimension** | 12 |
Actions are arranged leg by leg, and each leg contains three actuators: `yaw / lift / extend`.
| Index | Action Meaning | Min Value | Max Value | Corresponding Actuator |
| ----: | --------------------------------- | :-------: | :-------: | ---------------------- |
| 0 | Front-left leg yaw control | -1.0 | 1.0 | `yaw_front_left` |
| 1 | Front-left leg lift control | -1.0 | 1.1 | `lift_front_left` |
| 2 | Front-left leg extension control | -0.8 | 0.8 | `extend_front_left` |
| 3 | Front-right leg yaw control | -1.0 | 1.0 | `yaw_front_right` |
| 4 | Front-right leg lift control | -1.0 | 1.1 | `lift_front_right` |
| 5 | Front-right leg extension control | -0.8 | 0.8 | `extend_front_right` |
| 6 | Rear-right leg yaw control | -1.0 | 1.0 | `yaw_back_right` |
| 7 | Rear-right leg lift control | -1.0 | 1.1 | `lift_back_right` |
| 8 | Rear-right leg extension control | -0.8 | 0.8 | `extend_back_right` |
| 9 | Rear-left leg yaw control | -1.0 | 1.0 | `yaw_back_left` |
| 10 | Rear-left leg lift control | -1.0 | 1.1 | `lift_back_left` |
| 11 | Rear-left leg extension control | -0.8 | 0.8 | `extend_back_left` |
---
## Observation Space
| Environment | Details |
| --------------------- | -------------------------------- |
| `dm-quadruped-walk` | `Box(-inf, inf, (54,), float32)` |
| `dm-quadruped-run` | `Box(-inf, inf, (54,), float32)` |
| `dm-quadruped-escape` | `Box(-inf, inf, (57,), float32)` |
| `dm-quadruped-fetch` | `Box(-inf, inf, (66,), float32)` |
All four tasks share nearly the same proprioceptive observations. `escape` adds 3 task-related dimensions associated with the origin, while `fetch` adds ball state and target position information:
| Part | Description | Dimension | `walk/run` | `escape` | `fetch` |
| ---------------------- | --------------------------------------------------------- | --------- | ---------- | -------- | ------- |
| **egocentric dof pos** | Body generalized position state | 16 | Yes | Yes | Yes |
| **egocentric dof vel** | Body generalized velocity state | 16 | Yes | Yes | Yes |
| **actuator ctrl** | Current 12-dimensional actuator controls | 12 | Yes | Yes | Yes |
| **torso velocity** | Torso linear velocity sensor `velocimeter` | 3 | Yes | Yes | Yes |
| **torso upright** | Scalar representing torso uprightness | 1 | Yes | Yes | Yes |
| **imu** | IMU acceleration and angular velocity | 6 | Yes | Yes | Yes |
| **origin** | World origin position in the body frame | 3 | No | Yes | No |
| **ball state** | Ball position, relative linear velocity, angular velocity | 9 | No | No | Yes |
| **target** | Relative target position in the body frame | 3 | No | No | Yes |
The XML also defines foot force/torque sensors and a center-of-mass sensor, but these values are not directly concatenated into the default observation in the current implementation.
---
## Reward Function Design
All four tasks use torso uprightness as the core constraint. In the implementation, `upright_reward` is computed first from `torso_upright`, encouraging the robot to keep the body close to upright.
### Walk / Run
`dm-quadruped-walk` and `dm-quadruped-run` use the same reward structure, with different target speeds:
- `walk` tracks `0.5 m/s`
- `run` tracks `5.0 m/s`
The total reward is composed of:
```python
# Speed reward: reach the target forward speed
# Posture reward: keep the torso upright
# Auxiliary rewards: height, lateral stability, heading alignment, action smoothness
# Penalties: backward motion, excessive vertical speed, excessive roll/pitch angular velocity, deviation from default posture
total_reward = upright_reward * move_reward + shaping_terms - penalty_terms
```
The main shaping and penalty terms include:
- `height_reward`: encourages the torso to stay near the standing height
- `lateral_reward`: suppresses excessive lateral velocity
- `heading_reward`: encourages forward motion along the +X direction
- `smooth_reward`: penalizes large action changes between consecutive timesteps
- `backward_penalty`: suppresses backward movement
- `lin_vel_z_penalty` and `ang_vel_xy_penalty`: suppress vertical bouncing and excessive torso roll/pitch
- `similar_to_default_penalty`: encourages joint posture to stay reasonably close to the default standing pose
### Escape
`dm-quadruped-escape` adds a task term for escaping away from the origin area on top of the locomotion reward. This task uses the heightfield terrain in `quadruped_escape.xml`:
```python
# Base locomotion reward
# + reward for getting farther from the origin
# + reward for outward radial speed
total_reward = locomotion_reward + upright_reward * escape_reward + radial_speed_reward
```
Additional task terms include:
- `escape_reward`: rewards the robot based on its distance from the origin area
- `radial_speed_reward`: encourages acceleration along the outward direction away from the origin
This makes `escape` require not only fast locomotion, but also correct outward motion on rough terrain.
### Fetch
`dm-quadruped-fetch` uses a task-specific shaping structure centered on positioning and ball pushing. In the current implementation, the reward is mainly based on the geometric relationship between the robot, the ball, and the target:
```python
# Positioning stage: encourage the robot to move behind or slightly behind the ball
# Ready stage: encourage facing the ball, getting close to it, and aligning with the ball-target line
# Pushing stage: encourage the ball to roll toward the target and eventually enter the target area
# Penalties: moving in the wrong direction, pushing the ball away from the target, getting the legs too close to the ball
total_reward = stage_terms + ready_terms + push_terms - penalty_terms
```
The main terms include:
- `stage_move`: encourages movement toward the current stage waypoint
- `stage_reach`: encourages reaching a suitable waypoint behind or to the side of the ball
- `behind_align`: encourages the robot to position itself behind the ball relative to the target
- `face_ball`: encourages the torso heading to point toward the ball
- `near_ball`: encourages the robot to approach the ball
- `ready` and `ready_gate`: combine position, orientation, and distance into a readiness signal for active pushing
- `fetch`: encourages the ball to get closer to the target region
- `push`: encourages the ball to move along the target direction
- `backward`: penalizes moving opposite to the current stage target
- `away`: penalizes pushing the ball away from the target
- `leg_ball`: penalizes leg geometry getting too close to the ball, reducing ball trapping and squeezing behavior
In addition, `fetch` uses a `stability` gate on torso uprightness and torso height so the agent cannot easily exploit obviously collapsed poses to collect task reward.
---
## Initial State
- `walk`, `run`, and `escape` reset from the default quadruped standing pose defined in the XML
- For these three tasks, the root orientation is fixed to the initial heading instead of being randomly rotated
- `fetch` randomizes the robot position and yaw on the plane, and also randomizes the ball position on the ground
- All tasks initialize joint velocities, and in `fetch` also ball velocities, to zero
- During reset, the robot is automatically lifted until there is no initial penetration/contact with the ground
## Episode Termination Conditions
- Maximum episode duration is 20 seconds
- The episode terminates when `NaN` appears in the observation
- `walk`, `run`, and `escape` do not currently define a separate fall termination condition
- `fetch` terminates early when the robot has clearly fallen, based on low torso uprightness or low torso height
- The current implementation does not yet define a separate success termination condition for "ball enters the target area"
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-quadruped-walk
uv run scripts/view.py --env dm-quadruped-run
uv run scripts/view.py --env dm-quadruped-escape
uv run scripts/view.py --env dm-quadruped-fetch
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-quadruped-walk
uv run scripts/train.py --env dm-quadruped-run
uv run scripts/train.py --env dm-quadruped-escape
uv run scripts/train.py --env dm-quadruped-fetch
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-quadruped-walk
uv run tensorboard --logdir runs/dm-quadruped-run
uv run tensorboard --logdir runs/dm-quadruped-escape
uv run tensorboard --logdir runs/dm-quadruped-fetch
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-quadruped-walk
uv run scripts/play.py --env dm-quadruped-run
uv run scripts/play.py --env dm-quadruped-escape
uv run scripts/play.py --env dm-quadruped-fetch
```
---
## Expected Training Results
### Walking Task (`dm-quadruped-walk`)
1. Maintain a stable forward speed close to `0.5 m/s`
2. Keep body posture stable with small lateral sway
3. Sustain walking along the +X direction
### Running Task (`dm-quadruped-run`)
1. Increase speed to near or above `5.0 m/s`
2. Produce larger stride length and more explosive motions
3. Maintain good torso stability under high-speed locomotion
### Escape Task (`dm-quadruped-escape`)
1. Move away from the origin region quickly
2. Maintain stable footholds on the heightfield terrain without easily tipping over
3. Move primarily outward rather than spinning in place
### Fetch Task (`dm-quadruped-fetch`)
1. First move into a reasonable position along the ball-target line instead of randomly colliding with the ball from the side
2. Push the ball toward the target area consistently instead of kicking it away or repeatedly sending it off course
3. Maintain better body stability during pushing, with fewer collapsed poses, flips, or leg-ball entanglement behaviors

257
docs/source/en/user_guide/demo/rm65_open_cabinet.md Normal file
View 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
```