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

8
.github/workflows/docker-build.yml vendored
View File

@@ -21,17 +21,17 @@ jobs:
run: |
echo "Removing hosted toolcache..."
sudo rm -rf /opt/hostedtoolcache || true
echo "Removing .NET..."
sudo rm -rf /usr/share/dotnet || true
echo "Removing Android SDK..."
sudo rm -rf /usr/local/lib/android || true
echo "Removing GHC / Haskell..."
sudo rm -rf /opt/ghc || true
sudo rm -rf /usr/local/.ghcup || true
echo "Docker prune (just in case runner had leftovers)..."
sudo docker container prune -f || true
sudo docker image prune -af || true

3
.gitignore vendored
View File

@@ -8,4 +8,5 @@ logs
node_modules
docs/build
docs/source/user_guide
docs/source/index.md
docs/source/index.md
.worktrees

3
.prettierignore
View File

@@ -7,4 +7,5 @@ node_modules
.venv
.gitlab-ci.yml
docs/build/
.readthedocs.yaml
.readthedocs.yaml
CLAUDE.md

55
CLAUDE.md
View File

@@ -4,7 +4,7 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co
## Project Overview
MotrixLab is a reinforcement learning framework built on top of MotrixSim simulation backend. It provides a unified interface for training RL agents using multiple simulation backends (MotrixSim) and primarily integrates with the SKRL library. The framework is designed for robotics simulation and supports various environments including basic cartpole, locomotion tasks, and manipulation tasks.
MotrixLab is a reinforcement learning framework built on top of MotrixSim simulation backend. It provides a unified interface for training RL agents using multiple simulation backends (MotrixSim) and integrates with SKRL and RSLRL libraries. The framework is designed for robotics simulation and supports various environments including basic cartpole, locomotion tasks, and manipulation tasks.
## Development Setup
@@ -21,13 +21,18 @@ For SKRL framework with specific backend:
```bash
uv sync --all-packages --extra skrl-jax # JAX backend
uv sync --all-packages --extra skrl-torch # PyTorch backend
```
For rslrl frame:
```bash
uv sync --all-packages --extra rslrl
```
**Available dependency groups in MotrixLab:**
- `skrl-jax`: SKRL RL framework with JAX backend
- `skrl-torch`: SKRL RL framework with PyTorch backend
- `test`: Test dependencies (pytest)
see `pyproject.toml`
**Note**: This is a workspace project with two main packages: `motrix_envs` (simulation environments) and `motrix_rl` (RL framework integration).
@@ -35,10 +40,16 @@ uv sync --all-packages --extra skrl-torch # PyTorch backend
### Training
Train with SKRL (default):
```bash
uv run scripts/train.py --env cartpole
```
Train with RSLRL:
```bash
uv run scripts/train.py --env cartpole --rllib rslrl
```
### Environment Visualization
View environment without training:
@@ -84,13 +95,11 @@ uv run pytest
### Core Components
1. **Workspace Structure**:
- `motrix_envs/`: Simulation environment definitions using MotrixSim backend
- `motrix_rl/`: RL framework integration (primarily SKRL) and training utilities
2. **Scripts** (`scripts/`):
- `train.py`: Main training script with configurable environments and backends
- `train.py`: Main training script with configurable environments, frameworks, and backends (use `--rllib` to select)
- `view.py`: Environment visualization without training
- `play.py`: Policy evaluation and testing
@@ -98,28 +107,28 @@ uv run pytest
### Key Architecture Points
- **Workspace Project**: Uses UV workspace with two packages sharing dependencies
- **MotrixSim Backend**: Built on MotrixSim simulation engine for physics simulation
- **SKRL Integration**: Primary RL framework supporting both JAX and PyTorch backends
- **Environment Naming**: Simple string-based environment identification (e.g., "cartpole")
- **Automatic Backend Selection**: Training script automatically selects JAX or PyTorch based on GPU availability
- **Multi-Backend Training**: Supports different simulation backends for the same environment
- **Workspace Project**: Uses UV workspace with two packages sharing dependencies
- **MotrixSim Backend**: Built on MotrixSim simulation engine for physics simulation
- **SKRL Integration**: RL framework supporting both JAX and PyTorch backends
- **RSLRL Integration**: RL framework supporting PyTorch backend (use `--rllib rslrl`)
- **Environment Naming**: Simple string-based environment identification (e.g., "cartpole")
- **Automatic Backend Selection**: For SKRL, training script automatically selects JAX or PyTorch based on GPU availability; RSLRL uses PyTorch only
- **Multi-Backend Training**: Supports different simulation backends for the same environment
### Environment Usage Pattern
```python
# Environment creation handled internally by scripts
# Use string names to specify environments
uv run scripts/train.py --env cartpole
```
## Results Storage
Training results are saved to `runs/{env-name}/` directory structure with checkpoints and tensorboard logs.
## Important Notes
- **Python Version**: Requires exactly Python 3.10.\*
- **GPU Support**: Includes CUDA support for both JAX and PyTorch backends
- **Private PyPI**: Uses internal PyPI server for MotrixSim packages
- **No Manual Tests**: No test files found in the repository structure
- **Python Version**: Requires exactly Python 3.10.\*
- **GPU Support**: Includes CUDA support for both JAX and PyTorch backends
- **Private PyPI**: Uses internal PyPI server for MotrixSim packages
- **No Manual Tests**: No test files found in the repository structure
### RSLRL Configuration
- **Field Correspondence**: When modifying `RslrlRunnerCfg` in `motrix_rl/rslrl/cfg.py`, ensure fields match `template/rslrl_config.yaml` exactly - no extra or missing fields. This is critical for proper configuration serialization and deserialization.
- **Reference Template**: Use `template/rslrl_config.yaml` as the source of truth for valid runner configuration fields

18
README.md
View File

@@ -12,14 +12,14 @@
The project is divided into two core components:
- **motrix_envs**: Various RL simulation environments built on MotrixSim, defining observation, action, and reward. Framework-agnostic and currently supports MotrixSim's CPU backend
- **motrix_rl**: Integrates RL frameworks and uses various environment parameters from motrix_envs for training. Currently supports SKRL framework's PPO algorithm
- **motrix_rl**: Integrates RL frameworks and uses various environment parameters from motrix_envs for training. Currently supports SKRL framework (JAX/PyTorch) and RSLRL framework (PyTorch) PPO algorithms
> Documentation: https://motrixlab.readthedocs.io
## Key Features
- **Unified Interface**: Provides a concise and unified reinforcement learning training and evaluation interface
- **Multi-backend Support**: Supports JAX and PyTorch training backends, with flexible selection based on hardware environment
- **Multi-framework Support**: Supports SKRL (JAX/PyTorch) and RSLRL (PyTorch) training frameworks with flexible selection based on hardware environment
- **Rich Environments**: Includes various robot simulation environments such as basic control, locomotion, and manipulation tasks
- **High-performance Simulation**: Built on MotrixSim's high-performance physics simulation engine
- **Visual Training**: Supports real-time rendering and training process visualization
@@ -62,6 +62,12 @@ Install PyTorch as training backend:
uv sync --all-packages --extra skrl-torch
```
Install RSLRL framework (PyTorch backend only):
```bash
uv sync --all-packages --extra rslrl
```
## 🎯 Usage Guide
### Environment Visualization
@@ -74,10 +80,18 @@ uv run scripts/view.py --env cartpole
### Model Training
Train with SKRL framework (default):
```bash
uv run scripts/train.py --env cartpole
```
Train with RSLRL framework:
```bash
uv run scripts/train.py --env cartpole --rllib rslrl
```
Training results are saved in the `runs/{env-name}/` directory.
View training data through TensorBoard:

18
README.zh-CN.md
View File

@@ -12,14 +12,14 @@
该项目分为两个核心部分：
- **motrix_envs**: 基于 MotrixSim 构建的各种 RL 仿真环境，定义了 observation、action、reward。与具体的 RL 框架无关，目前支持 MotrixSim 的 CPU 后端
- **motrix_rl**: 集成 RL 框架，并使用 motrix_envs 里的各种环境参数进行训练。目前支持 SKRL 框架的 PPO 算法
- **motrix_rl**: 集成 RL 框架，并使用 motrix_envs 里的各种环境参数进行训练。目前支持 SKRL 框架（JAX/PyTorch）和 RSLRL 框架（PyTorch）的 PPO 算法
> 文档地址：https://motrixlab.readthedocs.io
## 主要特性
- **统一接口**: 提供简洁统一的强化学习训练和评估接口
- **多后端支持**: 支持 JAX 和 PyTorch 训练后端，可根据硬件环境灵活选择
- **多框架支持**: 支持 SKRL (JAX/PyTorch) 和 RSLRL (PyTorch) 训练框架，可根据硬件环境灵活选择
- **丰富环境**: 包含基础控制、运动、操作等多种机器人仿真环境
- **高性能仿真**: 基于 MotrixSim 的高性能物理仿真引擎
- **可视化训练**: 支持实时渲染和训练过程可视化
@@ -62,6 +62,12 @@ uv sync --all-packages --extra skrl-jax
uv sync --all-packages --extra skrl-torch
```
安装 RSLRL 框架（仅支持 PyTorch 后端）：
```bash
uv sync --all-packages --extra rslrl
```
## 🎯 使用指南
### 环境可视化
@@ -74,10 +80,18 @@ uv run scripts/view.py --env cartpole
### 训练模型
使用 SKRL 框架训练（默认）：
```bash
uv run scripts/train.py --env cartpole
```
使用 RSLRL 框架训练：
```bash
uv run scripts/train.py --env cartpole --rllib rslrl
```
训练结果会保存在 `runs/{env-name}/` 目录下。
通过 TensorBoard 查看训练数据：

3
docs/source/_static/images/poster/dm_lqr_2_1.jpg Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:48e8e5cecf80a0b8c640a70d51c64ef8416e4c46b1ac66e7594f30819fbf71d7
size 2525404

3
docs/source/_static/images/poster/dm_lqr_6_2.jpg Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e68b01048e54591da250d636cb7b8be1d5bb6cf8bebb3509484bf2ee4c61d386
size 212340

3
docs/source/_static/images/poster/dm_quadruped_escape.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d0e769091ae74cf540c68dd531200f776553f09f0a15eaa51d349216e5053ec2
size 2288132

3
docs/source/_static/images/poster/dm_quadruped_fetch.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:06cbce23b71c6d8ec00e67e41c57e83e75b47ac9330998c03fa6734da49a848d
size 1788861

3
docs/source/_static/images/poster/dm_quadruped_run.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:43e3894b9dc3a286e9983a2f4786cf47caaf4fbbf00bfb75ea61661d47b92258
size 2066473

3
docs/source/_static/images/poster/dm_quadruped_walk.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:9de7df58544554f1a7b4c523490e52e016e4057794ac27112b28ebaef704534e
size 2678511

3
docs/source/_static/images/poster/point_mass.jpg Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:4c306cf5e29c1380b808ecb327d34fc68fead362695b241632b4858f893ab4de
size 777697

3
docs/source/_static/images/poster/rm65_open_cabinet.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:fe0d085abcf2e8acc9c17ac5bd9ae4019c8f625971633295d5ac78278c84cecd
size 2923711

3
docs/source/_static/videos/dm_lqr_2_1.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:8392355f75fef17266bb6b40169aa2fd868cd42a3649c63ab241008ab9993374
size 519681

3
docs/source/_static/videos/dm_lqr_6_2.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a8b679d72551575db86e836823416be9f9e7d06a44210dacb17803467676a1a4
size 1377670

3
docs/source/_static/videos/dm_quadruped_escape.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:078a36bafb869cef0244ef3e33b9eaa5c661d49eaa9d42a8534f711993ec2fd5
size 1738757

3
docs/source/_static/videos/dm_quadruped_fetch.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:8b8b4097ccf1c2ac509baf2a29e37c1e288e966472cde220eae77fc47644c324
size 508065

3
docs/source/_static/videos/dm_quadruped_run.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:29f418ee33792ecf4621a3d9b68341d6b0addfc5a248f5556bf6b310bfa4675c
size 3177164

3
docs/source/_static/videos/dm_quadruped_walk.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:ecc0db612a958bcc20be30da04c131fe920818bd052eebeb3c8918551a28fe29
size 11380668

3
docs/source/_static/videos/point_mass.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:388d28839dfa4792a9b8f5778cb7991ea00fd522990a77e6bf0b9e4b9c4580c8
size 3663098

3
docs/source/_static/videos/rm65_open_cabinet.mp4 Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:163f1c45d3cda32837e1f08dd403b96c8061ff1c38860e60421b7c5dd6c06222
size 5637813

34
docs/source/en/index.md
View File

@@ -7,8 +7,8 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:::{grid-item-card}
```{video} _static/videos/cartpole.mp4
:poster: _static/images/poster/cartpole.jpg
```{video} _static/videos/shadow_hand_repose.mp4
:poster: _static/images/poster/shadow_hand_repose.jpg
:nocontrols:
:autoplay:
:playsinline:
@@ -147,8 +147,8 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:::{grid-item-card}
```{video} _static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
```{video} _static/videos/cartpole.mp4
:poster: _static/images/poster/cartpole.jpg
:nocontrols:
:autoplay:
:playsinline:
@@ -185,6 +185,32 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_quadruped_fetch.mp4
:poster: _static/images/poster/dm_quadruped_fetch.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_quadruped_run.mp4
:poster: _static/images/poster/dm_quadruped_run.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
::::

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
```

24
docs/source/en/user_guide/getting_started/installation.md
View File

@@ -1,9 +1,13 @@
# Installation Environment
## Installation Requirements
This document will guide you through the installation and configuration of MotrixLab. Please read the system requirements carefully and choose the appropriate installation method based on your use case.
## System Requirements
- **Python Version**: {bdg-danger-line}`3.10.*`
This project requires a specific Python version, other versions are not currently supported:
| Python Version | Support Status |
| :------------: | :------------: |
| ≤ 3.9 | ❌ |
@@ -12,6 +16,8 @@
- **Package Manager**: {bdg-danger-line}`UV`
This project uses UV as the exclusive package management tool to provide fast, reproducible dependency management environment. For UV installation, please refer to the [official documentation](https://docs.astral.sh/uv/getting-started/installation/).
- **System and Architecture**:
- {bdg-danger-line}`Windows(x86_64)`
@@ -26,30 +32,34 @@
| Windows | ✅ | ✅ | 🛠️ In Development |
```
## Installation Method
## Installation Steps
### Clone Project
### Clone Project Repository
```bash
git clone https://github.com/Motphys/MotrixLab.git
cd MotrixLab
```
### Install Dependencies
### Configure Dependencies
Use UV to install project dependencies:
Execute the following command to install complete dependencies:
```bash
# Install all dependencies
uv sync --all-packages --all-extras
```
If you only need to install one training backend, you can choose to install a specific backend type:
If you only need specific training frameworks, you can selectively install to reduce dependency size:
```bash
# Install SKRL JAX (support Linux only)
# Install SKRL JAX (Linux only)
uv sync --all-packages --extra skrl-jax
# Install SKRL PyTorch
uv sync --all-packages --extra skrl-torch
# Install RSLRL (PyTorch only)
uv sync --all-packages --extra rslrl
```

4
docs/source/en/user_guide/index.md
View File

@@ -34,14 +34,18 @@ demo/dm_walker
demo/dm_cheetah
demo/dm_hopper
demo/dm_reacher
demo/dm_lqr
demo/dm_finger
demo/dm_humanoid
demo/dm_quadruped
demo/dm_point_mass
demo/bring_ball
demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c
demo/franka_lift_cube
demo/franka_open_cabinet
demo/rm65_open_cabinet
demo/shadow_hand_repose
```

13
docs/source/en/user_guide/tutorial/basic_frame.md
View File

@@ -16,6 +16,7 @@ MotrixLab/
│ └── registry.py # Environment registry system
├── motrix_rl/ # Training layer: RL algorithms and configuration
│ ├── skrl/ # SKRL framework integration (JAX/PyTorch)
│ ├── rslrl/ # RSLRL framework integration (PyTorch)
│ ├── base.py # RL configuration base class
│ └── registry.py # RL configuration registry system
└── scripts
@@ -34,7 +35,7 @@ MotrixLab/
│
▼
┌─────────────────────────────────────────────────────────────────┐
│ Training Algorithm Layer (SKRL) │
│ Training Algorithm Layer (SKRL / RSLRL) │
│ PPO Trainer │ Network Architecture │ Optimizer │
└─────────────────────────────────────────────────────────────────┘
│
@@ -128,13 +129,15 @@ Configuration parameters play a key connecting role throughout the process:
- **Reward Configuration** affects learning signals (reward weights, calculation methods, etc.)
- **Training Configuration** controls algorithm behavior (network structure, learning rate, batch size, etc.)
## Multi-Backend Support
## Multi-Framework Support
MotrixLab's layered design naturally supports multiple backends:
MotrixLab's layered design naturally supports multiple RL frameworks:
- **Simulation Backends**: MotrixSim (CPU)
- **Training Backends**: JAX and PyTorch, supporting GPU acceleration
- **Algorithm Framework**: Mainly integrates SKRL, easy to extend to other algorithms
- **Training Frameworks**:
- **SKRL**: Supports JAX and PyTorch backends with GPU acceleration
- **RSLRL**: Supports PyTorch backend with GPU acceleration
- **Framework Selection**: Use `--rllib` parameter to choose between `skrl` (default) and `rslrl`
## Design Advantages

58
docs/source/en/user_guide/tutorial/training_and_result.md
View File

@@ -7,56 +7,64 @@ This section introduces how to execute reinforcement learning training and how t
### Basic Training Commands
```bash
# Train with default parameters
# Train with default parameters (SKRL framework)
uv run scripts/train.py --env cartpole
# Specify RL framework
uv run scripts/train.py --env cartpole --rllib skrl
uv run scripts/train.py --env cartpole --rllib rslrl
# Specify simulation backend
uv run scripts/train.py --env cartpole --sim-backend np
# Specify training backend
uv run scripts/train.py --env cartpole --train-backend jax
uv run scripts/train.py --env cartpole --train-backend torch
# Specify training backend (SKRL only)
uv run scripts/train.py --env cartpole --rllib skrl --train-backend jax
uv run scripts/train.py --env cartpole --rllib skrl --train-backend torch
```
### Advanced Training Configuration
```bash
# Customize training parameters
# Customize training parameters with SKRL
uv run scripts/train.py --env cartpole \
--rllib skrl \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
# Customize training parameters with RSLRL
uv run scripts/train.py --env cartpole \
--rllib rslrl \
--num-envs 1024 \
--sim-backend np
# Note: Parameters like learning rate need to be set through configuration files or code override
# Enable rendering to monitor training process
uv run scripts/train.py --env cartpole --render
```
### Different Backend Configuration Differences
### Different Framework Configuration
The system supports configuring different reinforcement learning parameters for different training backends (JAX/Torch). For example:
The system supports different RL frameworks with different configuration systems:
- **dm-walker environment**:
- **SKRL Framework**: Supports JAX and PyTorch training backends with configurable parameters per backend (via Python dataclasses)
- **RSLRL Framework**: Supports PyTorch backend with configuration via Python dataclasses (RslrlCfg)
- JAX backend: `mini_batches: 4`
- Torch backend: `mini_batches: 32`
- **dm-runner environment**:
- JAX backend: `learning_epochs: 4`
- Torch backend: `learning_epochs: 2`
These differences are implemented through the `@rlcfg(env_name, backend="jax/torch")` decorator in configuration classes. The system automatically applies the corresponding configuration based on the selected training backend.
For SKRL, the system supports configuring different reinforcement learning parameters for different training backends (JAX/Torch).
### Supported Command Line Parameters
| Parameter | Description | Default Value |
| ----------------- | ------------------------------- | ------------- |
| `--env` | Environment name | `cartpole` |
| `--sim-backend` | Simulation backend (np) | Auto select |
| `--train-backend` | Training backend (jax/torch) | Auto select |
| `--num-envs` | Number of parallel environments | 2048 |
| `--render` | Enable rendering | False |
| Parameter | Description | Default Value |
| ----------------- | --------------------------------------- | ------------- |
| `--env` | Environment name | `cartpole` |
| `--rllib` | RL framework (skrl/rslrl) | `skrl` |
| `--sim-backend` | Simulation backend (np) | Auto select |
| `--train-backend` | Training backend (jax/torch, SKRL only) | Auto select |
| `--num-envs` | Number of parallel environments | 2048 |
| `--render` | Enable rendering | False |
> **Note**: Other parameters such as learning rate, network structure, etc., can be set in configuration files. Some environments support configuring different parameters for different training backends.
> **Note**: Other parameters such as learning rate, network structure, etc., need to be set through separate configuration files.
## Training Process Monitoring
@@ -83,7 +91,7 @@ uv run tensorboard --logdir runs/cartpole
uv run scripts/play.py --env cartpole
# Manually specify policy file for testing
uv run scripts/play.py --env cartpole --policy runs/cartpole/nn/best_policy.pickle
uv run scripts/play.py --env cartpole --policy runs/cartpole/nn/best_agent.pickle
# Specify number of test environments
uv run scripts/play.py --env cartpole --num-envs 100

241
docs/source/en/user_guide/tutorial/training_environment_config.md
View File

@@ -2,141 +2,80 @@
MotrixLab provides a flexible configuration system that allows users to customize reinforcement learning training parameters. This section introduces how to configure training environments and reinforcement learning algorithm parameters.
## RL Training Configuration (PPOCfg)
## RL Training Configuration
Training configuration defines parameters for reinforcement learning algorithms. MotrixLab now supports configuring different parameters for different training backends.
MotrixLab supports multiple RL frameworks with different configuration systems:
### Basic Training Configuration
- **SKRL Framework**: Uses Python dataclass configuration (SkrlCfg)
- **RSLRL Framework**: Uses Python dataclass configuration (RslrlCfg)
```python
from dataclasses import dataclass
from motrix_rl.skrl.cfg import PPOCfg
from motrix_rl import registry
### SKRL Configuration (SkrlCfg)
# Universal configuration (applies to all backends)
@registry.rlcfg("my-task")
@dataclass
class MyTaskRLCfg(PPOCfg):
# Environment parameters
num_envs: int = 2048 # Number of parallel environments during training
play_num_envs: int = 16 # Number of parallel environments during evaluation
Training configuration defines parameters for reinforcement learning algorithms based on PPO. MotrixLab now supports configuring different parameters for different training backends.
# PPO algorithm parameters
learning_rate: float = 3e-4 # Learning rate
rollouts: int = 32 # Experience replay rounds
learning_epochs: int = 10 # Number of epochs per update
mini_batches: int = 32 # Number of mini-batches
discount_factor: float = 0.99 # Discount factor
grad_norm_clip: float = 1.0 # Gradient clipping
#### Complete Configuration Example
# Network structure parameters
policy_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64) # Policy network hidden layers
value_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64) # Value network hidden layers
The following is the actual `CartPoleSkrlPpo` configuration, demonstrating complete explicit parameter filling. This configuration uses a smaller network `[32, 32]` suitable for simple tasks like CartPole.
# Training control parameters
max_env_steps: int = 1_000_000 # Maximum environment steps
check_point_interval: int = 10_000 # Checkpoint interval
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/cartpole.py
:language: python
:start-after: docs-start: cartpole-skrl-config
:end-before: docs-end: cartpole-skrl-config
```
### Different Backend Configurations
**Key Configuration Notes:**
```python
# JAX backend specific configuration
@registry.rlcfg("my-task", backend="jax")
@dataclass
class MyTaskJAXCfg(PPOCfg):
# JAX optimized configuration
learning_rate: float = 2e-4 # JAX backend uses smaller learning rate
mini_batches: int = 4 # JAX supports large batches, fewer mini-batches
learning_epochs: int = 4 # Training rounds
num_envs: int = 2048 # More parallel environments
- **Network Architecture**: `hiddens=[32, 32]` - CartPole is a simple task, a small network is sufficient (default: `[256, 128, 64]`)
- **Training Epochs**: `learning_epochs=5` - Higher than default value 2, ensuring thorough learning
- **Mini-batches**: `mini_batches=4` - Fewer than default 32, suitable for simple tasks
- **Training Duration**: `timesteps=5000` - Sufficient for CartPole (default: 10000)
- **All Parameters**: All parameters from parent classes are explicitly specified, no hidden defaults
# Torch backend specific configuration
@registry.rlcfg("my-task", backend="torch")
@dataclass
class MyTaskTorchCfg(PPOCfg):
# Torch optimized configuration
learning_rate: float = 1e-4 # Torch backend uses even smaller learning rate
mini_batches: int = 32 # Torch needs more mini-batches
learning_epochs: int = 2 # Fewer training rounds
num_envs: int = 1024 # Fewer parallel environments
For complete source code, see: [`motrix_rl/src/motrix_rl/tasks/cartpole.py`](https://github.com/Motphys/motrix-lab/blob/main/motrix_rl/src/motrix_rl/tasks/cartpole.py)
### RSLRL Configuration (RslrlCfg)
RSLRL is another high-performance reinforcement learning library, specifically designed for complex control tasks like quadruped robots.
#### Complete Configuration Example
The following is the actual `CartPoleRslrlPpo` configuration, demonstrating complete explicit parameter filling. This configuration uses a smaller network `[32, 32]` suitable for simple tasks like CartPole.
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/cartpole.py
:language: python
:start-after: docs-start: cartpole-rslrl-config
:end-before: docs-end: cartpole-rslrl-config
```
### Complete Configuration Example
**Key Configuration Notes:**
```python
@dataclass
class CompletePPOConfig(PPOCfg):
"""
Complete reinforcement learning training configuration example
Contains all configuration parameters from basic to advanced
"""
- **Network Architecture**: `hidden_dims=[32, 32]` - CartPole is a simple task, a small network is sufficient (default: `[256, 128, 64]`)
- **Training Iterations**: `max_iterations=300` - Total of 300 training iterations
- **Steps per Environment**: `num_steps_per_env=16` - Number of steps to collect per environment
- **Learning Rate**: `learning_rate=5.0e-4` - Learning rate setting
- **Entropy Coefficient**: `entropy_coef=5e-3` - Entropy coefficient for exploration
- **All Parameters**: All parameters from parent classes are explicitly specified, no hidden defaults
# ===== Basic Training Parameters =====
seed: Optional[int] = None # Random seed
num_envs: int = 2048 # Number of parallel environments during training
play_num_envs: int = 16 # Number of parallel environments during evaluation
max_env_steps: int = 2_048_000 # Maximum training steps
check_point_interval: int = 1000 # Checkpoint save interval
For detailed RSLRL configuration options and default values, refer to:
# ===== PPO Algorithm Core Parameters =====
learning_rate: float = 3e-4 # Learning rate
rollouts: int = 32 # Experience replay rounds
learning_epochs: int = 2 # Number of training rounds per update
mini_batches: int = 32 # Number of mini-batches
discount_factor: float = 0.99 # Discount factor
lambda_param: float = 0.95 # GAE parameter
grad_norm_clip: float = 1.0 # Gradient clipping
# ===== PPO Clipping Parameters =====
ratio_clip: float = 0.2 # PPO clipping ratio
value_clip: float = 0.2 # Value clipping
clip_predicted_values: bool = True # Clip predicted values
# ===== Loss Function Parameters =====
entropy_loss_scale: float = 0.0 # Entropy loss coefficient
value_loss_scale: float = 2.0 # Value loss coefficient
kl_threshold: float = 0 # KL divergence threshold
# ===== Learning Rate Scheduler =====
learning_rate_scheduler_kl_threshold: float = 0.008 # Adaptive learning rate KL threshold
# ===== Network Architecture Configuration =====
# Small network (suitable for simple tasks like CartPole)
# policy_hidden_layer_sizes: tuple[int, ...] = (128, 64)
# value_hidden_layer_sizes: tuple[int, ...] = (128, 64)
# Medium network (default configuration, suitable for most tasks)
policy_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64)
value_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64)
# Large network (suitable for complex tasks like robot control)
# policy_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
# value_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
# ===== Network Sharing Configuration =====
share_policy_value_features: bool = True # Policy and value networks share feature extraction layers
# ===== Training Control Parameters =====
random_timesteps: int = 0 # Random timesteps
learning_starts: int = 0 # Timesteps to start learning
time_limit_bootstrap: bool = True # Time limit bootstrap
# ===== Reward Shaping =====
rewards_shaper_scale: float = 1.0 # Reward scaling factor
```
- `motrix_rl/rslrl/cfg.py`: Configuration class definitions
- `motrix_rl/template/rslrl_config.yaml`: YAML reference template
## Configuration Usage Methods
### 1. Default Configuration Usage
```bash
# Use configuration given in code
# Use configuration given in code (default: SKRL framework)
uv run scripts/train.py --env my-task
# Specify training backend, system will automatically select corresponding backend configuration
uv run scripts/train.py --env my-task --train-backend jax
uv run scripts/train.py --env my-task --train-backend torch
# Specify RL framework
uv run scripts/train.py --env my-task --rllib skrl
uv run scripts/train.py --env my-task --rllib rslrl
# Specify training backend for SKRL, system will automatically select corresponding backend configuration
uv run scripts/train.py --env my-task --rllib skrl --train-backend jax
uv run scripts/train.py --env my-task --rllib skrl --train-backend torch
```
### 2. Command Line Parameter Override
@@ -144,88 +83,10 @@ uv run scripts/train.py --env my-task --train-backend torch
```bash
# Override supported command line parameters
uv run scripts/train.py --env my-task \
--rllib skrl \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
# System will automatically select JAX backend configuration
```
### 3. Configuration Priority
System selects configuration in the following priority:
1. **Backend-specific configuration**: If there is a configuration decorated with `@rlcfg(env_name, backend="jax/torch")`
2. **Universal configuration**: If there is a configuration decorated with `@rlcfg(env_name)` (no backend parameter)
3. **Configuration override**: Command line `cfg_override` parameter
For example:
```python
# Highest priority - backend specific configuration
@rlcfg("my-task", backend="jax")
@dataclass
class MyTaskJAXCfg(PPOCfg):
mini_batches: int = 4
# Second priority - universal configuration
@rlcfg("my-task")
@dataclass
class MyTaskRLCfg(PPOCfg):
mini_batches: int = 32
# When using --train-backend jax, system will select MyTaskJAXCfg
# When using --train-backend torch, system will select MyTaskRLCfg
```
#### User Configurable Parameters
| MotrixLab Configuration Class | SKRL Framework Parameter | Description |
| -------------------------------------- | --------------------------------------------- | ----------------------------------- |
| `learning_rate` | `learning_rate` | Learning rate |
| `rollouts` | `rollouts` | Experience replay rounds |
| `learning_epochs` | `learning_epochs` | Training rounds |
| `mini_batches` | `mini_batches` | Number of mini-batches |
| `discount_factor` | `discount_factor` | Discount factor |
| `grad_norm_clip` | `grad_norm_clip` | Gradient clipping |
| `lambda_param` | `lambda` | GAE parameter |
| `ratio_clip` | `ratio_clip` | PPO clipping ratio |
| `value_clip` | `value_clip` | Value clipping |
| `clip_predicted_values` | `clip_predicted_values` | Clip predicted values |
| `entropy_loss_scale` | `entropy_loss_scale` | Entropy loss coefficient |
| `value_loss_scale` | `value_loss_scale` | Value loss coefficient |
| `kl_threshold` | `kl_threshold` | KL divergence threshold |
| `random_timesteps` | `random_timesteps` | Random timesteps |
| `learning_starts` | `learning_starts` | Learning start timesteps |
| `time_limit_bootstrap` | `time_limit_bootstrap` | Time limit bootstrap |
| `learning_rate_scheduler_kl_threshold` | `learning_rate_scheduler_kwargs.kl_threshold` | Adaptive learning rate KL threshold |
| `check_point_interval` | `experiment.write_interval` | Log write interval |
| `check_point_interval` | `experiment.checkpoint_interval` | Checkpoint save interval |
| `rewards_shaper_scale` | `rewards_shaper` | Reward scaling function |
#### Preprocessor Parameters
| SKRL Framework Parameter | Type | Description |
| ------------------------ | --------------------- | ------------------- |
| `state_preprocessor` | RunningStandardScaler | State normalization |
| `value_preprocessor` | RunningStandardScaler | Value normalization |
### Configuration Hierarchy Summary
```
User Configuration Class (PPOCfg)
↓ Backend specific selection
Backend Configuration (JAX/Torch)
↓ Parameter mapping
SKRL Framework Configuration Dictionary
↓ Pass to
PPO Agent
↓ Execute
Reinforcement Learning Training
```
This design allows users to:
1. Control complex training parameters through simple configuration classes
2. Configure different parameters for different training backends to achieve optimal performance
3. Maintain full compatibility with the SKRL framework

34
docs/source/zh_CN/index.md
View File

@@ -7,8 +7,8 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:::{grid-item-card}
```{video} _static/videos/cartpole.mp4
:poster: _static/images/poster/cartpole.jpg
```{video} _static/videos/shadow_hand_repose.mp4
:poster: _static/images/poster/shadow_hand_repose.jpg
:nocontrols:
:autoplay:
:playsinline:
@@ -147,8 +147,8 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:::{grid-item-card}
```{video} _static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
```{video} _static/videos/cartpole.mp4
:poster: _static/images/poster/cartpole.jpg
:nocontrols:
:autoplay:
:playsinline:
@@ -185,6 +185,32 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_quadruped_fetch.mp4
:poster: _static/images/poster/dm_quadruped_fetch.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_quadruped_run.mp4
:poster: _static/images/poster/dm_quadruped_run.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
::::

43
docs/source/zh_CN/user_guide/demo/acrobot.md
View File

@@ -103,45 +103,18 @@ uv run scripts/play.py --env acrobot --policy runs/acrobot/nn/best_policy.pickle
### 环境配置
```python
@dataclass
class AcrobotEnvCfg(EnvCfg):
model_file: str = ".../acrobot.xml" # MJCF 模型
max_episode_seconds: float = 10.0
sim_dt: float = 0.01
ctrl_dt: float = 0.02
reset_noise_scale: float = 0.1
render_spacing: float = 2.0
```{literalinclude} ../../../../motrix_envs/src/motrix_envs/basic/acrobot/cfg.py
:language: python
:start-after: '# -- docs-tag-start: acrobot-env-cfg --'
:end-before: '# -- docs-tag-end: acrobot-env-cfg --'
```
### 训练配置（PPO 示例）
```python
@rlcfg("acrobot", backend="jax")
@dataclass
class AcrobotPPO(PPOCfg):
max_env_steps: int = 60_000_000
check_point_interval: int = 500
# 覆盖 PPO 配置
policy_hidden_layer_sizes: tuple[int, ...] = (32, 32)
value_hidden_layer_sizes: tuple[int, ...] = (32, 32)
rollouts: int = 64
learning_epochs: int = 5
mini_batches: int = 8
learning_rate: float = 3e-4
grad_norm_clip: float = 0.1
clip_predicted_values: bool = False
value_clip: float = 10.0
entropy_loss_scale: float = 0.1
learning_rate_scheduler_kl_threshold: float = 0.02
discount_factor: float = 0.995
lambda_param: float = 0.97
ratio_clip: float = 0.2
value_loss_scale: float = 0.5
random_timesteps: int = 0
learning_starts: int = 0
kl_threshold: float = 0.03
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/acrobot.py
:language: python
:start-after: '# -- docs-tag-start: acrobot-train-cfg --'
:end-before: '# -- docs-tag-end: acrobot-train-cfg --'
```
---

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

@@ -0,0 +1,199 @@
# 线性二次调节器
LQR（Linear Quadratic Regulator，线性二次调节器）是一个典型的连续控制与稳定化任务。本仓库当前提供两个变体：
- `dm-lqr-2-1`：两个小球通过绳索相连，只有最后一个小球受控。
- `dm-lqr-6-2`：六个小球串联成链，只有最后两个小球受控。
智能体需要在尽量小的控制代价下，将整条系统拉回中心附近并稳定停下。
```{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%
```
---
## 任务描述
这两个任务都可以看作一维弹簧-阻尼链的平衡控制问题。每个小球只有一个沿 x 轴的滑动自由度，相邻小球之间通过“绳”形成耦合，系统同时受到：
- 球体自身阻尼
- 相邻球体之间的弹簧力和相对速度阻尼
- 指向原点的中心回复力
- 控制输入作用在末端受控自由度上
其中：
- `dm-lqr-2-1` 更简单，适合先验证是否能学会稳定驻点。
- `dm-lqr-6-2` 需要把控制作用沿链条传递到更远的非受控球，难度更高。
---
## 动作空间（Action Space）
### dm-lqr-2-1
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (1,), float32)` |
| **维度** | 1 |
| 序号 | 动作含义 | 最小值 | 最大值 | 对应 XML 关节 |
| ---: | ------------------------ | :----: | :----: | :-----------: |
| 0 | 作用在末端球上的控制输入 | -1.0 | 1.0 | `q1` |
### dm-lqr-6-2
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (2,), float32)` |
| **维度** | 2 |
| 序号 | 动作含义 | 最小值 | 最大值 | 对应 XML 关节 |
| ---: | ---------------------- | :----: | :----: | :-----------: |
| 0 | 倒数第二个球的控制输入 | -1.0 | 1.0 | `q4` |
| 1 | 最后一个球的控制输入 | -1.0 | 1.0 | `q5` |
---
## 观察空间
观察由所有位置 `qpos` 和速度 `qvel` 直接拼接而成。
### dm-lqr-2-1
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (4,), float32)` |
| **维度** | 4 |
| 序号 | 观察量 | 含义 |
| ---: | ------ | --------------- |
| 0 | `q0` | 第 1 个球的位置 |
| 1 | `q1` | 第 2 个球的位置 |
| 2 | `dq0` | 第 1 个球的速度 |
| 3 | `dq1` | 第 2 个球的速度 |
### dm-lqr-6-2
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (12,), float32)` |
| **维度** | 12 |
前 6 维为 `q0 ~ q5`，后 6 维为 `dq0 ~ dq5`。
---
## 奖励函数设计
当前实现的奖励由状态代价、速度代价、控制代价、成功奖励和越界惩罚组成：
```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
```
直观上：
- 离中心越远，奖励越低
- 速度越大，奖励越低
- 控制越激进，奖励越低
- 当系统进入“接近原点且速度足够小”的稳定区域时，会得到成功奖励
- 越界时会受到额外惩罚
---
## 初始状态
每次重置时：
- 位置向量从随机方向采样，再归一化到固定范数
- 速度初始化为 0
当前配置中：
- `dm-lqr-2-1` 的初始位置范数约为 `0.8`
- `dm-lqr-6-2` 的初始位置范数约为 `1.0`
---
## Episode 终止条件
满足以下任一条件时，当前 episode 结束并重置：
- 达到成功条件：
位置范数小于成功距离阈值，且速度范数小于成功速度阈值
- 超出边界：
任意位置超过边界范围，或任意速度超过速度边界
- 状态非常接近零点平衡态
- 观察或动作中出现 `NaN`
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-lqr-2-1
uv run scripts/view.py --env dm-lqr-6-2
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-lqr-2-1
uv run scripts/train.py --env dm-lqr-6-2
```
### 3. 查看训练曲线
```bash
uv run tensorboard --logdir runs/dm-lqr-2-1
uv run tensorboard --logdir runs/dm-lqr-6-2
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-lqr-2-1
uv run scripts/play.py --env dm-lqr-6-2
```
---
## 预期训练结果
### dm-lqr-2-1
1. 受控球能够带动未受控球一起回到中心附近。
2. 两个球的位置和速度最终都收敛到较小范围内。
3. 策略不会长期停在偏离中心的静止点。
### dm-lqr-6-2
1. 最后两个受控球能够把整条链逐步拉回中心。
2. 链条传播过程中不会出现明显发散或持续振荡。
3. 成功率随训练推进逐步上升，越界率逐步下降。

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

@@ -0,0 +1,159 @@
# 质点环境
质点（Point Mass）环境是一个简单但基础的 2D 导航任务，智能体通过控制一个质点来到达目标位置。这个环境是强化学习概念和连续动作空间的优秀入门案例。
```{video} /_static/videos/point_mass.mp4
:poster: _static/images/poster/point_mass.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
质点环境是一个 2D 导航任务。智能体需要通过施加力来控制一个质点，使其移动到随机生成的目标位置。该任务要求智能体学习高效的导航策略，以最小的控制成本到达目标。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (2,), float32)` |
| **维度** | 2 |
动作对应如下：
| 序号 | 动作含义（施加的力） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | -------------------- | :----: | :----: | :-------------: |
| 0 | x 方向力 | -1 | 1 | `x_force` |
| 1 | y 方向力 | -1 | 1 | `y_force` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (9,), float32)` |
| **维度** | 9 |
质点环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| -------- | -------------------------- | ---- | ---- |
| **位置** | 质点的 x、y 坐标 | 2 | |
| **速度** | 质点的 x、y 方向速度 | 2 | |
| **目标** | 目标的 x、y 坐标 | 2 | |
| **距离** | 到目标的 x、y 方向距离向量 | 2 | |
| **距离** | 到目标的欧几里得距离 | 1 | |
---
## 奖励函数设计
质点环境的奖励函数由以下几个部分组成：
### 距离奖励
```python
# 指数距离奖励 - 离目标越近奖励越强
distance_reward = np.exp(-10 * dist_to_target)
```
### 目标到达和停留奖励
```python
# 到达目标的大额奖励
target_bonus = 100.0 * in_target
# 在目标内持续停留的奖励
continuous_reward = 30.0 * in_target
```
### 控制和路径优化
```python
# 在目标内时，距离目标中心越远的惩罚
center_penalty = np.where(in_target, 10.0 * dist_to_target, 0.0)
# 控制惩罚，鼓励平滑移动
control_penalty = 0.1 * vel_magnitude
# 路径优化奖励，鼓励直线移动
path_reward = 0.5 * direction_alignment
```
### 总奖励计算
```python
# 组合所有奖励组件
rwd = distance_reward + target_bonus + continuous_reward + path_reward - center_penalty - control_penalty
```
---
## 初始状态
- 质点位置在[-1.0, 1.0]范围内随机初始化
- 目标位置在[-1.5, 1.5]范围内随机初始化
- 质点速度初始化为 0
## Episode 终止条件
- 质点到达目标并在目标内停留 0.5 秒
- 模拟时间达到 10 秒
- 观测值出现异常数值（NaN）
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env point_mass
```
### 2. 开始训练
```bash
uv run scripts/train.py --env point_mass
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/point_mass
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env point_mass
```
---
## 预期训练结果
### 导航性能
1. 智能体学会直接向目标移动
2. 移动平滑，控制 effort 最小
3. 在 episode 持续时间内一致地到达目标
### 学习进度
1. 初始学习阶段迅速，智能体发现基本导航策略
2. 控制策略逐渐精细化
3. 在不同目标位置上表现稳定
### 行为特征
1. 朝向目标的高效路径规划
2. 平滑接近目标中心
3. 最小化过冲或振荡行为

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

@@ -0,0 +1,290 @@
# 四足机器人
四足机器人（Quadruped）是 DeepMind Control Suite 中的经典连续控制任务。在 MotrixLab 中，`motrix_envs/src/motrix_envs/basic/quadruped` 目录当前注册了四个可直接训练的任务：平地行走 `dm-quadruped-walk`、平地奔跑 `dm-quadruped-run`、复杂地形逃离 `dm-quadruped-escape`，以及平地推球到目标区 `dm-quadruped-fetch`。
## 任务预览
### 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%
```
## 任务概览
| 环境 ID | 任务目标 | 模型文件 | 目标速度 | 观察维度 |
| --------------------- | -------------------------------- | ---------------------- | -------- | -------- |
| `dm-quadruped-walk` | 在平地上稳定向前行走并保持朝向 | `quadruped_walk.xml` | 0.5 m/s | 54 |
| `dm-quadruped-run` | 在平地上高速奔跑并保持稳定姿态 | `quadruped_walk.xml` | 5.0 m/s | 54 |
| `dm-quadruped-escape` | 在起伏地形上尽快向外逃离原点区域 | `quadruped_escape.xml` | 3.0 m/s | 57 |
| `dm-quadruped-fetch` | 在平地上将球体推动到目标区域 | `quadruped_fetch.xml` | 2.0 m/s | 66 |
## 任务描述
Quadruped 是一个三维四足机器人任务。机器人主体由一个躯干和四条腿组成，每条腿具有偏航、抬升和伸展相关的控制能力。底层 XML 中定义了髋部、膝部和踝部的关节结构，而动作层采用每条腿 3 个执行器的耦合设计：
- `yaw`：控制腿部偏航
- `lift`：通过 tendon 耦合控制抬腿动作
- `extend`：通过 tendon 耦合控制腿部伸展/收缩
`walk` 与 `run` 使用相同的平地模型，区别主要在目标速度；`escape` 使用带高度场的 `quadruped_escape.xml`，要求机器人在崎岖地形中快速远离世界原点，同时保持躯干直立和运动稳定；`fetch` 使用 `quadruped_fetch.xml`，在场景中额外引入自由球体与目标区域，要求机器人先调整到合适站位，再将球向目标方向推进。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(low, high, (12,), float32)` |
| **维度** | 12 |
动作按照四条腿依次排列，每条腿包含 `yaw / lift / extend` 三个执行器：
| 序号 | 动作含义 | 最小值 | 最大值 | 对应执行器 |
| ---: | -------------- | :----: | :----: | -------------------- |
| 0 | 左前腿偏航控制 | -1.0 | 1.0 | `yaw_front_left` |
| 1 | 左前腿抬升控制 | -1.0 | 1.1 | `lift_front_left` |
| 2 | 左前腿伸展控制 | -0.8 | 0.8 | `extend_front_left` |
| 3 | 右前腿偏航控制 | -1.0 | 1.0 | `yaw_front_right` |
| 4 | 右前腿抬升控制 | -1.0 | 1.1 | `lift_front_right` |
| 5 | 右前腿伸展控制 | -0.8 | 0.8 | `extend_front_right` |
| 6 | 右后腿偏航控制 | -1.0 | 1.0 | `yaw_back_right` |
| 7 | 右后腿抬升控制 | -1.0 | 1.1 | `lift_back_right` |
| 8 | 右后腿伸展控制 | -0.8 | 0.8 | `extend_back_right` |
| 9 | 左后腿偏航控制 | -1.0 | 1.0 | `yaw_back_left` |
| 10 | 左后腿抬升控制 | -1.0 | 1.1 | `lift_back_left` |
| 11 | 左后腿伸展控制 | -0.8 | 0.8 | `extend_back_left` |
---
## 观察空间
| 环境 | 详细信息 |
| --------------------- | -------------------------------- |
| `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)` |
四种任务共享绝大部分本体观测，`escape` 额外增加了 3 维与原点相关的任务信息，`fetch` 额外增加了球体状态与目标位置信息：
| 部分 | 内容说明 | 维度 | `walk/run` | `escape` | `fetch` |
| ---------------------- | ------------------------------------ | ---- | ---------- | -------- | ------- |
| **egocentric dof pos** | 身体广义位置状态 | 16 | 是 | 是 | 是 |
| **egocentric dof vel** | 身体广义速度状态 | 16 | 是 | 是 | 是 |
| **actuator ctrl** | 当前 12 维执行器控制量 | 12 | 是 | 是 | 是 |
| **torso velocity** | 躯干线速度传感器 `velocimeter` | 3 | 是 | 是 | 是 |
| **torso upright** | 躯干朝上程度标量 | 1 | 是 | 是 | 是 |
| **imu** | IMU 加速度与角速度 | 6 | 是 | 是 | 是 |
| **origin** | 世界原点在本体坐标系中的位置 | 3 | 否 | 是 | 否 |
| **ball state** | 球相对本体的位置、相对线速度与角速度 | 9 | 否 | 否 | 是 |
| **target** | 目标区域在本体坐标系中的相对位置 | 3 | 否 | 否 | 是 |
环境还在 XML 中定义了足端力/力矩传感器与质心传感器，但当前实现的默认观测并未直接拼接这些量。
---
## 奖励函数设计
四种任务都以“保持躯干直立”为核心约束。实现中首先根据 `torso_upright` 计算 `upright_reward`，要求机器人主体保持接近直立的姿态。
### Walk / Run
`dm-quadruped-walk` 与 `dm-quadruped-run` 使用相同的奖励结构，只是目标速度不同：
- `walk` 追踪 `0.5 m/s`
- `run` 追踪 `5.0 m/s`
总奖励由以下部分组成：
```python
# 速度奖励：沿前向达到目标速度
# 姿态奖励：保持躯干直立
# 辅助奖励：高度、横向稳定性、朝向一致性、动作平滑性
# 惩罚项：后退、竖直速度过大、横滚/俯仰角速度过大、偏离默认姿态
total_reward = upright_reward * move_reward + shaping_terms - penalty_terms
```
其中主要 shaping/penalty 项包括：
- `height_reward`：鼓励躯干保持在站立高度附近
- `lateral_reward`：抑制过大的横向速度
- `heading_reward`：鼓励机器人持续朝 +X 方向前进
- `smooth_reward`：惩罚相邻时刻动作变化过大
- `backward_penalty`：抑制向后运动
- `lin_vel_z_penalty` 与 `ang_vel_xy_penalty`：抑制上下颠簸和躯干横滚/俯仰过大
- `similar_to_default_penalty`：鼓励关节姿态不要偏离默认站姿太远
### Escape
`dm-quadruped-escape` 在 locomotion 奖励基础上，增加了“尽快逃离原点区域”的任务项。该任务使用 `quadruped_escape.xml` 中的高度场地形：
```python
# 基础 locomotion 奖励
# + 远离原点奖励
# + 径向外逃速度奖励
total_reward = locomotion_reward + upright_reward * escape_reward + radial_speed_reward
```
额外任务项包括：
- `escape_reward`：根据机器人离原点区域的距离给出奖励
- `radial_speed_reward`：鼓励沿着“远离原点”的方向加速前进
这使得 `escape` 不仅要求机器人跑得快，还要求它在复杂地形上沿正确方向脱离中心区域。
### Fetch
`dm-quadruped-fetch` 使用一套面向“站位 + 推球”的专用 shaping 结构。当前实现中，奖励主要围绕机器人相对球和目标的几何关系展开：
```python
# 站位阶段：鼓励机器人先移动到球后方或侧后方
# 就绪阶段：鼓励机器人面向球、贴近球，并与球-目标连线对齐
# 推球阶段：鼓励球向目标方向滚动，并最终进入目标区域
# 惩罚项：反向移动、把球推离目标方向、腿部与球体过近
total_reward = stage_terms + ready_terms + push_terms - penalty_terms
```
其中主要项包括：
- `stage_move`：鼓励机器人朝当前阶段目标点移动
- `stage_reach`：鼓励机器人先到达球后方或侧后方的站位点
- `behind_align`：鼓励机器人站到球和目标连线的后方
- `face_ball`：鼓励机体朝向球体
- `near_ball`：鼓励机器人接近球体
- `ready` 与 `ready_gate`：综合站位、朝向和距离关系，决定是否进入更积极的推球阶段
- `fetch`：鼓励球体靠近目标区域
- `push`：鼓励球沿目标方向滚动
- `backward`：惩罚朝阶段目标的反向运动
- `away`：惩罚把球推向远离目标的方向
- `leg_ball`：惩罚腿部几何体与球体过近，减少“缠球”或挤球现象
此外，`fetch` 还通过 `stability` 门控同时约束躯干直立程度和机体高度，避免机器人通过明显跌倒或趴地的方式获取任务奖励。
---
## 初始状态
- `walk`、`run` 和 `escape` 都从 XML 中的默认四足站姿开始重置
- 这三个任务的根关节朝向固定为初始朝向，不随机旋转
- `fetch` 会在平面上随机初始化机器人位置与偏航角，同时随机初始化球体在地面上的位置
- 所有任务的关节速度与球体速度都初始化为 0
- 重置时会自动抬高机体，直到机器人与地面不发生初始穿透/碰撞
## Episode 终止条件
- 最大时长为 20 秒
- 当观测中出现 `NaN` 时，episode 终止
- `walk`、`run` 与 `escape` 当前实现没有单独设置“跌倒即终止”的条件
- `fetch` 在机体明显跌倒时会提前终止，条件包括躯干直立程度过低或躯干高度过低
- 当前实现尚未单独设置“球进入目标区域即成功终止”的条件
---
## 使用指南
### 1. 环境预览
```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. 开始训练
```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. 查看训练进度
```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. 测试训练结果
```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
```
---
## 预期训练结果
### 行走任务（`dm-quadruped-walk`）
1. 稳定维持接近 `0.5 m/s` 的平地前进速度
2. 身体姿态平稳，横向摆动较小
3. 能够持续保持朝 +X 方向行走
### 奔跑任务（`dm-quadruped-run`）
1. 速度提升到接近或超过 `5.0 m/s`
2. 步幅明显增大，动作具有更强爆发性
3. 高速运动下仍能保持较好的躯干稳定性
### 逃离任务（`dm-quadruped-escape`）
1. 能够快速离开原点附近区域
2. 在高度场地形上保持稳定落脚，不易侧翻
3. 运动方向以向外逃离为主，而不是原地打转
### 推球任务（`dm-quadruped-fetch`）
1. 能够先调整到球和目标连线的合理站位，而不是直接从侧面乱撞球
2. 能够稳定地将球向目标区域推进，而不是把球踢飞或持续推离目标
3. 在推球过程中保持较好的机体稳定性，减少趴地、翻倒和腿部缠球现象

23
docs/source/zh_CN/user_guide/demo/locomotion_unitree_go1.md
View File

@@ -61,25 +61,14 @@ GO1 环境的观测空间由以下部分组成（按顺序）：
GO1 的奖励函数是一个复杂的复合函数，包含多个组件：
```python
# 主要奖励组件
reward_config.scales = {
"tracking_lin_vel": 1.0, # 线速度跟踪奖励
"tracking_ang_vel": 0.5, # 角速度跟踪奖励
"feet_air_time": 1.0, # 足部空中时间奖励
"lin_vel_z": -2.0, # Z轴线速度惩罚
"ang_vel_xy": -0.05, # XY轴角速度惩罚
"orientation": -0.0, # 姿态偏离惩罚
"torques": -0.00001, # 力矩消耗惩罚
"dof_acc": -2.5e-7, # 关节加速度惩罚
"action_rate": -0.001, # 动作变化率惩罚
"hip_pos": -1, # 髋关节位置惩罚
"calf_pos": -0.3, # 腿关节位置惩罚
}
# 总奖励 = 加权组合以上所有项
```{literalinclude} ../../../../motrix_envs/src/motrix_envs/locomotion/go1/cfg.py
:language: python
:start-after: '# -- docs-tag-start: go1-reward-config --'
:end-before: '# -- docs-tag-end: go1-reward-config --'
```
_总奖励 = 加权组合以上所有项_
---
## 初始状态

23
docs/source/zh_CN/user_guide/demo/locomotion_unitree_go1_rough_terrain.md
View File

@@ -86,25 +86,14 @@ Unitree GO1 复杂地形行走环境是一个四足机器人强化学习任务
GO1 复杂地形的奖励函数采用多目标加权设计：
```python
# 核心奖励组件
reward_config.scales = {
"tracking_lin_vel": 1.0, # 线速度跟踪精度
"tracking_ang_vel": 0.5, # 角速度跟踪精度
"orientation": -0.0, # 身体姿态稳定性惩罚
"torques": -0.00001, # 关节力矩惩罚（能量效率）
"dof_acc": -2.5e-7, # 关节加速度惩罚
"action_rate": -0.001, # 动作平滑性惩罚
"feet_air_time": 1.0, # 足部空中时间奖励（鼓励大步幅）
"stand_still": 0.0, # 静止指令的关节位置保持
"hip_pos": -1, # 髋关节位置偏好
"calf_pos": -0.3, # 小腿关节位置偏好
"feet_stumble": -0.5, # 当足部横向触碰障碍物时施加惩罚
}
# 总奖励 = 加权组合以上所有项
```{literalinclude} ../../../../motrix_envs/src/motrix_envs/locomotion/go1/cfg.py
:language: python
:start-after: '# -- docs-tag-start: go1-reward-config --'
:end-before: '# -- docs-tag-end: go1-reward-config --'
```
_总奖励 = 加权组合以上所有项_
---
## 初始状态

28
docs/source/zh_CN/user_guide/demo/pendulum.md
View File

@@ -104,30 +104,18 @@ uv run scripts/play.py --env pendulum --policy runs/pendulum/nn/best_policy.pick
### 环境配置（示例）
```python
@dataclass
class PendulumEnvCfg(EnvCfg):
model_file: str = ".../pendulum.xml" # MJCF 模型，默认 gear=5
max_episode_seconds: float = 20.0
sim_dt: float = 0.0125
ctrl_dt: float = 0.025
```{literalinclude} ../../../../motrix_envs/src/motrix_envs/basic/pendulum/cfg.py
:language: python
:start-after: '# -- docs-tag-start: pendulum-env-cfg --'
:end-before: '# -- docs-tag-end: pendulum-env-cfg --'
```
### 训练配置（示例 PPO）
```python
@rlcfg("pendulum")
@dataclass
class PendulumPPO(PPOCfg):
seed: int = 42
max_env_steps: int = 10_000_000
num_envs: int = 1024
learning_rate: float = 3e-4
rollouts: int = 32
learning_epochs: int = 5
mini_batches: int = 4
policy_hidden_layer_sizes: tuple[int, ...] = (64, 64)
value_hidden_layer_sizes: tuple[int, ...] = (64, 64)
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/pendulum.py
:language: python
:start-after: '# -- docs-tag-start: pendulum-train-cfg --'
:end-before: '# -- docs-tag-end: pendulum-train-cfg --'
```
---

258
docs/source/zh_CN/user_guide/demo/rm65_open_cabinet.md Normal file
View File

@@ -0,0 +1,258 @@
# RM65 打开抽屉
## 概述
本文档描述 `rm65-open-cabinet` 操作任务环境。该环境使用 RM65 六自由度机械臂与并联夹爪，目标是接近柜体底部抽屉把手、完成抓取并将抽屉拉开。
```{video} /_static/videos/rm65_open_cabinet.mp4
:poster: _static/images/poster/rm65_open_cabinet.png
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 环境描述
该任务基于 RM65 机械臂和柜体抽屉场景构建。虽然环境注册名为 `rm65-open-cabinet`，但当前实际操作对象是柜体底部抽屉，代码中对应目标为 `drawer_bottom_handle` 与 `drawer_bottom_joint`。
### 机器人结构
RM65 机器人在该环境中包含以下主要部分：
- **基座（base_link）**：固定在工作台前方
- **6 个机械臂关节**：`joint_1` ~ `joint_6`
- **并联夹爪**：主驱动关节为 `gripper_Left_1_Joint`，其余 5 个夹爪关节通过联动关系同步
- **末端执行器（TCP）**：`gripper` 站点，用于计算末端与把手的相对位姿
- **手指接触点**：`left_finger_pad`、`right_finger_pad`，用于判定是否对准把手以及是否发生穿透
### 场景物体
- **柜体**：包含多个门和抽屉
- **目标把手**：`drawer_bottom_handle`
- **目标关节**：`drawer_bottom_joint`，滑动范围为 `0.0 ~ 0.4 m`
### 任务目标
机器人需要完成以下阶段：
1. **靠近把手**：将末端执行器移动到抽屉把手附近
2. **姿态对齐**：让夹爪姿态与把手姿态匹配
3. **稳定抓取**：闭合夹爪并连续保持抓取
4. **打开抽屉**：沿抽屉滑动方向将底部抽屉拉开
---
## 动作空间
动作空间为 `Box(-inf, inf, (7,), float32)`。
其中前 6 维控制机械臂关节目标，最后 1 维控制夹爪开合。
### 控制模式
- **机械臂**：默认使用 `joint_target` 模式，并启用归一化目标控制
机械臂动作会先裁剪到 `[-1, 1]`，再线性映射到各关节控制范围
- **夹爪**：默认使用 `binary` 模式
原始动作经过 Sigmoid 映射为闭合概率，再通过带迟滞的二值开关决定夹爪开闭
### 动作维度详细说明
| 编号 | 动作说明 | 原始输入范围 | 实际控制对象 |
| ---- | ------------ | ------------- | ---------------------- |
| 0 | 关节 1 目标 | `(-inf, inf)` | `joint_1` |
| 1 | 关节 2 目标 | `(-inf, inf)` | `joint_2` |
| 2 | 关节 3 目标 | `(-inf, inf)` | `joint_3` |
| 3 | 关节 4 目标 | `(-inf, inf)` | `joint_4` |
| 4 | 关节 5 目标 | `(-inf, inf)` | `joint_5` |
| 5 | 关节 6 目标 | `(-inf, inf)` | `joint_6` |
| 6 | 夹爪开合命令 | `(-inf, inf)` | `gripper_Left_1_Joint` |
### 控制约束
- 控制周期 `ctrl_dt = 0.025s`，对应 40Hz 控制频率
- 机械臂默认启用速度限制、加速度限制、动作延迟与执行器一阶滞后
- 训练时会在每个 episode 内随机化机械臂延迟、滞后、速度上限和加速度上限，以增强 sim2real 鲁棒性
- 夹爪使用迟滞阈值控制：
- 开启到闭合阈值：`0.78`
- 闭合到开启阈值：`0.62`
- 最小切换间隔：`0.25s`
---
## 观测空间
观测空间为 `Box(-inf, inf, (84,), float32)`，最终观测会裁剪到 `[-5, 5]`。
### 观测组成
观测由以下 4 部分拼接而成：
1. **关节位置（7 维）**
- 6 个机械臂关节
- 1 个夹爪主驱动关节
- 统一归一化到 `[-1, 1]`
2. **关节速度（7 维）**
- 使用相邻两帧关节位置差分估计
- 再除以 `2` 做缩放
3. **目标相对位姿（7 维）**
- 末端到把手的相对位置 `Δx, Δy, Δz`
- 把手相对末端的四元数姿态差
4. **动作历史（63 维）**
- 最近 `9` 步原始动作历史
- 每步 `7` 维，共 `9 × 7 = 63` 维
### 观测维度详细说明
| 编号范围 | 内容说明 | 维度 |
| -------- | -------------------------- | ---- |
| 0-6 | 归一化关节位置 | 7 |
| 7-13 | 关节速度 | 7 |
| 14-16 | 末端到把手的相对位置 | 3 |
| 17-20 | 末端到把手的相对姿态四元数 | 4 |
| 21-83 | 最近 9 步动作历史 | 63 |
### 观测噪声
该环境默认开启 sim2real 观测扰动，主要包括：
- 关节位置与速度噪声
- 把手位置与姿态噪声
- 把手观测偏置
- 把手观测随机丢帧，并在丢帧时保持上一帧结果
---
## 奖励函数
奖励函数采用分阶段复合设计，既鼓励接近与对齐，也鼓励稳定抓取和持续拉开抽屉。
### 主要奖励项
1. **距离奖励**
```python
dist_reward = 15.0 * (1 - tanh(distance / 0.4))
```
鼓励末端执行器持续靠近把手。
2. **姿态奖励**
基于末端姿态与把手姿态的四元数相似度计算，并仅在末端距离把手较近时生效。
3. **夹爪闭合奖励**
当末端距离小于 `0.035m` 且左右手指在把手上下方对齐时，闭合夹爪可获得正奖励；
否则闭合会受到惩罚。该项还会随夹爪闭合程度缩放。
4. **抽屉打开奖励**
```python
open_reward = (exp(open_dist) - 1.0) * 420.0
```
只有在满足“已经抓住或进入抓取阶段”且末端仍靠近把手时才生效。
5. **抽屉增量奖励**
对本步新增的打开距离给予额外奖励，鼓励稳定、连续地拉动抽屉。
6. **稳定抓取奖励**
当末端距离小于 `0.03m`、夹爪闭合比例大于 `0.7`，并持续保持 `6` 步后，环境认为已经抓住把手，并提供持续奖励。
7. **里程碑奖励**
- 抽屉打开超过 `0.15m` 时奖励 `35`
- 抽屉打开超过 `0.22m` 时额外奖励 `70`
### 惩罚项
1. **滑脱惩罚**
如果已经进入抓取阶段但后来失去抓取，同时抽屉已经被拉开，则施加额外惩罚。
2. **手指穿透惩罚**
当手指接触点穿过把手上下边界时施加惩罚，用于抑制不合理接触。
3. **夹爪频繁切换惩罚**
当末端靠近把手时，频繁在开合之间切换会触发惩罚。
4. **动作变化惩罚**
对相邻两步动作差的平方和施加惩罚。
5. **关节速度惩罚**
对关节速度平方和施加惩罚，后期训练权重更高。
6. **终止惩罚**
当触发终止条件时，额外施加 `-10.0` 惩罚。
---
## 初始状态
### 机器人初始化
- 机械臂默认关节初值为零位姿
- 夹爪默认处于张开状态
- 复位时默认不添加关节角随机噪声
- 所有关节速度初始化为零
### 场景初始化
- 目标抽屉初始为完全关闭状态
- 抽屉与柜体其余部分固定在场景默认位置
### 随机化内容
每次 reset 会重新采样部分 sim2real 参数，包括：
- 机械臂动作延迟
- 机械臂执行器滞后
- 机械臂速度/加速度上限
- 把手观测偏置
---
## Episode 终止条件
以下任一条件满足时，episode 会提前终止：
1. **TCP 落到把手后方过多**
- 终止阈值：`tcp_x - handle_x < -0.02`
2. **关节速度过大**
- 任意机器人关节速度绝对值超过 `3.93 rad/s`
此外，环境最大时长为 `30s`。
---
## 使用方法
### 训练
```bash
uv run scripts/train.py --env rm65-open-cabinet --train-backend torch
```
### 策略评估
```bash
uv run scripts/play.py --env rm65-open-cabinet
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/rm65_open_cabinet
```

3
docs/source/zh_CN/user_guide/getting_started/installation.md
View File

@@ -89,4 +89,7 @@ uv sync --all-packages --extra skrl-jax
# 安装 SKRL PyTorch
uv sync --all-packages --extra skrl-torch
# 安装 RSLRL（仅支持 PyTorch）
uv sync --all-packages --extra rslrl
```

4
docs/source/zh_CN/user_guide/index.md
View File

@@ -34,14 +34,18 @@ demo/dm_walker
demo/dm_cheetah
demo/dm_hopper
demo/dm_reacher
demo/dm_lqr
demo/dm_finger
demo/dm_humanoid
demo/dm_quadruped
demo/bring_ball
demo/dm_point_mass
demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c
demo/franka_lift_cube
demo/franka_open_cabinet
demo/rm65_open_cabinet
demo/shadow_hand_repose
```

15
docs/source/zh_CN/user_guide/tutorial/basic_frame.md
View File

@@ -16,6 +16,7 @@ MotrixLab/
│ └── registry.py # 环境注册系统
├── motrix_rl/ # 训练层：RL算法和配置
│ ├── skrl/ # SKRL框架集成（JAX/PyTorch）
│ ├── rslrl/ # RSLRL框架集成（PyTorch）
│ ├── base.py # RL配置基类
│ └── registry.py # RL配置注册系统
└── scripts
@@ -34,7 +35,7 @@ MotrixLab/
│
▼
┌─────────────────────────────────────────────────────────────────┐
│ 训练算法层 (SKRL) │
│ 训练算法层 (SKRL / RSLRL) │
│ PPO训练器 │ 网络架构 │ 优化器 │
└─────────────────────────────────────────────────────────────────┘
│
@@ -128,13 +129,15 @@ train.py --env cartpole
- **奖励配置**影响学习信号（奖励权重、计算方式等）
- **训练配置**控制算法行为（网络结构、学习率、批次大小等）
## 多后端支持
## 多框架支持
MotrixLab 的分层设计天然支持多种后端：
MotrixLab 的分层设计天然支持多种 RL 框架：
- **仿真后端**：MotrixSim
- **训练后端**：JAX 和 PyTorch，支持 GPU 加速
- **算法框架**：主要集成 SKRL，易于扩展其他算法
- **仿真后端**：MotrixSim（CPU）
- **训练框架**：
- **SKRL**：支持 JAX 和 PyTorch 后端，支持 GPU 加速
- **RSLRL**：支持 PyTorch 后端，支持 GPU 加速
- **框架选择**：使用 `--rllib` 参数在 `skrl`（默认）和 `rslrl` 之间选择
## 设计优势

45
docs/source/zh_CN/user_guide/tutorial/training_and_result.md
View File

@@ -7,41 +7,62 @@
### 基本训练命令
```bash
# 使用默认参数训练
# 使用默认参数训练（SKRL 框架）
uv run scripts/train.py --env cartpole
# 指定 RL 框架
uv run scripts/train.py --env cartpole --rllib skrl
uv run scripts/train.py --env cartpole --rllib rslrl
# 指定仿真后端
uv run scripts/train.py --env cartpole --sim-backend np
# 指定训练后端
uv run scripts/train.py --env cartpole --train-backend jax
uv run scripts/train.py --env cartpole --train-backend torch
# 指定训练后端（仅 SKRL）
uv run scripts/train.py --env cartpole --rllib skrl --train-backend jax
uv run scripts/train.py --env cartpole --rllib skrl --train-backend torch
```
### 高级训练配置
```bash
# 自定义训练参数
# 使用 SKRL 自定义训练参数
uv run scripts/train.py --env cartpole \
--rllib skrl \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
# 使用 RSLRL 自定义训练参数
uv run scripts/train.py --env cartpole \
--rllib rslrl \
--num-envs 1024 \
--sim-backend np
# 注意：学习率等参数需要通过配置文件或代码覆盖设置
# 启用渲染监控训练过程
uv run scripts/train.py --env cartpole --render
```
### 不同框架配置
系统支持不同的 RL 框架，具有不同的配置系统：
- **SKRL 框架**：支持 JAX 和 PyTorch 训练后端，可通过 Python 数据类为每个后端配置参数
- **RSLRL 框架**：支持 PyTorch 后端，通过 Python 数据类（RslrlCfg）进行配置
对于 SKRL，系统支持为不同训练后端（JAX/Torch）配置不同的强化学习参数。例如：
### 支持的命令行参数
| 参数 | 说明 | 默认值 |
| ----------------- | -------------------- | ---------- |
| `--env` | 环境名称 | `cartpole` |
| `--sim-backend` | 仿真后端 (np) | 自动选择 |
| `--train-backend` | 训练后端 (jax/torch) | 自动选择 |
| `--num-envs` | 并行环境数量 | 2048 |
| `--render` | 启用渲染 | False |
| 参数 | 说明 | 默认值 |
| ----------------- | ----------------------------- | ---------- |
| `--env` | 环境名称 | `cartpole` |
| `--rllib` | RL 框架 (skrl/rslrl) | `skrl` |
| `--sim-backend` | 仿真后端 (np) | 自动选择 |
| `--train-backend` | 训练后端 (jax/torch，仅 SKRL) | 自动选择 |
| `--num-envs` | 并行环境数量 | 2048 |
| `--render` | 启用渲染 | False |
> **注意**: 其他参数如学习率、网络结构等需要通过单独文件设置。

208
docs/source/zh_CN/user_guide/tutorial/training_environment_config.md
View File

@@ -2,85 +2,80 @@
MotrixLab 提供了灵活的配置系统，允许用户自定义强化学习训练参数。本节介绍如何配置训练环境和强化学习算法参数。
## RL 训练配置 (PPOCfg)
## RL 训练配置
MotrixLab 支持多个 RL 框架，具有不同的配置系统：
- **SKRL 框架**：使用 Python 数据类配置（SkrlCfg）
- **RSLRL 框架**：使用 Python 数据类配置（RslrlCfg）
### SKRL 配置 (SkrlCfg)
训练配置定义了基于 PPO 算法的强化学习算法的参数。MotrixLab 现在支持为不同训练后端配置不同的参数。
### 完整配置示例
#### 完整配置示例
```python
@dataclass
class CompletePPOConfig(PPOCfg):
"""
完整的强化学习训练配置示例
包含了从基础到高级的所有配置参数
"""
以下是 `CartPoleSkrlPpo` 的实际配置，展示了完整的显式参数填充方法。该配置使用较小的网络 `[32, 32]`，适合 CartPole 这样的简单任务。
# ===== 基础训练参数 =====
seed: Optional[int] = None # 随机种子
num_envs: int = 2048 # 训练时并行环境数量
play_num_envs: int = 16 # 评估时并行环境数量
max_env_steps: int = 2_048_000 # 最大训练步数
check_point_interval: int = 1000 # 检查点保存间隔
# ===== PPO算法核心参数 =====
learning_rate: float = 3e-4 # 学习率
rollouts: int = 32 # 经验回放轮数
learning_epochs: int = 2 # 每次更新的训练轮数
mini_batches: int = 32 # 小批量数量
discount_factor: float = 0.99 # 折扣因子
lambda_param: float = 0.95 # GAE参数
grad_norm_clip: float = 1.0 # 梯度裁剪
# ===== PPO裁剪参数 =====
ratio_clip: float = 0.2 # PPO裁剪比率
value_clip: float = 0.2 # 价值裁剪
clip_predicted_values: bool = True # 裁剪预测值
# ===== 损失函数参数 =====
entropy_loss_scale: float = 0.0 # 熵损失系数
value_loss_scale: float = 2.0 # 价值损失系数
kl_threshold: float = 0 # KL散度阈值
# ===== 学习率调度器 =====
learning_rate_scheduler_kl_threshold: float = 0.008 # 自适应学习率KL阈值
# ===== 网络架构配置 =====
# 小型网络（适合简单任务如 CartPole）
# policy_hidden_layer_sizes: tuple[int, ...] = (128, 64)
# value_hidden_layer_sizes: tuple[int, ...] = (128, 64)
# 中型网络（默认配置，适合大部分任务）
policy_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64)
value_hidden_layer_sizes: tuple[int, ...] = (256, 128, 64)
# 大型网络（适合复杂任务如机器人控制）
# policy_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
# value_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
# ===== 网络共享配置 =====
share_policy_value_features: bool = True # 策略和价值网络共享特征提取层
# ===== 训练控制参数 =====
random_timesteps: int = 0 # 随机步数
learning_starts: int = 0 # 开始学习的步数
time_limit_bootstrap: bool = True # 时间限制引导
# ===== 奖励整形 =====
rewards_shaper_scale: float = 1.0 # 奖励缩放因子
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/cartpole.py
:language: python
:start-after: docs-start: cartpole-skrl-config
:end-before: docs-end: cartpole-skrl-config
```
**关键配置说明：**
- **网络架构**: `hiddens=[32, 32]` - CartPole 是简单任务，使用小网络即可（默认: `[256, 128, 64]`）
- **训练轮数**: `learning_epochs=5` - 比默认值 2 更高，确保充分学习
- **小批量数量**: `mini_batches=4` - 比默认值 32 更少，适合简单任务
- **训练时长**: `timesteps=5000` - 对于 CartPole 来说已经足够（默认: 10000）
- **所有参数**: 从父类继承的所有参数都被显式指定，无隐藏默认值
完整的源代码请参考: [`motrix_rl/src/motrix_rl/tasks/cartpole.py`](https://github.com/Motphys/motrix-lab/blob/main/motrix_rl/src/motrix_rl/tasks/cartpole.py)
### RSLRL 配置 (RslrlCfg)
RSLRL 是另一个高性能强化学习库，专门用于四足机器人等复杂控制任务。
#### 完整配置示例
以下是 `CartPoleRslrlPpo` 的实际配置，展示了完整的显式参数填充方法。该配置使用较小的网络 `[32, 32]`，适合 CartPole 这样的简单任务。
```{literalinclude} ../../../../motrix_rl/src/motrix_rl/tasks/cartpole.py
:language: python
:start-after: docs-start: cartpole-rslrl-config
:end-before: docs-end: cartpole-rslrl-config
```
**关键配置说明：**
- **网络架构**: `hidden_dims=[32, 32]` - CartPole 是简单任务，使用小网络即可（默认: `[256, 128, 64]`）
- **训练迭代**: `max_iterations=300` - 总共训练 300 次迭代
- **每轮步数**: `num_steps_per_env=16` - 每个环境收集 16 步
- **学习率**: `learning_rate=5.0e-4` - 学习率设置
- **熵系数**: `entropy_coef=5e-3` - 熵系数，用于探索
- **所有参数**: 从父类继承的所有参数都被显式指定，无隐藏默认值
详细的 RSLRL 配置选项和默认值，请参考：
- `motrix_rl/rslrl/cfg.py`：配置类定义
- `motrix_rl/template/rslrl_config.yaml`：YAML 参考模板
## 配置使用方法
### 1. 默认配置使用
```bash
# 使用代码中给定的配置
# 使用代码中给定的配置（默认：SKRL 框架）
uv run scripts/train.py --env my-task
# 指定训练后端，系统会自动选择对应的后端配置
uv run scripts/train.py --env my-task --train-backend jax
uv run scripts/train.py --env my-task --train-backend torch
# 指定 RL 框架
uv run scripts/train.py --env my-task --rllib skrl
uv run scripts/train.py --env my-task --rllib rslrl
# 指定 SKRL 的训练后端，系统会自动选择对应的后端配置
uv run scripts/train.py --env my-task --rllib skrl --train-backend jax
uv run scripts/train.py --env my-task --rllib skrl --train-backend torch
```
### 2. 命令行参数覆盖
@@ -88,91 +83,10 @@ uv run scripts/train.py --env my-task --train-backend torch
```bash
# 覆盖支持的命令行参数
uv run scripts/train.py --env my-task \
--rllib skrl \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
# 系统会自动选择JAX后端对应的配置
```
### 3. 配置优先级
系统按以下优先级选择配置：
1. **后端特定配置**: 如果存在 `@rlcfg(env_name, backend="jax/torch")` 装饰的配置
2. **通用配置**: 如果存在 `@rlcfg(env_name)` 装饰的配置（无 backend 参数）
例如：
```python
# 最高优先级 - 后端特定配置
@rlcfg("my-task", backend="jax")
@dataclass
class MyTaskJAXCfg(PPOCfg):
mini_batches: int = 4
# 次优先级 - 通用配置
@rlcfg("my-task")
@dataclass
class MyTaskRLCfg(PPOCfg):
mini_batches: int = 32
# 当使用 --train-backend jax 时，系统会选择 MyTaskJAXCfg
# 当使用 --train-backend torch 时，系统会选择 MyTaskRLCfg
```
## SKRL 框架配置映射
在 MotrixLab 中，用户通过 `PPOCfg` 配置类设置参数，这些参数会被映射到 SKRL 框架的配置字典中。
### 用户可配置参数
| MotrixLab 配置类 | SKRL 框架参数 | 说明 |
| -------------------------------------- | --------------------------------------------- | -------------------- |
| `learning_rate` | `learning_rate` | 学习率 |
| `rollouts` | `rollouts` | 经验回放轮数 |
| `learning_epochs` | `learning_epochs` | 训练轮数 |
| `mini_batches` | `mini_batches` | 小批量数量 |
| `discount_factor` | `discount_factor` | 折扣因子 |
| `grad_norm_clip` | `grad_norm_clip` | 梯度裁剪 |
| `lambda_param` | `lambda` | GAE 参数 |
| `ratio_clip` | `ratio_clip` | PPO 裁剪比率 |
| `value_clip` | `value_clip` | 价值裁剪 |
| `clip_predicted_values` | `clip_predicted_values` | 裁剪预测值 |
| `entropy_loss_scale` | `entropy_loss_scale` | 熵损失系数 |
| `value_loss_scale` | `value_loss_scale` | 价值损失系数 |
| `kl_threshold` | `kl_threshold` | KL 散度阈值 |
| `random_timesteps` | `random_timesteps` | 随机步数 |
| `learning_starts` | `learning_starts` | 开始学习的步数 |
| `time_limit_bootstrap` | `time_limit_bootstrap` | 时间限制引导 |
| `learning_rate_scheduler_kl_threshold` | `learning_rate_scheduler_kwargs.kl_threshold` | 自适应学习率 KL 阈值 |
| `check_point_interval` | `experiment.write_interval` | 日志写入间隔 |
| `check_point_interval` | `experiment.checkpoint_interval` | 检查点保存间隔 |
| `rewards_shaper_scale` | `rewards_shaper` | 奖励缩放函数 |
### 预处理器参数
| SKRL 框架参数 | 类型 | 说明 |
| -------------------- | --------------------- | ---------- |
| `state_preprocessor` | RunningStandardScaler | 状态标准化 |
| `value_preprocessor` | RunningStandardScaler | 价值标准化 |
### 配置层次总结
```
用户配置类 (PPOCfg)
↓ 后端特定选择
后端配置 (JAX/Torch)
↓ 参数映射
SKRL 框架配置字典
↓ 传递给
PPO Agent
↓ 执行
强化学习训练
```
这种设计允许用户：
1. 通过简单的配置类来控制复杂的训练参数
2. 为不同训练后端配置不同的参数以获得最佳性能
3. 保持与 SKRL 框架的完全兼容性

4
motrix_envs/pyproject.toml
View File

@@ -4,12 +4,12 @@ build-backend = "uv_build"
[project]
name = "motrix-envs"
version = "0.2.0"
version = "0.3.0"
description = "Robot simulation environment library based on MotrixSim providing multi-task RL environments."
authors = [{ name = "Motphys", email = "developers@motphys.com" }]
requires-python = "==3.10.*"
readme = "README.md"
license = "Apache-2.0"
dependencies = [
"motrixsim>=0.6.0b1",
"motrixsim>=0.7.0",
]

3
motrix_envs/src/motrix_envs/basic/__init__.py
View File

@@ -21,8 +21,11 @@ from . import ( # noqa: F401 import to register envs
finger,
hopper,
humanoid,
lqr,
manipulator,
pendulum,
point_mass,
quadruped,
reacher,
walker,
)

4
motrix_envs/src/motrix_envs/basic/acrobot/cfg.py
View File

@@ -22,6 +22,7 @@ from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/acrobot.xml"
# -- docs-tag-start: acrobot-env-cfg --
@registry.envcfg("acrobot")
@dataclass
class AcrobotEnvCfg(EnvCfg):
@@ -31,3 +32,6 @@ class AcrobotEnvCfg(EnvCfg):
render_spacing: float = 2.0
sim_dt: float = 0.01
ctrl_dt: float = 0.02
# -- docs-tag-end: acrobot-env-cfg --

2
motrix_envs/src/motrix_envs/basic/finger/cfg.py
View File

@@ -28,7 +28,7 @@ turn_hard_model_file = os.path.dirname(__file__) + "/finger_turn_hard.xml"
@dataclass
class FingerBaseCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 20.0
max_episode_seconds: float = 10.0
sim_dt: float = 0.01
ctrl_dt: float = 0.02

2
motrix_envs/src/motrix_envs/basic/finger/finger_np.py
View File

@@ -23,8 +23,6 @@ from motrix_envs.np.env import NpEnv, NpEnvState
def _sanitize_joint_limits(low: np.ndarray, high: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
low = low.copy()
high = high.copy()
low = np.where(np.isfinite(low), low, -np.pi)
high = np.where(np.isfinite(high), high, np.pi)
return low, high

224
motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py
View File

@@ -54,7 +54,7 @@ class Humanoid3DEnv(NpEnv):
def _build_qpos_limits(self, model) -> tuple[np.ndarray, np.ndarray]:
num_dof_pos = int(model.num_dof_pos)
jl = np.asarray(model.joint_limits, dtype=np.float32)
jl = model.joint_limits
if jl.ndim != 2 or jl.shape[0] != 2:
low = np.full((num_dof_pos,), -np.inf, dtype=np.float32)
high = np.full((num_dof_pos,), np.inf, dtype=np.float32)
@@ -168,17 +168,17 @@ class Humanoid3DEnv(NpEnv):
return obs, {}
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
joint_angles = np.asarray(data.dof_pos[:, 7:], dtype=np.float32)
head_height = self._get_head_height(data).astype(np.float32)[:, None]
extremities = self._get_extremities(data).astype(np.float32)
joint_angles = data.dof_pos[:, 7:]
head_height = self._get_head_height(data)[:, None]
extremities = self._get_extremities(data)
torso_rot = self._torso.get_rotation_mat(data)
torso_vertical = np.asarray(torso_rot[:, 2, :], dtype=np.float32)
torso_vertical = torso_rot[:, 2, :]
com_vel = np.asarray(self._model.get_sensor_value("torso_subtreelinvel", data), dtype=np.float32)
com_vel = self._model.get_sensor_value("torso_subtreelinvel", data)
qvel = np.asarray(data.dof_vel, dtype=np.float32)
target_direction_local = self._get_target_direction_local(data).astype(np.float32)
qvel = data.dof_vel
target_direction_local = self._get_target_direction_local(data)
obs = np.concatenate(
[joint_angles, head_height, extremities, torso_vertical, com_vel, qvel, target_direction_local], axis=-1
@@ -186,14 +186,14 @@ class Humanoid3DEnv(NpEnv):
return obs
def _get_head_height(self, data: mtx.SceneData) -> np.ndarray:
return np.asarray(self._head.get_position(data)[:, 2], dtype=np.float32)
return self._head.get_position(data)[:, 2]
def _get_pelvis_height(self, data: mtx.SceneData) -> np.ndarray:
return np.asarray(self._pelvis.get_position(data)[:, 2], dtype=np.float32)
return self._pelvis.get_position(data)[:, 2]
def _get_torso_upright(self, data: mtx.SceneData) -> np.ndarray:
torso_rot = self._torso.get_rotation_mat(data)
return np.asarray(torso_rot[:, 2, 2], dtype=np.float32)
return torso_rot[:, 2, 2]
def _get_extremities(self, data: mtx.SceneData) -> np.ndarray:
torso_rot = self._torso.get_rotation_mat(data)
@@ -206,12 +206,10 @@ class Humanoid3DEnv(NpEnv):
self._right_foot.get_position(data),
]
out = []
torso_rot_f32 = np.asarray(torso_rot, dtype=np.float32)
torso_pos_f32 = np.asarray(torso_pos, dtype=np.float32)
for p in parts:
torso_to_limb = np.asarray(p, dtype=np.float32) - torso_pos_f32
v_body = np.einsum("ni,nij->nj", torso_to_limb, torso_rot_f32)
torso_to_limb = p - torso_pos
v_body = np.einsum("ni,nij->nj", torso_to_limb, torso_rot)
out.append(v_body)
return np.concatenate(out, axis=-1)
@@ -219,9 +217,8 @@ class Humanoid3DEnv(NpEnv):
def _get_target_direction_local(self, data: mtx.SceneData) -> np.ndarray:
n = int(data.shape[0])
torso_rot = self._torso.get_rotation_mat(data)
torso_rot_f32 = np.asarray(torso_rot, dtype=np.float32)
target_world = np.ones((n, 3), dtype=np.float32) * self._target_direction[None, :]
target_local = np.einsum("ni,nij->nj", target_world, torso_rot_f32)
target_local = np.einsum("ni,nij->nj", target_world, torso_rot)
return target_local
def _compute_reward(
@@ -251,39 +248,27 @@ class Humanoid3DEnv(NpEnv):
torso_upright: np.ndarray,
pelvis_height: np.ndarray,
) -> np.ndarray:
stand_reward = (
reward.tolerance(
head_height,
bounds=(self._head_height_min, float("inf")),
margin=0.5,
)
.astype(np.float32)
.flatten()
)
stand_reward = reward.tolerance(
head_height,
bounds=(self._head_height_min, float("inf")),
margin=0.5,
).flatten()
upright_reward = (
reward.tolerance(
torso_upright,
bounds=(0.9, float("inf")),
sigmoid="linear",
margin=0.9,
)
.astype(np.float32)
.flatten()
)
upright_reward = reward.tolerance(
torso_upright,
bounds=(0.9, float("inf")),
sigmoid="linear",
margin=0.9,
).flatten()
pelvis_height_reward = (
reward.tolerance(
pelvis_height,
bounds=(self._pelvis_height_min, float("inf")),
sigmoid="linear",
margin=self._pelvis_height_margin,
)
.astype(np.float32)
.flatten()
)
pelvis_height_reward = reward.tolerance(
pelvis_height,
bounds=(self._pelvis_height_min, float("inf")),
sigmoid="linear",
margin=self._pelvis_height_margin,
).flatten()
return (stand_reward * upright_reward * pelvis_height_reward).astype(np.float32)
return stand_reward * upright_reward * pelvis_height_reward
def _compute_speed_and_energy_reward(
self,
@@ -291,50 +276,38 @@ class Humanoid3DEnv(NpEnv):
) -> tuple[np.ndarray, np.ndarray]:
target_dir_xy = self._target_direction_xy
ctrls = np.asarray(data.actuator_ctrls, dtype=np.float32)
com_vel = np.asarray(self._model.get_sensor_value("torso_subtreelinvel", data), dtype=np.float32)
ctrls = data.actuator_ctrls
com_vel = self._model.get_sensor_value("torso_subtreelinvel", data)
if self._move_speed <= 0.0:
energy_reward = np.exp(-1.0 * np.mean(np.square(ctrls), axis=-1)).astype(np.float32)
actual_speed = np.linalg.norm(com_vel[:, :2], axis=-1).astype(np.float32)
speed_reward = (
reward.tolerance(
actual_speed,
bounds=(self._move_speed, self._move_speed),
margin=1.0,
value_at_margin=0.01,
)
.astype(np.float32)
.flatten()
)
energy_reward = np.exp(-1.0 * np.mean(np.square(ctrls), axis=-1))
actual_speed = np.linalg.norm(com_vel[:, :2], axis=-1)
speed_reward = reward.tolerance(
actual_speed,
bounds=(self._move_speed, self._move_speed),
margin=1.0,
value_at_margin=0.01,
).flatten()
elif self._move_speed <= 3.0:
energy_reward = np.exp(-0.5 * np.mean(np.square(ctrls), axis=-1)).astype(np.float32)
actual_speed = np.sum(com_vel[:, :2] * target_dir_xy, axis=-1).astype(np.float32)
speed_reward = (
reward.tolerance(
actual_speed,
bounds=(self._move_speed, self._move_speed),
margin=self._move_speed,
value_at_margin=0.0,
sigmoid="linear",
)
.astype(np.float32)
.flatten()
)
energy_reward = np.exp(-0.5 * np.mean(np.square(ctrls), axis=-1))
actual_speed = np.sum(com_vel[:, :2] * target_dir_xy, axis=-1)
speed_reward = reward.tolerance(
actual_speed,
bounds=(self._move_speed, self._move_speed),
margin=self._move_speed,
value_at_margin=0.0,
sigmoid="linear",
).flatten()
else:
energy_reward = np.exp(-0.3 * np.mean(np.square(ctrls), axis=-1)).astype(np.float32)
actual_speed = np.sum(com_vel[:, :2] * target_dir_xy, axis=-1).astype(np.float32)
speed_reward = (
reward.tolerance(
actual_speed,
bounds=(self._move_speed, float("inf")),
margin=self._move_speed,
value_at_margin=0.0,
sigmoid="linear",
)
.astype(np.float32)
.flatten()
)
energy_reward = np.exp(-0.3 * np.mean(np.square(ctrls), axis=-1))
actual_speed = np.sum(com_vel[:, :2] * target_dir_xy, axis=-1)
speed_reward = reward.tolerance(
actual_speed,
bounds=(self._move_speed, float("inf")),
margin=self._move_speed,
value_at_margin=0.0,
sigmoid="linear",
).flatten()
return speed_reward, energy_reward
@@ -346,17 +319,13 @@ class Humanoid3DEnv(NpEnv):
margin,
) -> np.ndarray:
dot = np.sum(forward_vec * target_dir, axis=-1)
return (
reward.tolerance(
dot,
bounds=bounds,
margin=margin,
value_at_margin=0.0,
sigmoid="linear",
)
.astype(np.float32)
.flatten()
)
return reward.tolerance(
dot,
bounds=bounds,
margin=margin,
value_at_margin=0.0,
sigmoid="linear",
).flatten()
def _compute_gait_reward(self, data: mtx.SceneData) -> np.ndarray:
target_dir = self._target_direction
@@ -365,49 +334,36 @@ class Humanoid3DEnv(NpEnv):
head_rot = self._head.get_rotation_mat(data)
pelvis_rot = self._pelvis.get_rotation_mat(data)
torso_forward = np.asarray(torso_rot[:, 0, 0:3], dtype=np.float32)
torso_forward = torso_rot[:, 0, 0:3]
torso_heading_reward = self._compute_heading_reward(torso_forward, target_dir, bounds=(0.9, 1.0), margin=0.3)
head_forward = np.asarray(head_rot[:, 0, 0:3], dtype=np.float32)
head_forward = head_rot[:, 0, 0:3]
head_heading_reward = self._compute_heading_reward(head_forward, target_dir, bounds=(0.9, 1.0), margin=0.3)
pelvis_forward = np.asarray(pelvis_rot[:, 0, 0:3], dtype=np.float32)
pelvis_forward = pelvis_rot[:, 0, 0:3]
pelvis_yaw_reward = self._compute_heading_reward(pelvis_forward, target_dir, bounds=(0.9, 1.0), margin=0.3)
pelvis_up = np.asarray(pelvis_rot[:, 2, 2], dtype=np.float32)
pelvis_level_reward = (
reward.tolerance(
pelvis_up,
bounds=(0.9, 1.0),
margin=0.3,
sigmoid="linear",
value_at_margin=0.0,
)
.astype(np.float32)
.flatten()
)
pelvis_up = pelvis_rot[:, 2, 2]
pelvis_level_reward = reward.tolerance(
pelvis_up,
bounds=(0.9, 1.0),
margin=0.3,
sigmoid="linear",
value_at_margin=0.0,
).flatten()
left_foot_pos = self._left_foot.get_position(data)
right_foot_pos = self._right_foot.get_position(data)
max_foot_h = np.maximum(
np.asarray(left_foot_pos[:, 2], dtype=np.float32),
np.asarray(right_foot_pos[:, 2], dtype=np.float32),
)
feet_height_reward = (
reward.tolerance(
max_foot_h,
bounds=(0.0, 0.3),
margin=0.5,
sigmoid="quadratic",
value_at_margin=0.0,
)
.astype(np.float32)
.flatten()
)
max_foot_h = np.maximum(left_foot_pos[:, 2], right_foot_pos[:, 2])
feet_height_reward = reward.tolerance(
max_foot_h,
bounds=(0.0, 0.3),
margin=0.5,
sigmoid="quadratic",
value_at_margin=0.0,
).flatten()
return (
torso_heading_reward * head_heading_reward * pelvis_yaw_reward * pelvis_level_reward * feet_height_reward
).astype(np.float32)
return torso_heading_reward * head_heading_reward * pelvis_yaw_reward * pelvis_level_reward * feet_height_reward
def _compute_terminated(
self,
@@ -415,13 +371,13 @@ class Humanoid3DEnv(NpEnv):
head_height: np.ndarray,
torso_upright: np.ndarray,
) -> np.ndarray:
qpos = np.asarray(data.dof_pos, dtype=np.float32)
qvel = np.asarray(data.dof_vel, dtype=np.float32)
qpos = data.dof_pos
qvel = data.dof_vel
bad = ~np.isfinite(qpos).all(axis=-1) | ~np.isfinite(qvel).all(axis=-1)
too_low = head_height < self._term_head_height_min
too_tilted = torso_upright < self._term_torso_upright_threshold
extreme_vel = np.abs(qvel).max(axis=-1) > self._term_extreme_vel_threshold
return (bad | too_low | too_tilted | extreme_vel).astype(bool)
return bad | too_low | too_tilted | extreme_vel
def _init_joint_randomization_config(self, cfg: HumanoidWalkCfg) -> None:
init_cfg = cfg.init_state

20
motrix_envs/src/motrix_envs/basic/lqr/__init__.py Normal file
View File

@@ -0,0 +1,20 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
from . import lqr_np # noqa: F401
from .cfg import Lqr21Cfg, Lqr62Cfg
from .lqr_np import LqrEnv
__all__ = ["Lqr21Cfg", "Lqr62Cfg", "LqrEnv"]

73
motrix_envs/src/motrix_envs/basic/lqr/cfg.py Normal file
View File

@@ -0,0 +1,73 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass
from motrix_envs import registry
from motrix_envs.base import EnvCfg
_DIR = os.path.dirname(__file__)
@dataclass
class LqrBaseCfg(EnvCfg):
sim_dt: float = 0.01
ctrl_dt: float = 0.03
max_episode_seconds: float = None
control_cost_coef: float = 0.1
velocity_cost_coef: float = 0.05
reset_position_norm: float = 2.0**0.5
boundary_position_limit: float = 1.2
boundary_velocity_limit: float = 8.0
success_position_tol: float = 0.06
success_velocity_tol: float = 0.05
success_bonus: float = 3.0
out_of_bounds_penalty: float = 2.0
expected_nq: int = 0
expected_nu: int = 0
@registry.envcfg("dm-lqr-2-1")
@dataclass
class Lqr21Cfg(LqrBaseCfg):
model_file: str = os.path.join(_DIR, "lqr_2_1.xml")
reset_position_norm: float = 0.8
control_cost_coef: float = 0.15
velocity_cost_coef: float = 0.15
boundary_position_limit: float = 1.15
boundary_velocity_limit: float = 6.0
success_position_tol: float = 0.04
success_velocity_tol: float = 0.03
success_bonus: float = 4.0
out_of_bounds_penalty: float = 3.0
expected_nq: int = 2
expected_nu: int = 1
@registry.envcfg("dm-lqr-6-2")
@dataclass
class Lqr62Cfg(LqrBaseCfg):
model_file: str = os.path.join(_DIR, "lqr_6_2.xml")
reset_position_norm: float = 1.0
velocity_cost_coef: float = 0.08
boundary_position_limit: float = 1.2
boundary_velocity_limit: float = 8.0
success_position_tol: float = 0.1
success_velocity_tol: float = 0.06
success_bonus: float = 5.0
out_of_bounds_penalty: float = 3.0
expected_nq: int = 6
expected_nu: int = 2

70
motrix_envs/src/motrix_envs/basic/lqr/lqr_2_1.xml Normal file
View File

@@ -0,0 +1,70 @@
<mujoco model="dm-lqr-2-1">
<include file="../../common/materials.xml"/>
<compiler angle="radian" inertiafromgeom="true"/>
<option timestep="0.01" integrator="RK4" gravity="0 0 -9.81"/>
<statistic center="0 0 0.12" extent="1.8" />
<default>
<geom type="sphere" size="0.04" density="300" material="self" contype="0" conaffinity="0"/>
<joint type="slide" axis="1 0 0" damping="0.3" limited="true" range="-1.5 1.5"/>
<motor ctrlrange="-1 1" ctrllimited="true" gear="1"/>
</default>
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light name="light" dir="0 1 -1" directional="true" />
<camera name="side" pos="0 -2.8 0" quat="0.707 0.707 0 0" mode="trackcom" />
<camera name="back" pos="-1.2 -1.8 1.1" xyaxes="0.55 -0.84 0 0.28 0.18 0.94" />
<geom name="ground" type="plane" pos="0 0 -0.02" size="0 0 0.01"
material="motphys-ground"
contype="0"
conaffinity="0" />
<geom
name="origin"
type="capsule"
fromto="0 -0.6 0.05 0 0.6 0.05"
size="0.01"
rgba="1 0.8 0.2 1"
contype="0"
conaffinity="0"/>
<body name="m0" pos="0 -0.15 0.1">
<joint name="q0"/>
<geom name="geom_0"/>
</body>
<body name="m1" pos="0 0.15 0.1">
<joint name="q1"/>
<geom name="geom_1"/>
</body>
</worldbody>
<tendon>
<fixed
name="rope_01"
limited="false"
width="0.016"
rgba="0.9 0.82 0.74 1"
stiffness="50"
damping="2"
springlength="0">
<joint joint="q0" coef="-1"/>
<joint joint="q1" coef="1"/>
</fixed>
</tendon>
<actuator>
<motor name="motor_1" joint="q1"/>
</actuator>
</mujoco>

100
motrix_envs/src/motrix_envs/basic/lqr/lqr_6_2.xml Normal file
View File

@@ -0,0 +1,100 @@
<mujoco model="dm-lqr-6-2">
<include file="../../common/materials.xml"/>
<compiler angle="radian" inertiafromgeom="true"/>
<option timestep="0.01" integrator="RK4" gravity="0 0 -9.81"/>
<statistic center="0 0 0.12" extent="2.8" />
<default>
<geom type="sphere" size="0.04" density="300" material="self" contype="0" conaffinity="0"/>
<joint type="slide" axis="1 0 0" damping="0.3" limited="true" range="-1.5 1.5"/>
<motor ctrlrange="-1 1" ctrllimited="true" gear="1"/>
</default>
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light name="light" dir="0 1 -1" directional="true" />
<camera name="side" pos="0 -4.2 0" quat="0.707 0.707 0 0" mode="trackcom" />
<camera name="back" pos="-1.0 -2.6 1.4" xyaxes="0.55 -0.84 0 0.24 0.16 0.96" />
<geom name="ground" type="plane" pos="0 0 -0.02" size="0 0 0.01"
material="motphys-ground"
contype="0"
conaffinity="0" />
<geom
name="origin"
type="capsule"
fromto="0 -1.0 0.05 0 1.0 0.05"
size="0.01"
rgba="1 0.8 0.2 1"
contype="0"
conaffinity="0"/>
<body name="m0" pos="0 -0.5 0.1">
<joint name="q0"/>
<geom name="geom_0"/>
</body>
<body name="m1" pos="0 -0.3 0.1">
<joint name="q1"/>
<geom name="geom_1"/>
</body>
<body name="m2" pos="0 -0.1 0.1">
<joint name="q2"/>
<geom name="geom_2"/>
</body>
<body name="m3" pos="0 0.1 0.1">
<joint name="q3"/>
<geom name="geom_3"/>
</body>
<body name="m4" pos="0 0.3 0.1">
<joint name="q4"/>
<geom name="geom_4"/>
</body>
<body name="m5" pos="0 0.5 0.1">
<joint name="q5"/>
<geom name="geom_5"/>
</body>
</worldbody>
<tendon>
<fixed name="rope_01" limited="false" width="0.016" rgba="0.9 0.82 0.74 1" stiffness="50" damping="2" springlength="0">
<joint joint="q0" coef="-1"/>
<joint joint="q1" coef="1"/>
</fixed>
<fixed name="rope_12" stiffness="50" damping="2" springlength="0" width="0.016" rgba="0.9 0.82 0.74 1">
<joint joint="q1" coef="-1"/>
<joint joint="q2" coef="1"/>
</fixed>
<fixed name="rope_23" stiffness="50" damping="2" springlength="0" width="0.016" rgba="0.9 0.82 0.74 1">
<joint joint="q2" coef="-1"/>
<joint joint="q3" coef="1"/>
</fixed>
<fixed name="rope_34" stiffness="50" damping="2" springlength="0" width="0.016" rgba="0.9 0.82 0.74 1">
<joint joint="q3" coef="-1"/>
<joint joint="q4" coef="1"/>
</fixed>
<fixed name="rope_45" stiffness="50" damping="2" springlength="0" width="0.016" rgba="0.9 0.82 0.74 1">
<joint joint="q4" coef="-1"/>
<joint joint="q5" coef="1"/>
</fixed>
</tendon>
<actuator>
<motor name="motor_4" joint="q4"/>
<motor name="motor_5" joint="q5"/>
</actuator>
</mujoco>

162
motrix_envs/src/motrix_envs/basic/lqr/lqr_np.py Normal file
View File

@@ -0,0 +1,162 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrixsim.render import Color
from motrix_envs import registry
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import LqrBaseCfg
def _normalize_actions(actions: np.ndarray, num_envs: int, num_actuators: int) -> np.ndarray:
actions = np.asarray(actions, dtype=np.float32)
if actions.ndim == 1:
if num_envs != 1 or actions.shape[0] != num_actuators:
raise ValueError(f"Expected action shape ({num_envs}, {num_actuators}) or ({num_actuators},).")
actions = actions.reshape(1, num_actuators)
if actions.shape != (num_envs, num_actuators):
raise ValueError(f"Expected action shape ({num_envs}, {num_actuators}), got {actions.shape}.")
return np.ascontiguousarray(actions)
@registry.env("dm-lqr-2-1", "np")
@registry.env("dm-lqr-6-2", "np")
class LqrEnv(NpEnv):
_cfg: LqrBaseCfg
def __init__(self, cfg: LqrBaseCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._nq = int(self._model.num_dof_pos)
self._nv = int(self._model.num_dof_vel)
self._nu = int(self._model.num_actuators)
if self._nq != cfg.expected_nq or self._nv != cfg.expected_nq:
raise ValueError(f"LQR model mismatch: expected nq=nv={cfg.expected_nq}, got nq={self._nq}, nv={self._nv}.")
if self._nu != cfg.expected_nu:
raise ValueError(f"LQR model mismatch: expected nu={cfg.expected_nu}, got nu={self._nu}.")
obs_dim = self._nq + self._nv
self._action_low = np.asarray(self._model.actuator_ctrl_limits[0], dtype=np.float32)
self._action_high = np.asarray(self._model.actuator_ctrl_limits[1], dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (obs_dim,), dtype=np.float32)
self._action_space = gym.spaces.Box(self._action_low, self._action_high, (self._nu,), dtype=np.float32)
self._rope_geom_pairs = [
(self._model.get_geom(f"geom_{i}"), self._model.get_geom(f"geom_{i + 1}")) for i in range(self._nq - 1)
]
self._rope_color = Color.rgb(0.85, 0.75, 0.65)
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def apply_action(self, actions: np.ndarray, state: NpEnvState) -> NpEnvState:
actions = _normalize_actions(actions, self._num_envs, self._nu)
state.data.actuator_ctrls = np.clip(actions, self._action_low, self._action_high)
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = np.asarray(data.dof_pos, dtype=np.float32)
qvel = np.asarray(data.dof_vel, dtype=np.float32)
return np.concatenate([qpos, qvel], axis=-1)
def draw_gizmos(self, gizmos, render_offsets: np.ndarray) -> None:
if self.state is None:
return
offsets = np.asarray(render_offsets, dtype=np.float32)
data = self.state.data
gizmos.line_width = 6.0
for geom_a, geom_b in self._rope_geom_pairs:
start = np.asarray(geom_a.get_pose(data), dtype=np.float32)[..., :3]
end = np.asarray(geom_b.get_pose(data), dtype=np.float32)[..., :3]
for env_i in range(self._num_envs):
gizmos.draw_line(
start[env_i] + offsets[env_i],
end[env_i] + offsets[env_i],
color=self._rope_color,
)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
qpos = np.asarray(data.dof_pos, dtype=np.float32)
qvel = np.asarray(data.dof_vel, dtype=np.float32)
ctrl = np.asarray(data.actuator_ctrls, dtype=np.float32)
obs = self._get_obs(data)
out_of_bounds = np.any(np.abs(qpos) > self._cfg.boundary_position_limit, axis=-1)
out_of_bounds |= np.any(np.abs(qvel) > self._cfg.boundary_velocity_limit, axis=-1)
position_norm = np.linalg.norm(qpos, axis=-1)
velocity_norm = np.linalg.norm(qvel, axis=-1)
state_cost = 0.5 * np.sum(np.square(qpos), axis=-1)
velocity_cost = 0.5 * self._cfg.velocity_cost_coef * np.sum(np.square(qvel), axis=-1)
control_cost = 0.5 * self._cfg.control_cost_coef * np.sum(np.square(ctrl), axis=-1)
success = (position_norm <= self._cfg.success_position_tol) & (velocity_norm <= self._cfg.success_velocity_tol)
success &= ~out_of_bounds
success_reward = self._cfg.success_bonus * success.astype(np.float32)
boundary_penalty = self._cfg.out_of_bounds_penalty * out_of_bounds.astype(np.float32)
reward = 1.0 - (state_cost + velocity_cost + control_cost) + success_reward - boundary_penalty
terminated = success | out_of_bounds
terminated |= np.isnan(obs).any(axis=-1)
terminated |= np.isnan(ctrl).any(axis=-1)
state.info["metrics"] = {
"position_norm": position_norm.astype(np.float32),
"velocity_norm": velocity_norm.astype(np.float32),
"success": success.astype(np.float32),
"out_of_bounds": out_of_bounds.astype(np.float32),
}
state.info["Reward"] = {
"state_cost": (-state_cost).astype(np.float32),
"velocity_cost": (-velocity_cost).astype(np.float32),
"control_cost": (-control_cost).astype(np.float32),
"success_bonus": success_reward.astype(np.float32),
"out_of_bounds_penalty": (-boundary_penalty).astype(np.float32),
}
return state.replace(
obs=obs,
reward=reward.astype(np.float32),
terminated=terminated,
)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num_envs = int(data.shape[0])
qpos = np.random.standard_normal((num_envs, self._nq)).astype(np.float32)
norms = np.linalg.norm(qpos, axis=-1, keepdims=True)
zero_norm = norms[:, 0] < 1e-8
if np.any(zero_norm):
qpos[zero_norm, 0] = 1.0
norms = np.linalg.norm(qpos, axis=-1, keepdims=True)
qpos *= self._cfg.reset_position_norm / np.clip(norms, 1e-8, None)
qvel = np.zeros((num_envs, self._nv), dtype=np.float32)
data.set_dof_pos(qpos, self._model)
data.set_dof_vel(qvel)
self._model.forward_kinematic(data)
return np.concatenate([qpos, qvel], axis=-1), {}

2
motrix_envs/src/motrix_envs/basic/manipulator/cfg.py
View File

@@ -28,7 +28,7 @@ class BringBallCfg(EnvCfg):
# Simulation
model_file: str = bring_ball_model_file
max_episode_seconds: float = 10.0
sim_dt: float = 0.001
sim_dt: float = 0.01
ctrl_dt: float = 0.01
render_spacing: float = 2.5

2
motrix_envs/src/motrix_envs/basic/pendulum/cfg.py
View File

@@ -24,6 +24,7 @@ from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/pendulum.xml"
# -- docs-tag-start: pendulum-env-cfg --
@registry.envcfg("pendulum")
@dataclass
class PendulumEnvCfg(EnvCfg):
@@ -34,6 +35,7 @@ class PendulumEnvCfg(EnvCfg):
angle_bound: float = 8.0
cosing_bound: float = 0.0
# reset_noise_scale: float = 0.01
# -- docs-tag-end: pendulum-env-cfg --
def __post_init__(self):
self.cosing_bound = float(np.cos(np.deg2rad(self.angle_bound)))

16
motrix_envs/src/motrix_envs/basic/point_mass/__init__.py Normal file
View File

@@ -0,0 +1,16 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
from . import point_mass_np # noqa: F401

32
motrix_envs/src/motrix_envs/basic/point_mass/cfg.py Normal file
View File

@@ -0,0 +1,32 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/point_mass.xml"
@registry.envcfg("point_mass")
@dataclass
class PointMassEnvCfg(EnvCfg):
model_file: str = model_file
reset_noise_scale: float = 0.01
max_episode_seconds: float = 10
render_spacing: float = 2.0
target_radius: float = 0.1

43
motrix_envs/src/motrix_envs/basic/point_mass/point_mass.xml Normal file
View File

@@ -0,0 +1,43 @@
<mujoco model="point_mass">
<compiler inertiafromgeom="true" />
<option gravity="0 0 0" timestep="0.01" />
<default>
<joint armature="0" damping="0.1" />
<geom contype="0" conaffinity="0" friction="1 0.1 0.1" />
<motor ctrlrange="-1 1" gear="10" />
</default>
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" size="0 0 0.01" type="plane" material="motphys-ground" pos="0 0 0" />
</worldbody>
<worldbody>
<!-- Point mass -->
<body name="point_mass" pos="0 0 0.1">
<joint name="point_mass_x" type="slide" axis="1 0 0" />
<joint name="point_mass_y" type="slide" axis="0 1 0" />
<geom name="mass" type="sphere" size="0.05" rgba="0.5 0.5 1 1" mass="1" />
</body>
<!-- Target -->
<body name="target" pos="0 0 0.1" mocap="true">
<joint name="target_x" type="slide" axis="1 0 0" />
<joint name="target_y" type="slide" axis="0 1 0" />
<geom name="target_geom" type="sphere" size="0.1" rgba="1 0 0 0.8" contype="0" conaffinity="0" />
</body>
</worldbody>
<actuator>
<motor name="x_force" joint="point_mass_x" />
<motor name="y_force" joint="point_mass_y" />
</actuator>
</mujoco>

174
motrix_envs/src/motrix_envs/basic/point_mass/point_mass_np.py Normal file
View File

@@ -0,0 +1,174 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import PointMassEnvCfg
@registry.env("point_mass", "np")
class PointMassEnv(NpEnv):
_cfg: PointMassEnvCfg
def __init__(self, cfg: PointMassEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._action_space = gym.spaces.Box(-1.0, 1.0, (2,), dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (9,), dtype=np.float32)
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._point_mass = self._model.get_body("point_mass")
self._target = self._model.get_body("target")
self._target_radius = cfg.target_radius
# Target stay counter, used to control reset after 0.5 seconds of overlap
self._in_target_steps = np.zeros(self._num_envs, dtype=np.int32)
self._required_in_target_steps = int(0.5 / cfg.ctrl_dt)
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def apply_action(self, actions: np.ndarray, state: NpEnvState):
actions = np.clip(actions, -1.0, 1.0)
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
dof_pos = data.dof_pos[:, :2] # Only x and y positions
dof_vel = data.dof_vel[:, :2] # Only x and y velocities
# Get target position
target_pos = self._target.get_pose(data)[:, :2]
# Calculate distance and direction to target
delta = target_pos - dof_pos
distance = np.linalg.norm(delta, axis=-1, keepdims=True)
obs = np.concatenate([dof_pos, dof_vel, target_pos, delta, distance], axis=-1)
return obs
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
# Get positions of point mass and target
point_pos = self._point_mass.get_pose(data)[:, :2]
target_pos = self._target.get_pose(data)[:, :2]
dist_to_target = np.linalg.norm(point_pos - target_pos, axis=-1)
# Calculate effective target radius for complete overlap
# Blue ball radius is 0.05, red ball radius is 0.1
# For complete overlap, center distance should be very small
effective_target_radius = 0.02 # Smaller radius for complete overlap
# Fine-grained distance reward - exponential function, reward grows faster as distance decreases
distance_reward = np.exp(-10 * dist_to_target) # Stronger exponential reward
# Large bonus for complete target entry
in_target = dist_to_target < effective_target_radius
target_bonus = 100.0 * in_target # Significantly increased reward
# Continuous stay reward
continuous_reward = 30.0 * in_target # Increased continuous reward
# Penalty for distance from target center - encourages complete overlap
# When inside target, penalty increases with distance from center
center_penalty = np.where(in_target, 10.0 * dist_to_target, 0.0)
# Control penalty - increased penalty to encourage smoother movement
dof_vel = data.dof_vel[:, :2]
vel_magnitude = np.linalg.norm(dof_vel, axis=-1)
control_penalty = 0.1 * vel_magnitude # Increased penalty to reduce excessive movement
# Path optimization reward - encourages straight-line movement
# Calculate alignment between velocity direction and target direction
if dist_to_target.max() > 0:
delta = target_pos - point_pos
delta_norm = np.linalg.norm(delta, axis=-1, keepdims=True)
delta_normalized = delta / delta_norm
vel_normalized = dof_vel / (np.linalg.norm(dof_vel, axis=-1, keepdims=True) + 1e-6)
direction_alignment = np.sum(delta_normalized * vel_normalized, axis=-1)
path_reward = 0.5 * direction_alignment
else:
path_reward = 0.0
# Total reward
rwd = distance_reward + target_bonus + continuous_reward + path_reward - center_penalty - control_penalty
# Update target stay steps
self._in_target_steps = np.where(in_target, self._in_target_steps + 1, 0)
# Check if stayed in target long enough
in_target_long_enough = self._in_target_steps >= self._required_in_target_steps
# Check termination conditions - terminate when reaching target for 0.5 seconds or when NaN encountered
terminated = np.zeros((self._num_envs,), dtype=bool)
terminated = np.logical_or(in_target_long_enough, terminated)
terminated = np.logical_or(np.isnan(obs).any(axis=-1), terminated)
state.obs = obs
state.reward = rwd
state.terminated = terminated
return state
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num_reset = data.shape[0]
# Get the actual number of DOF from the model
num_dof_pos = self._model.num_dof_pos
num_dof_vel = self._model.num_dof_vel
# Random initial position within a range for the point mass (only x, y)
x_pos = np.random.uniform(-1.0, 1.0, size=num_reset).astype(np.float32)
y_pos = np.random.uniform(-1.0, 1.0, size=num_reset).astype(np.float32)
# Create dof_pos with the correct length (point mass x, y and target x, y)
dof_pos = np.zeros((num_reset, num_dof_pos), dtype=np.float32)
dof_pos[:, 0] = x_pos # point_mass_x
dof_pos[:, 1] = y_pos # point_mass_y
dof_vel = np.zeros((num_reset, num_dof_vel), dtype=np.float32)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
# Randomize target position using its slide joints
target_x = np.random.uniform(-1.5, 1.5, size=num_reset).astype(np.float32)
target_y = np.random.uniform(-1.5, 1.5, size=num_reset).astype(np.float32)
# Set target position via its slide joints (indices 2 and 3)
dof_pos[:, 2] = target_x # target_x
dof_pos[:, 3] = target_y # target_y
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
# Reset target stay counter for the environments being reset
self._in_target_steps[:num_reset] = 0
obs = self._get_obs(data)
return obs, {}

23
motrix_envs/src/motrix_envs/basic/quadruped/__init__.py Normal file
View File

@@ -0,0 +1,23 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
from . import quadruped_np # noqa: F401
from .cfg import ( # noqa: F401
QuadrupedBaseCfg,
QuadrupedEscapeCfg,
QuadrupedFetchCfg,
QuadrupedRunCfg,
QuadrupedWalkCfg,
)

3
motrix_envs/src/motrix_envs/basic/quadruped/assets/quadruped_escape_heightfield.png Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:775bf911a0d5ce8703f1a1f766da870a1dd717a3b9366c118bad91474bbeebf0
size 13996

149
motrix_envs/src/motrix_envs/basic/quadruped/cfg.py Normal file
View File

@@ -0,0 +1,149 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass
from motrix_envs import registry
from motrix_envs.base import EnvCfg
_DIR = os.path.dirname(__file__)
_WALK_MODEL = os.path.join(_DIR, "quadruped_walk.xml")
_ESCAPE_MODEL = os.path.join(_DIR, "quadruped_escape.xml")
_FETCH_MODEL = os.path.join(_DIR, "quadruped_fetch.xml")
@dataclass
class QuadrupedBaseCfg(EnvCfg):
model_file: str = _WALK_MODEL
max_episode_seconds: float = 20.0
sim_dt: float = 0.01
ctrl_dt: float = 0.01
render_spacing: float = 2.0
# Task parameters
desired_speed: float = 0.0
deviation_angle: float = 0.0
fix_heading: bool = False
clip_env_actions: bool = True
# Observation toggles
include_origin: bool = False
include_rangefinder: bool = False
include_ball: bool = False
include_target: bool = False
# Task geometry (fallbacks when sites/geom metadata are unavailable)
target_radius: float = 0.7
terrain_size: float = 30.0
# Gait shaping (walk/run)
stand_height: float = 0.55
stand_height_margin: float = 0.25
height_reward_weight: float = 0.1
lateral_velocity_limit: float = 0.2
lateral_velocity_margin: float = 0.5
lateral_reward_weight: float = 0.05
heading_reward_margin: float = 1.0
heading_reward_weight: float = 0.0
action_smoothness_margin: float = 5.0
action_smoothness_weight: float = 0.08
lin_vel_z_weight: float = 0.05
ang_vel_xy_weight: float = 0.02
similar_to_default_weight: float = 0.025
# Forward motion shaping (walk/run)
backward_penalty_weight: float = 0.2
# Radial motion shaping (escape)
radial_velocity_weight: float = 0.2
# Fetch shaping
fetch_reward_margin: float = 10.0
fetch_reward_weight: float = 1.0
fetch_behind_distance: float = 0.7
fetch_ahead_distance: float = 0.2
fetch_side_stage_offset: float = 0.6
fetch_side_stage_ball_distance: float = 1.0
fetch_side_stage_gate_threshold: float = 0.5
fetch_side_stage_align_threshold: float = 0.8
fetch_stage_radius: float = 0.5
fetch_stage_speed: float = 0.5
fetch_stage_reward_weight: float = 0.5
fetch_corridor_width: float = 0.3
fetch_behind_align_margin: float = 1.0
fetch_heading_margin: float = 1.0
fetch_heading_weight: float = 0.2
fetch_ready_ball_distance: float = 0.5
fetch_ready_weight: float = 0.3
fetch_ready_threshold: float = 0.7
fetch_push_speed: float = 0.2
fetch_push_reward_weight: float = 0.3
fetch_backward_penalty_weight: float = 0.2
fetch_away_penalty_weight: float = 0.4
fetch_leg_ball_penalty_weight: float = 0.1
fetch_leg_ball_penalty_margin = 0.03
fetch_stability_upright_min: float = 0.8
fetch_stability_upright_margin: float = 0.5
fetch_stability_height_min: float = 0.45
fetch_stability_height_margin: float = 0.2
fetch_fall_upright_min: float = 0.2
fetch_fall_height_min: float = 0.25
@registry.envcfg("dm-quadruped-walk")
@dataclass
class QuadrupedWalkCfg(QuadrupedBaseCfg):
model_file: str = _WALK_MODEL
desired_speed: float = 0.5
fix_heading: bool = True
heading_reward_weight: float = 0.2
@registry.envcfg("dm-quadruped-run")
@dataclass
class QuadrupedRunCfg(QuadrupedBaseCfg):
model_file: str = _WALK_MODEL
desired_speed: float = 5.0
fix_heading: bool = True
heading_reward_weight: float = 0.2
@registry.envcfg("dm-quadruped-escape")
@dataclass
class QuadrupedEscapeCfg(QuadrupedBaseCfg):
model_file: str = _ESCAPE_MODEL
render_camera_name: str = "global"
desired_speed: float = 3.0
include_origin: bool = True
include_rangefinder: bool = False
deviation_angle: float = 20.0
fix_heading: bool = True
heading_reward_weight: float = 0.2
radial_velocity_weight: float = 0.5
similar_to_default_weight: float = 0.01
@registry.envcfg("dm-quadruped-fetch")
@dataclass
class QuadrupedFetchCfg(QuadrupedBaseCfg):
model_file: str = _FETCH_MODEL
desired_speed: float = 2.0
include_ball: bool = True
include_target: bool = True

327
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_base.xml Normal file
View File

@@ -0,0 +1,327 @@
<mujoco model="quadruped">
<include file="../../common/skybox.xml"/>
<include file="../../common/visual.xml"/>
<include file="./quadruped_materials.xml"/>
<visual>
<rgba rangefinder="1 1 0.1 0.1"/>
<map znear=".005" zfar="20"/>
</visual>
<asset>
<hfield name="terrain" ncol="201" nrow="201" size="30 30 5 .1"/>
</asset>
<option timestep=".005"/>
<default>
<geom solimp=".9 .99 .003" solref=".01 1"/>
<default class="body">
<geom type="capsule" size=".08" condim="1" material="self" density="500"/>
<joint type="hinge" damping="30" armature=".01"
limited="true" solimplimit="0 .99 .01"/>
<default class="hip">
<default class="yaw">
<joint axis="0 0 1" range="-50 50"/>
</default>
<default class="pitch">
<joint axis="0 1 0" range="-20 60"/>
</default>
<geom fromto="0 0 0 .3 0 .11"/>
</default>
<default class="knee">
<joint axis="0 1 0" range="-60 50"/>
<geom size=".065" fromto="0 0 0 .25 0 -.25"/>
</default>
<default class="ankle">
<joint axis="0 1 0" range="-45 55"/>
<geom size=".055" fromto="0 0 0 0 0 -.25"/>
</default>
<default class="toe">
<geom type="sphere" size=".08" material="effector" friction="1.5"/>
<site type="sphere" size=".084" material="site" group="4"/>
</default>
</default>
<default class="rangefinder">
<site type="capsule" size=".005 .1" material="site" group="4"/>
</default>
<default class="wall">
<geom type="plane" material="decoration"/>
</default>
<default class="coupling">
<equality solimp="0.95 0.99 0.01" solref=".005 .5"/>
</default>
<general ctrllimited="true" gainprm="1000" biasprm="0 -1000" biastype="affine" dyntype="filter" dynprm=".1"/>
<default class="yaw_act">
<general ctrlrange="-1 1"/>
</default>
<default class="lift_act">
<general ctrlrange="-1 1.1"/>
</default>
<default class="extend_act">
<general ctrlrange="-.8 .8"/>
</default>
</default>
<asset>
<texture name="ball" builtin="checker" mark="cross" width="151" height="151"
rgb1="0.1 0.1 0.1" rgb2="0.9 0.9 0.9" markrgb="1 1 1"/>
<material name="ball" texture="ball" />
</asset>
<worldbody>
<geom name="floor" type="plane" size="0 0 0.01" material="motphys-ground"/>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="wall_px" class="wall" pos="-15.7 0 .7" zaxis="1 0 1" size="1 15 .5"/>
<geom name="wall_py" class="wall" pos="0 -15.7 .7" zaxis="0 1 1" size="15 1 .5"/>
<geom name="wall_nx" class="wall" pos="15.7 0 .7" zaxis="-1 0 1" size="1 15 .5"/>
<geom name="wall_ny" class="wall" pos="0 15.7 .7" zaxis="0 -1 1" size="15 1 .5"/>
<site name="target" type="cylinder" size=".4 .06" pos="0 0 .05" material="target"/>
<geom name="terrain" type="hfield" hfield="terrain" rgba=".2 .3 .4 1" pos="0 0 -.01"/>
<camera name="global" pos="-10 10 10" xyaxes="-1 -1 0 1 0 1" mode="trackcom"/>
<body name="torso" childclass="body" pos="0 0 .57">
<freejoint name="root"/>
<camera name="x" pos="-1.7 0 1" xyaxes="0 -1 0 .75 0 1" mode="trackcom"/>
<camera name="y" pos="0 4 2" xyaxes="-1 0 0 0 -.5 1" mode="trackcom"/>
<camera name="egocentric" pos=".3 0 .11" xyaxes="0 -1 0 .4 0 1" fovy="60"/>
<geom name="eye_r" type="cylinder" size=".05" fromto=".1 -.07 .12 .31 -.07 .08" mass="0"/>
<site name="pupil_r" type="sphere" size=".033" pos=".3 -.07 .08" zaxis="1 0 0" material="eye"/>
<geom name="eye_l" type="cylinder" size=".05" fromto=".1 .07 .12 .31 .07 .08" mass="0"/>
<site name="pupil_l" type="sphere" size=".033" pos=".3 .07 .08" zaxis="1 0 0" material="eye"/>
<site name="workspace" type="sphere" size=".3 .3 .3" material="site" pos=".8 0 -.2" group="3"/>
<site name="rf_00" class="rangefinder" fromto=".41 -.02 .11 .34 0 .115"/>
<site name="rf_01" class="rangefinder" fromto=".41 -.01 .11 .34 0 .115"/>
<site name="rf_02" class="rangefinder" fromto=".41 0 .11 .34 0 .115"/>
<site name="rf_03" class="rangefinder" fromto=".41 .01 .11 .34 0 .115"/>
<site name="rf_04" class="rangefinder" fromto=".41 .02 .11 .34 0 .115"/>
<site name="rf_10" class="rangefinder" fromto=".41 -.02 .1 .36 0 .11"/>
<site name="rf_11" class="rangefinder" fromto=".41 -.02 .1 .36 0 .11"/>
<site name="rf_12" class="rangefinder" fromto=".41 0 .1 .36 0 .11"/>
<site name="rf_13" class="rangefinder" fromto=".41 .01 .1 .36 0 .11"/>
<site name="rf_14" class="rangefinder" fromto=".41 .02 .1 .36 0 .11"/>
<site name="rf_20" class="rangefinder" fromto=".41 -.02 .09 .38 0 .105"/>
<site name="rf_21" class="rangefinder" fromto=".41 -.01 .09 .38 0 .105"/>
<site name="rf_22" class="rangefinder" fromto=".41 0 .09 .38 0 .105"/>
<site name="rf_23" class="rangefinder" fromto=".41 .01 .09 .38 0 .105"/>
<site name="rf_24" class="rangefinder" fromto=".41 .02 .09 .38 0 .105"/>
<site name="rf_30" class="rangefinder" fromto=".41 -.02 .08 .4 0 .1"/>
<site name="rf_31" class="rangefinder" fromto=".41 -.01 .08 .4 0 .1"/>
<site name="rf_32" class="rangefinder" fromto=".41 0 .08 .4 0 .1"/>
<site name="rf_33" class="rangefinder" fromto=".41 .01 .08 .4 0 .1"/>
<site name="rf_34" class="rangefinder" fromto=".41 .02 .08 .4 0 .1"/>
<geom name="torso" type="ellipsoid" size=".3 .27 .2" density="1000"/>
<site name="torso_touch" type="box" size=".26 .26 .26" rgba="0 0 1 0"/>
<site name="torso" size=".05" rgba="1 0 0 1" />
<body name="hip_front_left" pos=".2 .2 0" euler="0 0 45" childclass="hip">
<joint name="yaw_front_left" class="yaw"/>
<joint name="pitch_front_left" class="pitch"/>
<geom name="thigh_front_left"/>
<body name="knee_front_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_left"/>
<geom name="shin_front_left"/>
<body name="ankle_front_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_left"/>
<geom name="foot_front_left"/>
<body name="toe_front_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_left"/>
<site name="toe_front_left"/>
</body>
</body>
</body>
</body>
<body name="hip_front_right" pos=".2 -.2 0" euler="0 0 -45" childclass="hip">
<joint name="yaw_front_right" class="yaw"/>
<joint name="pitch_front_right" class="pitch"/>
<geom name="thigh_front_right"/>
<body name="knee_front_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_right"/>
<geom name="shin_front_right"/>
<body name="ankle_front_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_right"/>
<geom name="foot_front_right"/>
<body name="toe_front_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_right"/>
<site name="toe_front_right"/>
</body>
</body>
</body>
</body>
<body name="hip_back_right" pos="-.2 -.2 0" euler="0 0 -135" childclass="hip">
<joint name="yaw_back_right" class="yaw"/>
<joint name="pitch_back_right" class="pitch"/>
<geom name="thigh_back_right"/>
<body name="knee_back_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_right"/>
<geom name="shin_back_right"/>
<body name="ankle_back_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_right"/>
<geom name="foot_back_right"/>
<body name="toe_back_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_right"/>
<site name="toe_back_right"/>
</body>
</body>
</body>
</body>
<body name="hip_back_left" pos="-.2 .2 0" euler="0 0 135" childclass="hip">
<joint name="yaw_back_left" class="yaw"/>
<joint name="pitch_back_left" class="pitch"/>
<geom name="thigh_back_left"/>
<body name="knee_back_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_left"/>
<geom name="shin_back_left"/>
<body name="ankle_back_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_left"/>
<geom name="foot_back_left"/>
<body name="toe_back_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_left"/>
<site name="toe_back_left"/>
</body>
</body>
</body>
</body>
</body>
<body name="ball" pos="0 0 3">
<freejoint name="ball_root"/>
<geom name="ball" size=".15" material="ball" priority="1" condim="6" friction=".7 .005 .005"
solref="-10000 -30"/>
</body>
</worldbody>
<tendon>
<fixed name="coupling_front_left">
<joint joint="pitch_front_left" coef=".333"/>
<joint joint="knee_front_left" coef=".333"/>
<joint joint="ankle_front_left" coef=".333"/>
</fixed>
<fixed name="coupling_front_right">
<joint joint="pitch_front_right" coef=".333"/>
<joint joint="knee_front_right" coef=".333"/>
<joint joint="ankle_front_right" coef=".333"/>
</fixed>
<fixed name="coupling_back_right">
<joint joint="pitch_back_right" coef=".333"/>
<joint joint="knee_back_right" coef=".333"/>
<joint joint="ankle_back_right" coef=".333"/>
</fixed>
<fixed name="coupling_back_left">
<joint joint="pitch_back_left" coef=".333"/>
<joint joint="knee_back_left" coef=".333"/>
<joint joint="ankle_back_left" coef=".333"/>
</fixed>
<fixed name="extend_front_left">
<joint joint="pitch_front_left" coef=".25"/>
<joint joint="knee_front_left" coef="-.5"/>
<joint joint="ankle_front_left" coef=".25"/>
</fixed>
<fixed name="lift_front_left">
<joint joint="pitch_front_left" coef=".5"/>
<joint joint="ankle_front_left" coef="-.5"/>
</fixed>
<fixed name="extend_front_right">
<joint joint="pitch_front_right" coef=".25"/>
<joint joint="knee_front_right" coef="-.5"/>
<joint joint="ankle_front_right" coef=".25"/>
</fixed>
<fixed name="lift_front_right">
<joint joint="pitch_front_right" coef=".5"/>
<joint joint="ankle_front_right" coef="-.5"/>
</fixed>
<fixed name="extend_back_right">
<joint joint="pitch_back_right" coef=".25"/>
<joint joint="knee_back_right" coef="-.5"/>
<joint joint="ankle_back_right" coef=".25"/>
</fixed>
<fixed name="lift_back_right">
<joint joint="pitch_back_right" coef=".5"/>
<joint joint="ankle_back_right" coef="-.5"/>
</fixed>
<fixed name="extend_back_left">
<joint joint="pitch_back_left" coef=".25"/>
<joint joint="knee_back_left" coef="-.5"/>
<joint joint="ankle_back_left" coef=".25"/>
</fixed>
<fixed name="lift_back_left">
<joint joint="pitch_back_left" coef=".5"/>
<joint joint="ankle_back_left" coef="-.5"/>
</fixed>
</tendon>
<equality>
<tendon name="coupling_front_left" tendon1="coupling_front_left" class="coupling"/>
<tendon name="coupling_front_right" tendon1="coupling_front_right" class="coupling"/>
<tendon name="coupling_back_right" tendon1="coupling_back_right" class="coupling"/>
<tendon name="coupling_back_left" tendon1="coupling_back_left" class="coupling"/>
</equality>
<actuator>
<general name="yaw_front_left" class="yaw_act" joint="yaw_front_left"/>
<general name="lift_front_left" class="lift_act" tendon="lift_front_left"/>
<general name="extend_front_left" class="extend_act" tendon="extend_front_left"/>
<general name="yaw_front_right" class="yaw_act" joint="yaw_front_right"/>
<general name="lift_front_right" class="lift_act" tendon="lift_front_right"/>
<general name="extend_front_right" class="extend_act" tendon="extend_front_right"/>
<general name="yaw_back_right" class="yaw_act" joint="yaw_back_right"/>
<general name="lift_back_right" class="lift_act" tendon="lift_back_right"/>
<general name="extend_back_right" class="extend_act" tendon="extend_back_right"/>
<general name="yaw_back_left" class="yaw_act" joint="yaw_back_left"/>
<general name="lift_back_left" class="lift_act" tendon="lift_back_left"/>
<general name="extend_back_left" class="extend_act" tendon="extend_back_left"/>
</actuator>
<sensor>
<accelerometer name="imu_accel" site="torso"/>
<gyro name="imu_gyro" site="torso"/>
<velocimeter name="velocimeter" site="torso"/>
<force name="force_toe_front_left" site="toe_front_left"/>
<force name="force_toe_front_right" site="toe_front_right"/>
<force name="force_toe_back_right" site="toe_back_right"/>
<force name="force_toe_back_left" site="toe_back_left"/>
<torque name="torque_toe_front_left" site="toe_front_left"/>
<torque name="torque_toe_front_right" site="toe_front_right"/>
<torque name="torque_toe_back_right" site="toe_back_right"/>
<torque name="torque_toe_back_left" site="toe_back_left"/>
<subtreecom name="center_of_mass" body="torso"/>
<rangefinder name="rf_00" site="rf_00"/>
<rangefinder name="rf_01" site="rf_01"/>
<rangefinder name="rf_02" site="rf_02"/>
<rangefinder name="rf_03" site="rf_03"/>
<rangefinder name="rf_04" site="rf_04"/>
<rangefinder name="rf_10" site="rf_10"/>
<rangefinder name="rf_11" site="rf_11"/>
<rangefinder name="rf_12" site="rf_12"/>
<rangefinder name="rf_13" site="rf_13"/>
<rangefinder name="rf_14" site="rf_14"/>
<rangefinder name="rf_20" site="rf_20"/>
<rangefinder name="rf_21" site="rf_21"/>
<rangefinder name="rf_22" site="rf_22"/>
<rangefinder name="rf_23" site="rf_23"/>
<rangefinder name="rf_24" site="rf_24"/>
<rangefinder name="rf_30" site="rf_30"/>
<rangefinder name="rf_31" site="rf_31"/>
<rangefinder name="rf_32" site="rf_32"/>
<rangefinder name="rf_33" site="rf_33"/>
<rangefinder name="rf_34" site="rf_34"/>
</sensor>
</mujoco>

272
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_escape.xml Normal file
View File

@@ -0,0 +1,272 @@
<mujoco model="quadruped">
<include file="../../common/skybox.xml" />
<include file="../../common/visual.xml" />
<include file="./quadruped_materials.xml" />
<visual>
<rgba rangefinder="1 1 0.1 0.1" />
<map znear=".005" zfar="20" />
</visual>
<asset>
<hfield name="terrain" ncol="201" nrow="201" size="30 30 4.0 .1" file="assets/quadruped_escape_heightfield.png" />
</asset>
<option timestep=".005" />
<default>
<geom solimp=".9 .99 .003" solref=".01 1" />
<default class="body">
<geom type="capsule" size=".08" condim="1" material="self" density="500" />
<joint type="hinge" damping="30" armature=".01" limited="true" solimplimit="0 .99 .01" />
<default class="hip">
<default class="yaw">
<joint axis="0 0 1" range="-50 50" />
</default>
<default class="pitch">
<joint axis="0 1 0" range="-20 60" />
</default>
<geom fromto="0 0 0 .3 0 .11" />
</default>
<default class="knee">
<joint axis="0 1 0" range="-60 50" />
<geom size=".065" fromto="0 0 0 .25 0 -.25" />
</default>
<default class="ankle">
<joint axis="0 1 0" range="-45 55" />
<geom size=".055" fromto="0 0 0 0 0 -.25" />
</default>
<default class="toe">
<geom type="sphere" size=".08" material="effector" friction="2.0" />
<site type="sphere" size=".084" material="site" group="4" />
</default>
</default>
<default class="rangefinder">
<site type="capsule" size=".005 .1" material="site" group="4" />
</default>
<default class="wall">
<geom type="plane" material="decoration" />
</default>
<default class="coupling">
<equality solimp="0.95 0.99 0.01" solref=".005 .5" />
</default>
<general ctrllimited="true" gainprm="1500" biasprm="0 -1500" biastype="affine" dyntype="filter" dynprm=".1" />
<default class="yaw_act">
<general ctrlrange="-1 1" />
</default>
<default class="lift_act">
<general ctrlrange="-1 1.1" />
</default>
<default class="extend_act">
<general ctrlrange="-.8 .8" />
</default>
</default>
<asset>
<texture name="ball" builtin="checker" mark="cross" width="151" height="151" rgb1="0.1 0.1 0.1" rgb2="0.9 0.9 0.9" markrgb="1 1 1" />
<material name="ball" texture="ball" />
</asset>
<worldbody>
<geom name="floor" type="plane" size="0 0 0.01" material="motphys-ground" />
<geom name="terrain" type="hfield" hfield="terrain" rgba=".2 .3 .4 1" pos="0 0 -.0" />
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<camera name="global" pos="-10 10 10" xyaxes="-1 -1 0 1 0 1" mode="trackcom" />
<body name="torso" childclass="body" pos="0 0 .57">
<freejoint name="root" />
<camera name="x" pos="-1.7 0 1" xyaxes="0 -1 0 .75 0 1" mode="trackcom" />
<camera name="y" pos="0 4 2" xyaxes="-1 0 0 0 -.5 1" mode="trackcom" />
<camera name="egocentric" pos=".3 0 .11" xyaxes="0 -1 0 .4 0 1" fovy="60" />
<geom name="eye_r" type="cylinder" size=".05" fromto=".1 -.07 .12 .31 -.07 .08" mass="0" />
<site name="pupil_r" type="sphere" size=".033" pos=".3 -.07 .08" zaxis="1 0 0" material="eye" />
<geom name="eye_l" type="cylinder" size=".05" fromto=".1 .07 .12 .31 .07 .08" mass="0" />
<site name="pupil_l" type="sphere" size=".033" pos=".3 .07 .08" zaxis="1 0 0" material="eye" />
<site name="workspace" type="sphere" size=".3 .3 .3" material="site" pos=".8 0 -.2" group="3" />
<geom name="torso" type="box" size=".3 .27 .2" density="1000" />
<site name="torso_touch" type="box" size=".26 .26 .26" rgba="0 0 1 0" />
<site name="torso" size=".05" rgba="1 0 0 1" />
<body name="hip_front_left" pos=".2 .2 0" euler="0 0 45" childclass="hip">
<joint name="yaw_front_left" class="yaw" />
<joint name="pitch_front_left" class="pitch" />
<geom name="thigh_front_left" />
<body name="knee_front_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_left" />
<geom name="shin_front_left" />
<body name="ankle_front_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_left" />
<geom name="foot_front_left" />
<body name="toe_front_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_left" />
<site name="toe_front_left" />
</body>
</body>
</body>
</body>
<body name="hip_front_right" pos=".2 -.2 0" euler="0 0 -45" childclass="hip">
<joint name="yaw_front_right" class="yaw" />
<joint name="pitch_front_right" class="pitch" />
<geom name="thigh_front_right" />
<body name="knee_front_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_right" />
<geom name="shin_front_right" />
<body name="ankle_front_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_right" />
<geom name="foot_front_right" />
<body name="toe_front_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_right" />
<site name="toe_front_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_right" pos="-.2 -.2 0" euler="0 0 -135" childclass="hip">
<joint name="yaw_back_right" class="yaw" />
<joint name="pitch_back_right" class="pitch" />
<geom name="thigh_back_right" />
<body name="knee_back_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_right" />
<geom name="shin_back_right" />
<body name="ankle_back_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_right" />
<geom name="foot_back_right" />
<body name="toe_back_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_right" />
<site name="toe_back_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_left" pos="-.2 .2 0" euler="0 0 135" childclass="hip">
<joint name="yaw_back_left" class="yaw" />
<joint name="pitch_back_left" class="pitch" />
<geom name="thigh_back_left" />
<body name="knee_back_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_left" />
<geom name="shin_back_left" />
<body name="ankle_back_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_left" />
<geom name="foot_back_left" />
<body name="toe_back_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_left" />
<site name="toe_back_left" />
</body>
</body>
</body>
</body>
</body>
</worldbody>
<tendon>
<fixed name="coupling_front_left">
<joint joint="pitch_front_left" coef=".333" />
<joint joint="knee_front_left" coef=".333" />
<joint joint="ankle_front_left" coef=".333" />
</fixed>
<fixed name="coupling_front_right">
<joint joint="pitch_front_right" coef=".333" />
<joint joint="knee_front_right" coef=".333" />
<joint joint="ankle_front_right" coef=".333" />
</fixed>
<fixed name="coupling_back_right">
<joint joint="pitch_back_right" coef=".333" />
<joint joint="knee_back_right" coef=".333" />
<joint joint="ankle_back_right" coef=".333" />
</fixed>
<fixed name="coupling_back_left">
<joint joint="pitch_back_left" coef=".333" />
<joint joint="knee_back_left" coef=".333" />
<joint joint="ankle_back_left" coef=".333" />
</fixed>
<fixed name="extend_front_left">
<joint joint="pitch_front_left" coef=".25" />
<joint joint="knee_front_left" coef="-.5" />
<joint joint="ankle_front_left" coef=".25" />
</fixed>
<fixed name="lift_front_left">
<joint joint="pitch_front_left" coef=".5" />
<joint joint="ankle_front_left" coef="-.5" />
</fixed>
<fixed name="extend_front_right">
<joint joint="pitch_front_right" coef=".25" />
<joint joint="knee_front_right" coef="-.5" />
<joint joint="ankle_front_right" coef=".25" />
</fixed>
<fixed name="lift_front_right">
<joint joint="pitch_front_right" coef=".5" />
<joint joint="ankle_front_right" coef="-.5" />
</fixed>
<fixed name="extend_back_right">
<joint joint="pitch_back_right" coef=".25" />
<joint joint="knee_back_right" coef="-.5" />
<joint joint="ankle_back_right" coef=".25" />
</fixed>
<fixed name="lift_back_right">
<joint joint="pitch_back_right" coef=".5" />
<joint joint="ankle_back_right" coef="-.5" />
</fixed>
<fixed name="extend_back_left">
<joint joint="pitch_back_left" coef=".25" />
<joint joint="knee_back_left" coef="-.5" />
<joint joint="ankle_back_left" coef=".25" />
</fixed>
<fixed name="lift_back_left">
<joint joint="pitch_back_left" coef=".5" />
<joint joint="ankle_back_left" coef="-.5" />
</fixed>
</tendon>
<equality>
<tendon name="coupling_front_left" tendon1="coupling_front_left" class="coupling" />
<tendon name="coupling_front_right" tendon1="coupling_front_right" class="coupling" />
<tendon name="coupling_back_right" tendon1="coupling_back_right" class="coupling" />
<tendon name="coupling_back_left" tendon1="coupling_back_left" class="coupling" />
</equality>
<actuator>
<general name="yaw_front_left" class="yaw_act" joint="yaw_front_left" />
<general name="lift_front_left" class="lift_act" tendon="lift_front_left" />
<general name="extend_front_left" class="extend_act" tendon="extend_front_left" />
<general name="yaw_front_right" class="yaw_act" joint="yaw_front_right" />
<general name="lift_front_right" class="lift_act" tendon="lift_front_right" />
<general name="extend_front_right" class="extend_act" tendon="extend_front_right" />
<general name="yaw_back_right" class="yaw_act" joint="yaw_back_right" />
<general name="lift_back_right" class="lift_act" tendon="lift_back_right" />
<general name="extend_back_right" class="extend_act" tendon="extend_back_right" />
<general name="yaw_back_left" class="yaw_act" joint="yaw_back_left" />
<general name="lift_back_left" class="lift_act" tendon="lift_back_left" />
<general name="extend_back_left" class="extend_act" tendon="extend_back_left" />
</actuator>
<sensor>
<accelerometer name="imu_accel" site="torso" />
<gyro name="imu_gyro" site="torso" />
<velocimeter name="velocimeter" site="torso" />
<force name="force_toe_front_left" site="toe_front_left" />
<force name="force_toe_front_right" site="toe_front_right" />
<force name="force_toe_back_right" site="toe_back_right" />
<force name="force_toe_back_left" site="toe_back_left" />
<torque name="torque_toe_front_left" site="toe_front_left" />
<torque name="torque_toe_front_right" site="toe_front_right" />
<torque name="torque_toe_back_right" site="toe_back_right" />
<torque name="torque_toe_back_left" site="toe_back_left" />
<subtreecom name="center_of_mass" body="torso" />
</sensor>
</mujoco>

274
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_fetch.xml Normal file
View File

@@ -0,0 +1,274 @@
<mujoco model="quadruped">
<include file="../../common/skybox.xml" />
<include file="../../common/visual.xml" />
<include file="./quadruped_materials.xml" />
<visual>
<rgba rangefinder="1 1 0.1 0.1" />
<map znear=".005" zfar="20" />
</visual>
<asset>
</asset>
<option timestep=".005" />
<default>
<geom solimp=".9 .99 .003" solref=".01 1" />
<default class="body">
<geom type="capsule" size=".08" condim="1" material="self" density="500" contype="1" conaffinity="1" />
<joint type="hinge" damping="30" armature=".01" limited="true" solimplimit="0 .99 .01" />
<default class="hip">
<default class="yaw">
<joint axis="0 0 1" range="-50 50" />
</default>
<default class="pitch">
<joint axis="0 1 0" range="-20 60" />
</default>
<geom fromto="0 0 0 .3 0 .11" />
</default>
<default class="knee">
<joint axis="0 1 0" range="-60 25" />
<geom size=".065" fromto="0 0 0 .25 0 -.25" />
</default>
<default class="ankle">
<joint axis="0 1 0" range="-45 45" />
<geom size=".055" fromto="0 0 0 0 0 -.25" />
</default>
<default class="toe">
<geom type="sphere" size=".08" material="effector" friction="1.5" />
<site type="sphere" size=".084" material="site" group="4" />
</default>
</default>
<default class="rangefinder">
<site type="capsule" size=".005 .1" material="site" group="4" />
</default>
<default class="wall">
<geom type="plane" material="decoration" contype="3" conaffinity="3" />
</default>
<default class="coupling">
<equality solimp="0.95 0.99 0.01" solref=".005 .5" />
</default>
<general ctrllimited="true" gainprm="1000" biasprm="0 -1000" biastype="affine" dyntype="filter" dynprm=".1" />
<default class="yaw_act">
<general ctrlrange="-1 1" />
</default>
<default class="lift_act">
<general ctrlrange="-1 1.1" />
</default>
<default class="extend_act">
<general ctrlrange="-.8 .8" />
</default>
</default>
<asset>
<texture name="ball" builtin="checker" mark="cross" width="151" height="151" rgb1="0.1 0.1 0.1" rgb2="0.9 0.9 0.9" markrgb="1 1 1" />
<material name="ball" texture="ball" />
</asset>
<worldbody>
<geom name="floor" type="plane" size="0 0 0.01" material="motphys-ground" contype="3" conaffinity="3" />
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="wall_px" class="wall" pos="-15.7 0 .7" zaxis="1 0 1" size="1 15 .5" />
<geom name="wall_py" class="wall" pos="0 -15.7 .7" zaxis="0 1 1" size="15 1 .5" />
<geom name="wall_nx" class="wall" pos="15.7 0 .7" zaxis="-1 0 1" size="1 15 .5" />
<geom name="wall_ny" class="wall" pos="0 15.7 .7" zaxis="0 -1 1" size="15 1 .5" />
<site name="target" type="cylinder" size=".4 .06" pos="0 0 .05" material="target" />
<geom name="target_marker" type="cylinder" size=".4 .005" pos="0 0 .005" rgba=".9 .2 .2 0.4" />
<camera name="global" pos="-10 10 10" xyaxes="-1 -1 0 1 0 1" mode="trackcom" />
<body name="torso" childclass="body" pos="0 0 .57">
<freejoint name="root" />
<camera name="x" pos="-1.7 0 1" xyaxes="0 -1 0 .75 0 1" mode="trackcom" />
<camera name="y" pos="0 4 2" xyaxes="-1 0 0 0 -.5 1" mode="trackcom" />
<camera name="egocentric" pos=".3 0 .11" xyaxes="0 -1 0 .4 0 1" fovy="60" />
<geom name="eye_r" type="cylinder" size=".05" fromto=".1 -.07 .12 .31 -.07 .08" mass="0" />
<site name="pupil_r" type="sphere" size=".033" pos=".3 -.07 .08" zaxis="1 0 0" material="eye" />
<geom name="eye_l" type="cylinder" size=".05" fromto=".1 .07 .12 .31 .07 .08" mass="0" />
<site name="pupil_l" type="sphere" size=".033" pos=".3 .07 .08" zaxis="1 0 0" material="eye" />
<site name="workspace" type="sphere" size=".3 .3 .3" material="site" pos=".8 0 -.2" group="3" />
<geom name="torso" type="box" size=".3 .27 .2" density="1000" contype="3" conaffinity="3" />
<geom name="torso_belly" type="box" size=".3 .27 .2" pos="0 0 -.12" mass="0" contype="2" conaffinity="2" />
<site name="torso_touch" type="box" size=".26 .26 .26" rgba="0 0 1 0" />
<site name="torso" size=".05" rgba="1 0 0 1" />
<body name="hip_front_left" pos=".2 .2 0" euler="0 0 45" childclass="hip">
<joint name="yaw_front_left" class="yaw" />
<joint name="pitch_front_left" class="pitch" />
<geom name="thigh_front_left" />
<body name="knee_front_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_left" />
<geom name="shin_front_left" />
<body name="ankle_front_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_left" />
<geom name="foot_front_left" />
<body name="toe_front_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_left" />
<site name="toe_front_left" />
</body>
</body>
</body>
</body>
<body name="hip_front_right" pos=".2 -.2 0" euler="0 0 -45" childclass="hip">
<joint name="yaw_front_right" class="yaw" />
<joint name="pitch_front_right" class="pitch" />
<geom name="thigh_front_right" />
<body name="knee_front_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_right" />
<geom name="shin_front_right" />
<body name="ankle_front_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_right" />
<geom name="foot_front_right" />
<body name="toe_front_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_right" />
<site name="toe_front_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_right" pos="-.2 -.2 0" euler="0 0 -135" childclass="hip">
<joint name="yaw_back_right" class="yaw" />
<joint name="pitch_back_right" class="pitch" />
<geom name="thigh_back_right" />
<body name="knee_back_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_right" />
<geom name="shin_back_right" />
<body name="ankle_back_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_right" />
<geom name="foot_back_right" />
<body name="toe_back_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_right" />
<site name="toe_back_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_left" pos="-.2 .2 0" euler="0 0 135" childclass="hip">
<joint name="yaw_back_left" class="yaw" />
<joint name="pitch_back_left" class="pitch" />
<geom name="thigh_back_left" />
<body name="knee_back_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_left" />
<geom name="shin_back_left" />
<body name="ankle_back_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_left" />
<geom name="foot_back_left" />
<body name="toe_back_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_left" />
<site name="toe_back_left" />
</body>
</body>
</body>
</body>
</body>
<body name="ball" pos="0 0 3">
<freejoint name="ball_root" />
<geom name="ball" size=".3" material="ball" priority="1" condim="6" friction="0.6 .02 .02" mass="1.0" solref="0.02 1" contype="2" conaffinity="2" />
</body>
</worldbody>
<tendon>
<fixed name="coupling_front_left">
<joint joint="pitch_front_left" coef=".333" />
<joint joint="knee_front_left" coef=".333" />
<joint joint="ankle_front_left" coef=".333" />
</fixed>
<fixed name="coupling_front_right">
<joint joint="pitch_front_right" coef=".333" />
<joint joint="knee_front_right" coef=".333" />
<joint joint="ankle_front_right" coef=".333" />
</fixed>
<fixed name="coupling_back_right">
<joint joint="pitch_back_right" coef=".333" />
<joint joint="knee_back_right" coef=".333" />
<joint joint="ankle_back_right" coef=".333" />
</fixed>
<fixed name="coupling_back_left">
<joint joint="pitch_back_left" coef=".333" />
<joint joint="knee_back_left" coef=".333" />
<joint joint="ankle_back_left" coef=".333" />
</fixed>
<fixed name="extend_front_left">
<joint joint="pitch_front_left" coef=".25" />
<joint joint="knee_front_left" coef="-.5" />
<joint joint="ankle_front_left" coef=".25" />
</fixed>
<fixed name="lift_front_left">
<joint joint="pitch_front_left" coef=".5" />
<joint joint="ankle_front_left" coef="-.5" />
</fixed>
<fixed name="extend_front_right">
<joint joint="pitch_front_right" coef=".25" />
<joint joint="knee_front_right" coef="-.5" />
<joint joint="ankle_front_right" coef=".25" />
</fixed>
<fixed name="lift_front_right">
<joint joint="pitch_front_right" coef=".5" />
<joint joint="ankle_front_right" coef="-.5" />
</fixed>
<fixed name="extend_back_right">
<joint joint="pitch_back_right" coef=".25" />
<joint joint="knee_back_right" coef="-.5" />
<joint joint="ankle_back_right" coef=".25" />
</fixed>
<fixed name="lift_back_right">
<joint joint="pitch_back_right" coef=".5" />
<joint joint="ankle_back_right" coef="-.5" />
</fixed>
<fixed name="extend_back_left">
<joint joint="pitch_back_left" coef=".25" />
<joint joint="knee_back_left" coef="-.5" />
<joint joint="ankle_back_left" coef=".25" />
</fixed>
<fixed name="lift_back_left">
<joint joint="pitch_back_left" coef=".5" />
<joint joint="ankle_back_left" coef="-.5" />
</fixed>
</tendon>
<equality>
<tendon name="coupling_front_left" tendon1="coupling_front_left" class="coupling" />
<tendon name="coupling_front_right" tendon1="coupling_front_right" class="coupling" />
<tendon name="coupling_back_right" tendon1="coupling_back_right" class="coupling" />
<tendon name="coupling_back_left" tendon1="coupling_back_left" class="coupling" />
</equality>
<actuator>
<general name="yaw_front_left" class="yaw_act" joint="yaw_front_left" />
<general name="lift_front_left" class="lift_act" tendon="lift_front_left" />
<general name="extend_front_left" class="extend_act" tendon="extend_front_left" />
<general name="yaw_front_right" class="yaw_act" joint="yaw_front_right" />
<general name="lift_front_right" class="lift_act" tendon="lift_front_right" />
<general name="extend_front_right" class="extend_act" tendon="extend_front_right" />
<general name="yaw_back_right" class="yaw_act" joint="yaw_back_right" />
<general name="lift_back_right" class="lift_act" tendon="lift_back_right" />
<general name="extend_back_right" class="extend_act" tendon="extend_back_right" />
<general name="yaw_back_left" class="yaw_act" joint="yaw_back_left" />
<general name="lift_back_left" class="lift_act" tendon="lift_back_left" />
<general name="extend_back_left" class="extend_act" tendon="extend_back_left" />
</actuator>
<sensor>
<framequat name="orientation" objtype="xbody" objname="torso" />
<velocimeter name="velocimeter" site="torso" />
<gyro name="imu_gyro" site="torso" />
<accelerometer name="imu_accel" site="torso" />
</sensor>
</mujoco>

21
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_materials.xml Normal file
View File

@@ -0,0 +1,21 @@
<!-- Quadruped-specific materials that extend the shared env visuals. -->
<mujoco>
<asset>
<texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="300" height="300" mark="edge" markrgb=".2 .3 .4"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png"/>
<material name="motphys-ground" texture="motphys-ground" texuniform="true" texrepeat="0.4 0.4"/>
<material name="self" rgba=".7 .5 .3 1"/>
<material name="self_default" rgba=".7 .5 .3 1"/>
<material name="self_highlight" rgba="0 .5 .3 1"/>
<material name="effector" rgba=".7 .4 .2 1"/>
<material name="effector_default" rgba=".7 .4 .2 1"/>
<material name="effector_highlight" rgba="0 .5 .3 1"/>
<material name="decoration" rgba=".3 .5 .7 1"/>
<material name="eye" rgba="0 .2 1 1"/>
<material name="target" rgba=".6 .3 .3 1"/>
<material name="target_default" rgba=".6 .3 .3 1"/>
<material name="target_highlight" rgba=".6 .3 .3 .4"/>
<material name="site" rgba=".5 .5 .5 .3"/>
</asset>
</mujoco>

893
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_np.py Normal file
View File

@@ -0,0 +1,893 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.basic.quadruped.cfg import QuadrupedBaseCfg
from motrix_envs.math import quaternion
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
_RANGEFINDER_SENSORS = [f"rf_{row}{col}" for row in range(4) for col in range(5)]
class QuadrupedEnv(NpEnv):
_cfg: QuadrupedBaseCfg
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: QuadrupedBaseCfg, num_envs: int = 1):
super().__init__(cfg, num_envs)
self._cfg = cfg
self._torso = self._model.get_link("torso")
self._floor_geom = self._model.get_geom("floor")
self._workspace_site = None
if cfg.include_origin:
self._workspace_site = self._model.get_site("workspace")
self._target_site = None
if cfg.include_target:
self._target_site = self._model.get_site("target")
self._ball_body = None
self._ball_geom = None
if cfg.include_ball:
self._ball_body = self._model.get_body("ball")
self._ball_geom = self._model.get_geom("ball")
self._leg_ball_geoms = []
self._leg_ball_geom_count = 0
self._leg_ball_geom_slices = []
if cfg.include_ball:
try:
leg_body_geom_names = [
["thigh_front_left", "shin_front_left", "foot_front_left", "toe_front_left"],
["thigh_front_right", "shin_front_right", "foot_front_right", "toe_front_right"],
["thigh_back_right", "shin_back_right", "foot_back_right", "toe_back_right"],
["thigh_back_left", "shin_back_left", "foot_back_left", "toe_back_left"],
]
leg_geoms = []
start = 0
for geom_names in leg_body_geom_names:
stop = start
for name in geom_names:
try:
geom = self._model.get_geom(name)
except Exception:
continue
leg_geoms.append(geom)
stop += 1
self._leg_ball_geom_slices.append(slice(start, stop))
start = stop
if leg_geoms:
self._leg_ball_geoms = leg_geoms
self._leg_ball_geom_count = len(leg_geoms)
except Exception:
self._leg_ball_geom_slices = []
self._body_dof_pos = self._model.num_dof_pos - 7 - (7 if cfg.include_ball else 0)
self._body_dof_vel = self._model.num_dof_vel - 6 - (6 if cfg.include_ball else 0)
self._dof_pos_slice = slice(7, 7 + self._body_dof_pos)
self._dof_vel_slice = slice(6, 6 + self._body_dof_vel)
self._ball_pos_slice = None
self._ball_vel_slice = None
if cfg.include_ball:
self._ball_pos_slice = slice(self._model.num_dof_pos - 7, self._model.num_dof_pos)
self._ball_vel_slice = slice(self._model.num_dof_vel - 6, self._model.num_dof_vel)
self._init_dof_pos = self._model.compute_init_dof_pos().astype(np.float32)
self._default_body_dof_pos = self._init_dof_pos[self._dof_pos_slice].copy()
self._terrain_size = float(cfg.terrain_size)
try:
if self._model.num_hfields:
hfield = self._model.get_hfield(0)
self._terrain_size = max(self._terrain_size, float(abs(hfield.bound[3])))
except Exception:
pass
self._init_obs_space()
self._init_action_space()
def _init_obs_space(self):
num_obs = self._body_dof_pos + self._body_dof_vel + self._model.num_actuators
num_obs += 3 # torso velocity
num_obs += 1 # torso upright
num_obs += 6 # imu accel + gyro
if self._cfg.include_origin:
num_obs += 3
if self._cfg.include_rangefinder:
num_obs += len(_RANGEFINDER_SENSORS)
if self._cfg.include_ball:
num_obs += 9
if self._cfg.include_target:
num_obs += 3
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
low, high = self._model.actuator_ctrl_limits
self._action_space = gym.spaces.Box(low, high, (self._model.num_actuators,), dtype=np.float32)
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
def apply_action(self, actions: np.ndarray, state: NpEnvState) -> NpEnvState:
if self._cfg.clip_env_actions:
actions = np.clip(actions, self._action_space.low, self._action_space.high)
actions = actions.astype(np.float32)
if "actions" not in state.info:
state.info["actions"] = np.zeros_like(actions, dtype=np.float32)
if "last_actions" not in state.info:
state.info["last_actions"] = np.zeros_like(actions, dtype=np.float32)
state.info["last_actions"] = state.info["actions"]
state.info["actions"] = actions
state.data.actuator_ctrls = actions
return state
def _sensor_value(self, data: mtx.SceneData, name: str) -> np.ndarray:
value = np.asarray(self._model.get_sensor_value(name, data))
return value.reshape(data.shape[0], -1)
def _sensor_vector(self, data: mtx.SceneData, names: list[str]) -> np.ndarray:
if not names:
return np.zeros((data.shape[0], 0), dtype=np.float32)
values = [self._sensor_value(data, name) for name in names]
return np.concatenate(values, axis=-1)
def _egocentric_state(self, data: mtx.SceneData) -> np.ndarray:
dof_pos = data.dof_pos[:, self._dof_pos_slice]
dof_vel = data.dof_vel[:, self._dof_vel_slice]
act = data.actuator_ctrls
return np.concatenate([dof_pos, dof_vel, act], axis=-1)
def _torso_upright(self, data: mtx.SceneData) -> np.ndarray:
return self._torso.get_rotation_mat(data)[:, 2, 2]
def _torso_velocity(self, data: mtx.SceneData) -> np.ndarray:
return self._sensor_value(data, "velocimeter")
def _imu(self, data: mtx.SceneData) -> np.ndarray:
accel = self._sensor_value(data, "imu_accel")
gyro = self._sensor_value(data, "imu_gyro")
return np.concatenate([accel, gyro], axis=-1)
def _rangefinder(self, data: mtx.SceneData) -> np.ndarray:
readings = self._sensor_vector(data, _RANGEFINDER_SENSORS)
no_intersection = -1.0
return np.where(readings == no_intersection, 1.0, np.tanh(readings))
def _origin(self, data: mtx.SceneData) -> np.ndarray:
torso_pos = self._torso.get_position(data)
torso_frame = self._torso.get_rotation_mat(data)
return -np.einsum("ni,nij->nj", torso_pos, torso_frame)
def _origin_distance(self, data: mtx.SceneData) -> np.ndarray:
workspace_pos = self._workspace_site.get_position(data)
return np.linalg.norm(workspace_pos, axis=-1)
def _ball_state(self, data: mtx.SceneData) -> np.ndarray:
ball_pose = self._ball_body.get_pose(data)
ball_pos = ball_pose[:, :3]
torso_pos = self._torso.get_position(data)
torso_frame = self._torso.get_rotation_mat(data)
ball_rel_pos = ball_pos - torso_pos
root_linvel = data.dof_vel[:, :3]
ball_vel = data.dof_vel[:, self._ball_vel_slice]
ball_rel_vel = ball_vel[:, :3] - root_linvel
ball_rot_vel = ball_vel[:, 3:]
stacked = np.stack([ball_rel_pos, ball_rel_vel, ball_rot_vel], axis=1)
local = np.einsum("nij,njk->nik", stacked, torso_frame)
return local.reshape(data.shape[0], -1)
def _target_position(self, data: mtx.SceneData) -> np.ndarray:
torso_pos = self._torso.get_position(data)
torso_frame = self._torso.get_rotation_mat(data)
to_target = self._target_site.get_position(data) - torso_pos
return np.einsum("ni,nij->nj", to_target, torso_frame)
def _ball_to_target_distance(self, data: mtx.SceneData) -> np.ndarray:
ball_pos = self._ball_body.get_pose(data)[:, :3]
target_pos = self._target_site.get_position(data)
return np.linalg.norm((target_pos - ball_pos)[:, :2], axis=-1)
def _aggregate_leg_ball_proximity(self, geom_penalties: np.ndarray) -> np.ndarray:
num_legs = len(self._leg_ball_geom_slices)
if num_legs == 0:
return np.zeros((geom_penalties.shape[0], 0), dtype=np.float32)
leg_penalties = []
for geom_slice in self._leg_ball_geom_slices:
if geom_slice.start == geom_slice.stop:
leg_penalties.append(np.zeros((geom_penalties.shape[0],), dtype=np.float32))
else:
leg_penalties.append(geom_penalties[:, geom_slice].max(axis=-1))
return np.stack(leg_penalties, axis=-1)
def _point_to_segment_distance(self, point: np.ndarray, start: np.ndarray, end: np.ndarray) -> np.ndarray:
segment = end - start
segment_sq_norm = np.sum(segment * segment, axis=-1)
safe_norm = np.where(segment_sq_norm > 1e-8, segment_sq_norm, 1.0)
t = np.sum((point - start) * segment, axis=-1) / safe_norm
t = np.where(segment_sq_norm > 1e-8, np.clip(t, 0.0, 1.0), 0.0)
closest = start + t[:, None] * segment
return np.linalg.norm(point - closest, axis=-1)
def _geom_ball_surface_clearance(
self, geom: mtx.Geom, ball_pos: np.ndarray, ball_radius: float, data: mtx.SceneData
) -> np.ndarray:
geom_pose = geom.get_pose(data)
geom_pos = geom_pose[:, :3]
geom_quat = geom_pose[:, 3:]
geom_size = np.atleast_1d(np.asarray(geom.size, dtype=np.float32))
geom_radius = float(geom_size[0])
if getattr(geom, "shape", None) == mtx.Shape.Capsule and geom_size.shape[0] > 1 and geom_size[1] > 0.0:
half_length = float(geom_size[1])
axis = quaternion.rotate_vector(geom_quat, np.array([0.0, 0.0, 1.0], dtype=np.float32))
start = geom_pos - axis * half_length
end = geom_pos + axis * half_length
center_distance = self._point_to_segment_distance(ball_pos, start, end)
else:
center_distance = np.linalg.norm(ball_pos - geom_pos, axis=-1)
return center_distance - (ball_radius + geom_radius)
def _leg_body_ball_penalty(self, data: mtx.SceneData) -> np.ndarray:
num_legs = len(self._leg_ball_geom_slices)
if self._ball_geom is None or self._leg_ball_geom_count == 0 or num_legs == 0:
return np.zeros((data.shape[0],), dtype=np.float32)
ball_pos = self._ball_geom.get_pose(data)[:, :3]
ball_radius = float(np.atleast_1d(self._ball_geom.size)[0])
geom_penalties = []
for geom in self._leg_ball_geoms:
clearance = self._geom_ball_surface_clearance(geom, ball_pos, ball_radius, data)
geom_penalties.append(
reward.tolerance(
-clearance,
bounds=(0.0, float("inf")),
margin=self._cfg.fetch_leg_ball_penalty_margin,
value_at_margin=0.0,
sigmoid="linear",
)
)
proximity = np.stack(geom_penalties, axis=-1).astype(np.float32)
leg_penalties = self._aggregate_leg_ball_proximity(proximity)
return leg_penalties.sum(axis=-1).astype(np.float32)
def _fetch_stability_gate(self, torso_upright: np.ndarray, torso_height: np.ndarray) -> np.ndarray:
upright_gate = reward.tolerance(
torso_upright,
bounds=(self._cfg.fetch_stability_upright_min, float("inf")),
margin=self._cfg.fetch_stability_upright_margin,
value_at_margin=0.0,
sigmoid="linear",
)
height_gate = reward.tolerance(
torso_height,
bounds=(self._cfg.fetch_stability_height_min, float("inf")),
margin=self._cfg.fetch_stability_height_margin,
value_at_margin=0.0,
sigmoid="linear",
)
return upright_gate * height_gate
def _fetch_fall_terminated(self, torso_upright: np.ndarray, torso_height: np.ndarray) -> np.ndarray:
return (torso_upright < self._cfg.fetch_fall_upright_min) | (torso_height < self._cfg.fetch_fall_height_min)
def _upright_reward(self, torso_upright: np.ndarray) -> np.ndarray:
deviation = float(np.cos(np.deg2rad(self._cfg.deviation_angle)))
return reward.tolerance(
torso_upright,
bounds=(deviation, float("inf")),
margin=1 + deviation,
value_at_margin=0.0,
sigmoid="linear",
)
def _move_reward(self, torso_vel: np.ndarray) -> np.ndarray:
return reward.tolerance(
torso_vel[:, 0],
bounds=(self._cfg.desired_speed, float("inf")),
margin=self._cfg.desired_speed,
value_at_margin=0.5,
sigmoid="linear",
)
def _backward_penalty(self, torso_vel: np.ndarray) -> np.ndarray:
return np.maximum(0.0, -torso_vel[:, 0])
def _escape_reward(self, data: mtx.SceneData) -> np.ndarray:
return reward.tolerance(
self._origin_distance(data),
bounds=(self._terrain_size, float("inf")),
margin=self._terrain_size,
value_at_margin=0.0,
sigmoid="linear",
)
def _radial_speed_reward(self, data: mtx.SceneData) -> np.ndarray:
radial_speed_reward = np.zeros((data.shape[0],), dtype=np.float32)
if not self._cfg.include_origin:
return radial_speed_reward
torso_pos = self._torso.get_position(data)
radial_vec = torso_pos[:, :2]
radial_norm = np.linalg.norm(radial_vec, axis=-1, keepdims=True)
radial_dir = np.divide(radial_vec, radial_norm, out=np.zeros_like(radial_vec), where=radial_norm > 1e-6)
radial_speed = np.sum(data.dof_vel[:, :2] * radial_dir, axis=-1)
radial_speed = np.maximum(0.0, radial_speed)
return reward.tolerance(
radial_speed,
bounds=(self._cfg.desired_speed, float("inf")),
margin=self._cfg.desired_speed,
value_at_margin=0.5,
sigmoid="linear",
)
def _heading_reward(self, data: mtx.SceneData) -> np.ndarray:
heading_reward = np.zeros((data.shape[0],), dtype=np.float32)
if self._cfg.heading_reward_weight <= 0.0:
return heading_reward
torso_frame = self._torso.get_rotation_mat(data)
heading_xy = torso_frame[:, 0, :2]
heading_norm = np.linalg.norm(heading_xy, axis=-1, keepdims=True)
heading_dir = np.divide(heading_xy, heading_norm, out=np.zeros_like(heading_xy), where=heading_norm > 1e-6)
heading_align = heading_dir[:, 0]
return reward.tolerance(
heading_align,
bounds=(1.0, 1.0),
margin=self._cfg.heading_reward_margin,
value_at_margin=0.0,
sigmoid="linear",
)
def _height_reward(self, data: mtx.SceneData) -> np.ndarray:
torso_height = self._torso.get_position(data)[:, 2]
return reward.tolerance(
torso_height,
bounds=(self._cfg.stand_height, float("inf")),
margin=self._cfg.stand_height_margin,
value_at_margin=0.0,
sigmoid="linear",
)
def _lateral_reward(self, torso_vel: np.ndarray) -> np.ndarray:
return reward.tolerance(
np.abs(torso_vel[:, 1]),
bounds=(0.0, self._cfg.lateral_velocity_limit),
margin=self._cfg.lateral_velocity_margin,
value_at_margin=0.0,
sigmoid="linear",
)
def _smooth_reward(self, state: NpEnvState) -> np.ndarray:
smooth_reward = np.zeros((state.data.shape[0],), dtype=np.float32)
if "actions" not in state.info or "last_actions" not in state.info:
return smooth_reward
delta = state.info["actions"] - state.info["last_actions"]
delta_norm = np.linalg.norm(delta, axis=-1)
return reward.tolerance(
delta_norm,
bounds=(0.0, 0.0),
margin=self._cfg.action_smoothness_margin,
value_at_margin=0.0,
sigmoid="linear",
)
def _lin_vel_z_penalty(self, torso_vel: np.ndarray) -> np.ndarray:
return np.square(torso_vel[:, 2]).astype(np.float32)
def _ang_vel_xy_penalty(self, data: mtx.SceneData) -> np.ndarray:
imu = self._imu(data)
return np.sum(np.square(imu[:, 3:5]), axis=1).astype(np.float32)
def _similar_to_default_penalty(self, data: mtx.SceneData) -> np.ndarray:
body_dof_pos = data.dof_pos[:, self._dof_pos_slice]
return np.sum(np.abs(body_dof_pos - self._default_body_dof_pos), axis=1).astype(np.float32)
def _locomotion_reward_terms(
self,
upright_reward: np.ndarray,
move_reward: np.ndarray,
backward_penalty: np.ndarray,
height_reward: np.ndarray,
lateral_reward: np.ndarray,
heading_reward: np.ndarray,
smooth_reward: np.ndarray,
lin_vel_z_penalty: np.ndarray,
ang_vel_xy_penalty: np.ndarray,
similar_to_default_penalty: np.ndarray,
) -> dict[str, np.ndarray]:
return {
"move": upright_reward * move_reward,
"backward": backward_penalty,
"height": height_reward,
"lateral": lateral_reward,
"heading": heading_reward,
"smooth": smooth_reward,
"lin_vel_z": lin_vel_z_penalty,
"ang_vel_xy": ang_vel_xy_penalty,
"similar_to_default": similar_to_default_penalty,
}
def _locomotion_reward_scales(self) -> dict[str, float]:
return {
"move": 1.0,
"backward": -self._cfg.backward_penalty_weight,
"height": self._cfg.height_reward_weight,
"lateral": self._cfg.lateral_reward_weight,
"heading": self._cfg.heading_reward_weight,
"smooth": self._cfg.action_smoothness_weight,
"lin_vel_z": -self._cfg.lin_vel_z_weight,
"ang_vel_xy": -self._cfg.ang_vel_xy_weight,
"similar_to_default": -self._cfg.similar_to_default_weight,
}
def _escape_reward_terms(
self, upright_reward: np.ndarray, escape_reward: np.ndarray, radial_speed_reward: np.ndarray
) -> dict[str, np.ndarray]:
return {
"escape": upright_reward * escape_reward,
"radial": radial_speed_reward,
}
def _escape_reward_scales(self) -> dict[str, float]:
return {
"escape": 1.0,
"radial": self._cfg.radial_velocity_weight,
}
def _sum_scaled_rewards(self, reward_terms: dict[str, np.ndarray], reward_scales: dict[str, float]) -> np.ndarray:
rewards = {name: value * reward_scales[name] for name, value in reward_terms.items()}
return sum(rewards.values())
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
parts = [
self._egocentric_state(data),
self._torso_velocity(data),
self._torso_upright(data).reshape(data.shape[0], 1),
self._imu(data),
]
if self._cfg.include_origin:
parts.append(self._origin(data))
if self._cfg.include_rangefinder:
parts.append(self._rangefinder(data))
if self._cfg.include_ball:
parts.append(self._ball_state(data))
if self._cfg.include_target:
parts.append(self._target_position(data))
return np.concatenate(parts, axis=-1).astype(np.float32)
def _locomotion_reward_info(self, num_envs: int) -> dict:
return {
"upright": np.zeros((num_envs,), dtype=np.float32),
"move": np.zeros((num_envs,), dtype=np.float32),
"backward": np.zeros((num_envs,), dtype=np.float32),
"height": np.zeros((num_envs,), dtype=np.float32),
"lateral": np.zeros((num_envs,), dtype=np.float32),
"heading": np.zeros((num_envs,), dtype=np.float32),
"smooth": np.zeros((num_envs,), dtype=np.float32),
"lin_vel_z": np.zeros((num_envs,), dtype=np.float32),
"ang_vel_xy": np.zeros((num_envs,), dtype=np.float32),
"similar_to_default": np.zeros((num_envs,), dtype=np.float32),
"total": np.zeros((num_envs,), dtype=np.float32),
}
def _escape_reward_info(self, num_envs: int) -> dict:
info = self._locomotion_reward_info(num_envs)
info.update(
{
"escape": np.zeros((num_envs,), dtype=np.float32),
"radial": np.zeros((num_envs,), dtype=np.float32),
}
)
return info
def _fetch_reward_info(self, num_envs: int) -> dict:
return {
"upright": np.zeros((num_envs,), dtype=np.float32),
"stage_move": np.zeros((num_envs,), dtype=np.float32),
"stage_reach": np.zeros((num_envs,), dtype=np.float32),
"stability": np.zeros((num_envs,), dtype=np.float32),
"behind_align": np.zeros((num_envs,), dtype=np.float32),
"face_ball": np.zeros((num_envs,), dtype=np.float32),
"near_ball": np.zeros((num_envs,), dtype=np.float32),
"ready": np.zeros((num_envs,), dtype=np.float32),
"ready_gate": np.zeros((num_envs,), dtype=np.float32),
"fetch": np.zeros((num_envs,), dtype=np.float32),
"push": np.zeros((num_envs,), dtype=np.float32),
"away": np.zeros((num_envs,), dtype=np.float32),
"leg_ball": np.zeros((num_envs,), dtype=np.float32),
"backward": np.zeros((num_envs,), dtype=np.float32),
"total": np.zeros((num_envs,), dtype=np.float32),
}
def _base_locomotion_components(self, data: mtx.SceneData, state: NpEnvState) -> dict[str, np.ndarray]:
torso_vel = self._torso_velocity(data)
return {
"move": self._move_reward(torso_vel),
"backward": self._backward_penalty(torso_vel),
"height": self._height_reward(data),
"lateral": self._lateral_reward(torso_vel),
"heading": self._heading_reward(data),
"smooth": self._smooth_reward(state),
"lin_vel_z": self._lin_vel_z_penalty(torso_vel),
"ang_vel_xy": self._ang_vel_xy_penalty(data),
"similar_to_default": self._similar_to_default_penalty(data),
}
def _locomotion_reward(self, upright_reward: np.ndarray, components: dict[str, np.ndarray]) -> np.ndarray:
reward_terms = self._locomotion_reward_terms(
upright_reward,
components["move"],
components["backward"],
components["height"],
components["lateral"],
components["heading"],
components["smooth"],
components["lin_vel_z"],
components["ang_vel_xy"],
components["similar_to_default"],
)
return self._sum_scaled_rewards(reward_terms, self._locomotion_reward_scales())
def _build_reset_info(self, num_envs: int) -> dict:
return {
"Reward": self._init_reward_info(num_envs),
"actions": np.zeros((num_envs, self._model.num_actuators), dtype=np.float32),
"last_actions": np.zeros((num_envs, self._model.num_actuators), dtype=np.float32),
}
def _random_quaternion(self, num: int) -> np.ndarray:
q = np.random.randn(num, 4).astype(np.float32)
q /= np.linalg.norm(q, axis=-1, keepdims=True)
return q
def _yaw_quaternion(self, yaw: np.ndarray) -> np.ndarray:
zeros = np.zeros_like(yaw)
half = yaw * 0.5
return np.stack([zeros, zeros, np.sin(half), np.cos(half)], axis=-1).astype(np.float32)
def _lift_non_contacting(self, data: mtx.SceneData, dof_pos: np.ndarray) -> np.ndarray:
z = dof_pos[:, 2].copy()
pending = np.ones((data.shape[0],), dtype=bool)
for _ in range(1000):
if not pending.any():
break
dof_pos[pending, 2] = z[pending]
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
num_contacts = self._model.get_contact_query(data).num_contacts
pending = num_contacts > 0
z[pending] += 0.01
return dof_pos
def _finish_reset(self, data: mtx.SceneData, dof_pos: np.ndarray, dof_vel: np.ndarray) -> tuple[np.ndarray, dict]:
dof_pos = self._lift_non_contacting(data, dof_pos)
data.set_dof_pos(dof_pos, self._model)
data.set_dof_vel(dof_vel)
self._model.forward_kinematic(data)
obs = self._get_obs(data)
info = self._build_reset_info(int(data.shape[0]))
return obs, info
@registry.env("dm-quadruped-walk", "np")
@registry.env("dm-quadruped-run", "np")
class QuadrupedLocomotionEnv(QuadrupedEnv):
def _init_reward_info(self, num_envs: int) -> dict:
return self._locomotion_reward_info(num_envs)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
torso_upright = self._torso_upright(data)
upright_reward = self._upright_reward(torso_upright)
locomotion_components = self._base_locomotion_components(data, state)
rwd = self._locomotion_reward(upright_reward, locomotion_components)
reward_components = {"upright": upright_reward}
reward_components.update(locomotion_components)
reward_components["total"] = rwd
terminated = np.isnan(obs).any(axis=-1)
rwd = np.where(terminated, 0.0, rwd).astype(np.float32)
state.info["Reward"] = reward_components
return state.replace(obs=obs, reward=rwd, terminated=terminated)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num = int(data.shape[0])
dof_pos = np.tile(self._init_dof_pos, (num, 1))
dof_vel = np.zeros((num, self._model.num_dof_vel), dtype=np.float32)
if self._cfg.fix_heading:
dof_pos[:, 3:7] = np.tile(np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), (num, 1))
else:
dof_pos[:, 3:7] = self._random_quaternion(num)
return self._finish_reset(data, dof_pos, dof_vel)
@registry.env("dm-quadruped-escape", "np")
class QuadrupedEscapeEnv(QuadrupedLocomotionEnv):
def _init_reward_info(self, num_envs: int) -> dict:
return self._escape_reward_info(num_envs)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
torso_upright = self._torso_upright(data)
upright_reward = self._upright_reward(torso_upright)
locomotion_components = self._base_locomotion_components(data, state)
escape_reward = self._escape_reward(data)
radial_speed_reward = self._radial_speed_reward(data)
reward_terms = self._locomotion_reward_terms(
upright_reward,
locomotion_components["move"],
locomotion_components["backward"],
locomotion_components["height"],
locomotion_components["lateral"],
locomotion_components["heading"],
locomotion_components["smooth"],
locomotion_components["lin_vel_z"],
locomotion_components["ang_vel_xy"],
locomotion_components["similar_to_default"],
)
reward_scales = self._locomotion_reward_scales()
reward_terms.update(self._escape_reward_terms(upright_reward, escape_reward, radial_speed_reward))
reward_scales.update(self._escape_reward_scales())
rwd = self._sum_scaled_rewards(reward_terms, reward_scales)
reward_components = {"upright": upright_reward}
reward_components.update(locomotion_components)
reward_components.update(
{
"escape": escape_reward,
"radial": radial_speed_reward,
"total": rwd,
}
)
terminated = np.isnan(obs).any(axis=-1)
rwd = np.where(terminated, 0.0, rwd).astype(np.float32)
state.info["Reward"] = reward_components
return state.replace(obs=obs, reward=rwd, terminated=terminated)
@registry.env("dm-quadruped-fetch", "np")
class QuadrupedFetchEnv(QuadrupedEnv):
def _init_reward_info(self, num_envs: int) -> dict:
return self._fetch_reward_info(num_envs)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
torso_upright = self._torso_upright(data)
upright_reward = self._upright_reward(torso_upright)
torso_height = self._torso.get_position(data)[:, 2]
stability_gate = self._fetch_stability_gate(torso_upright, torso_height)
target_radius = float(self._cfg.target_radius)
if self._target_site is not None:
try:
target_radius = float(np.atleast_1d(self._target_site.size)[0])
except Exception:
pass
ball_pos = self._ball_body.get_pose(data)[:, :3]
target_pos = self._target_site.get_position(data)
torso_pos = self._torso.get_position(data)
to_target = target_pos[:, :2] - ball_pos[:, :2]
to_target_norm = np.linalg.norm(to_target, axis=-1, keepdims=True)
to_target_dir = np.where(to_target_norm > 1e-6, to_target / to_target_norm, 0.0)
torso_frame = self._torso.get_rotation_mat(data)
heading_xy = torso_frame[:, 0, :2]
heading_norm = np.linalg.norm(heading_xy, axis=-1, keepdims=True)
heading_dir = np.where(heading_norm > 1e-6, heading_xy / heading_norm, 0.0)
to_ball = ball_pos[:, :2] - torso_pos[:, :2]
to_ball_norm = np.linalg.norm(to_ball, axis=-1, keepdims=True)
to_ball_dir = np.where(to_ball_norm > 1e-6, to_ball / to_ball_norm, 0.0)
behind_align = np.sum(to_ball_dir * to_target_dir, axis=-1)
behind_align_reward = reward.tolerance(
behind_align,
bounds=(1.0, 1.0),
margin=self._cfg.fetch_behind_align_margin,
value_at_margin=0.0,
sigmoid="linear",
)
heading_align = np.sum(heading_dir * to_ball_dir, axis=-1)
face_ball_reward = reward.tolerance(
heading_align,
bounds=(1.0, 1.0),
margin=self._cfg.fetch_heading_margin,
value_at_margin=0.0,
sigmoid="linear",
)
ball_to_robot = torso_pos[:, :2] - ball_pos[:, :2]
back_dir = -to_target_dir
corridor_lat = np.linalg.norm(
ball_to_robot - np.sum(ball_to_robot * back_dir, axis=-1, keepdims=True) * back_dir,
axis=-1,
)
corridor_reward = reward.tolerance(
corridor_lat,
bounds=(0.0, self._cfg.fetch_corridor_width),
margin=self._cfg.fetch_corridor_width,
value_at_margin=0.0,
sigmoid="linear",
)
ball_dist = np.linalg.norm(to_ball, axis=-1)
near_ball_reward = reward.tolerance(
ball_dist,
bounds=(0.0, self._cfg.fetch_ready_ball_distance),
margin=self._cfg.fetch_ready_ball_distance,
value_at_margin=0.0,
sigmoid="linear",
)
ready = behind_align_reward * face_ball_reward * corridor_reward * near_ball_reward
ready_gate = reward.tolerance(
ready,
bounds=(self._cfg.fetch_ready_threshold, 1.0),
margin=1.0 - self._cfg.fetch_ready_threshold,
value_at_margin=0.0,
sigmoid="linear",
)
behind_pos = ball_pos[:, :2] + back_dir * self._cfg.fetch_behind_distance
ahead_pos = ball_pos[:, :2] + to_target_dir * self._cfg.fetch_ahead_distance
stage_pos = (1.0 - ready_gate)[:, None] * behind_pos + ready_gate[:, None] * ahead_pos
if self._cfg.fetch_side_stage_offset > 0.0:
side_dir = np.stack([-back_dir[:, 1], back_dir[:, 0]], axis=-1)
ball_to_robot_side = np.sum(ball_to_robot * side_dir, axis=-1)
side_sign = np.where(ball_to_robot_side >= 0.0, 1.0, -1.0)
side_pos = behind_pos + (side_sign[:, None] * side_dir * self._cfg.fetch_side_stage_offset)
use_side_stage = (
(ready_gate < self._cfg.fetch_side_stage_gate_threshold)
& (ball_dist < self._cfg.fetch_side_stage_ball_distance)
& (behind_align < self._cfg.fetch_side_stage_align_threshold)
)
stage_pos = np.where(use_side_stage[:, None], side_pos, stage_pos)
to_stage = stage_pos - torso_pos[:, :2]
stage_dist = np.linalg.norm(to_stage, axis=-1)
stage_dir = np.where(stage_dist[:, None] > 1e-6, to_stage / stage_dist[:, None], 0.0)
speed_to_stage = np.sum(data.dof_vel[:, :2] * stage_dir, axis=-1)
stage_move = reward.tolerance(
speed_to_stage,
bounds=(self._cfg.fetch_stage_speed, float("inf")),
margin=self._cfg.fetch_stage_speed,
value_at_margin=0.0,
sigmoid="linear",
)
backward_penalty = np.maximum(0.0, -speed_to_stage)
stage_reach = reward.tolerance(
stage_dist,
bounds=(0.0, self._cfg.fetch_stage_radius),
margin=self._cfg.fetch_stage_radius,
value_at_margin=0.0,
sigmoid="linear",
)
fetch_reward = reward.tolerance(
self._ball_to_target_distance(data),
bounds=(0.0, target_radius),
margin=self._cfg.fetch_reward_margin,
value_at_margin=0.0,
sigmoid="linear",
)
ball_vel = data.dof_vel[:, self._ball_vel_slice][:, :2]
ball_speed_to_target = np.sum(ball_vel * to_target_dir, axis=-1)
push_reward = reward.tolerance(
np.maximum(0.0, ball_speed_to_target),
bounds=(self._cfg.fetch_push_speed, float("inf")),
margin=self._cfg.fetch_push_speed,
value_at_margin=0.0,
sigmoid="linear",
)
away_penalty = (1.0 - ready_gate) * np.maximum(0.0, -ball_speed_to_target)
leg_ball_penalty = self._leg_body_ball_penalty(data)
rwd = stability_gate * upright_reward * stage_move
rwd -= self._cfg.fetch_backward_penalty_weight * backward_penalty
rwd += self._cfg.fetch_stage_reward_weight * (stability_gate * stage_reach)
rwd += self._cfg.fetch_heading_weight * (stability_gate * face_ball_reward)
rwd += self._cfg.fetch_ready_weight * (stability_gate * ready)
rwd += self._cfg.fetch_reward_weight * (stability_gate * ready_gate * fetch_reward)
rwd += self._cfg.fetch_push_reward_weight * (stability_gate * ready_gate * push_reward)
rwd -= self._cfg.fetch_away_penalty_weight * away_penalty
rwd -= self._cfg.fetch_leg_ball_penalty_weight * leg_ball_penalty
reward_components = {
"upright": upright_reward,
"stage_move": stage_move,
"stage_reach": stage_reach,
"stability": stability_gate,
"behind_align": behind_align_reward,
"face_ball": face_ball_reward,
"near_ball": near_ball_reward,
"ready": ready,
"ready_gate": ready_gate,
"fetch": fetch_reward,
"push": push_reward,
"away": away_penalty,
"leg_ball": leg_ball_penalty,
"backward": backward_penalty,
"total": rwd,
}
terminated = np.isnan(obs).any(axis=-1)
terminated |= self._fetch_fall_terminated(torso_upright, torso_height)
rwd = np.where(terminated, 0.0, rwd).astype(np.float32)
for key, value in reward_components.items():
reward_components[key] = np.where(terminated, 0.0, value).astype(np.float32)
state.info["Reward"] = reward_components
return state.replace(obs=obs, reward=rwd, terminated=terminated)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num = int(data.shape[0])
dof_pos = np.tile(self._init_dof_pos, (num, 1))
dof_vel = np.zeros((num, self._model.num_dof_vel), dtype=np.float32)
floor_radius = float(self._floor_geom.size[0])
if floor_radius <= 0.0:
floor_radius = self._terrain_size
spawn_radius = 0.12 * floor_radius
yaw = np.random.uniform(0.0, 2 * np.pi, size=(num,))
dof_pos[:, 0] = np.random.uniform(-spawn_radius, spawn_radius, size=(num,))
dof_pos[:, 1] = np.random.uniform(-spawn_radius, spawn_radius, size=(num,))
dof_pos[:, 3:7] = self._yaw_quaternion(yaw)
ball_xy = np.random.uniform(-spawn_radius, spawn_radius, size=(num, 2))
ball_qpos = self._ball_pos_slice
dof_pos[:, ball_qpos.start : ball_qpos.start + 2] = ball_xy
ball_radius = float(self._ball_geom.size[0]) if self._ball_geom is not None else 0.15
dof_pos[:, ball_qpos.start + 2] = ball_radius
dof_pos[:, ball_qpos.start + 3 : ball_qpos.stop] = np.tile(
np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), (num, 1)
)
ball_qvel = self._ball_vel_slice
dof_vel[:, ball_qvel.start : ball_qvel.stop] = 0.0
return self._finish_reset(data, dof_pos, dof_vel)

269
motrix_envs/src/motrix_envs/basic/quadruped/quadruped_walk.xml Normal file
View File

@@ -0,0 +1,269 @@
<mujoco model="quadruped">
<include file="../../common/skybox.xml" />
<include file="../../common/visual.xml" />
<include file="./quadruped_materials.xml" />
<visual>
<rgba rangefinder="1 1 0.1 0.1" />
<map znear=".005" zfar="20" />
</visual>
<asset>
</asset>
<option timestep=".005" />
<default>
<geom solimp=".9 .99 .003" solref=".01 1" />
<default class="body">
<geom type="capsule" size=".08" condim="1" material="self" density="500" />
<joint type="hinge" damping="30" armature=".01" limited="true" solimplimit="0 .99 .01" />
<default class="hip">
<default class="yaw">
<joint axis="0 0 1" range="-50 50" />
</default>
<default class="pitch">
<joint axis="0 1 0" range="-20 60" />
</default>
<geom fromto="0 0 0 .3 0 .11" />
</default>
<default class="knee">
<joint axis="0 1 0" range="-60 25" />
<geom size=".065" fromto="0 0 0 .25 0 -.25" />
</default>
<default class="ankle">
<joint axis="0 1 0" range="-45 45" />
<geom size=".055" fromto="0 0 0 0 0 -.25" />
</default>
<default class="toe">
<geom type="sphere" size=".08" material="effector" friction="1.5" />
<site type="sphere" size=".084" material="site" group="4" />
</default>
</default>
<default class="rangefinder">
<site type="capsule" size=".005 .1" material="site" group="4" />
</default>
<default class="wall">
<geom type="plane" material="decoration" />
</default>
<default class="coupling">
<equality solimp="0.95 0.99 0.01" solref=".005 .5" />
</default>
<general ctrllimited="true" gainprm="1000" biasprm="0 -1000" biastype="affine" dyntype="filter" dynprm=".1" />
<default class="yaw_act">
<general ctrlrange="-1 1" />
</default>
<default class="lift_act">
<general ctrlrange="-1 1.1" />
</default>
<default class="extend_act">
<general ctrlrange="-.8 .8" />
</default>
</default>
<asset>
<texture name="ball" builtin="checker" mark="cross" width="151" height="151" rgb1="0.1 0.1 0.1" rgb2="0.9 0.9 0.9" markrgb="1 1 1" />
<material name="ball" texture="ball" />
</asset>
<worldbody>
<geom name="floor" type="plane" size="0 0 0.01" material="motphys-ground" />
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<camera name="global" pos="-10 10 10" xyaxes="-1 -1 0 1 0 1" mode="trackcom" />
<body name="torso" childclass="body" pos="0 0 .57">
<freejoint name="root" />
<camera name="x" pos="-1.7 0 1" xyaxes="0 -1 0 .75 0 1" mode="trackcom" />
<camera name="y" pos="0 4 2" xyaxes="-1 0 0 0 -.5 1" mode="trackcom" />
<camera name="egocentric" pos=".3 0 .11" xyaxes="0 -1 0 .4 0 1" fovy="60" />
<geom name="eye_r" type="cylinder" size=".05" fromto=".1 -.07 .12 .31 -.07 .08" mass="0" />
<site name="pupil_r" type="sphere" size=".033" pos=".3 -.07 .08" zaxis="1 0 0" material="eye" />
<geom name="eye_l" type="cylinder" size=".05" fromto=".1 .07 .12 .31 .07 .08" mass="0" />
<site name="pupil_l" type="sphere" size=".033" pos=".3 .07 .08" zaxis="1 0 0" material="eye" />
<site name="workspace" type="sphere" size=".3 .3 .3" material="site" pos=".8 0 -.2" group="3" />
<geom name="torso" type="box" size=".3 .27 .2" density="1000" />
<site name="torso_touch" type="box" size=".26 .26 .26" rgba="0 0 1 0" />
<site name="torso" size=".05" rgba="1 0 0 1" />
<body name="hip_front_left" pos=".2 .2 0" euler="0 0 45" childclass="hip">
<joint name="yaw_front_left" class="yaw" />
<joint name="pitch_front_left" class="pitch" />
<geom name="thigh_front_left" />
<body name="knee_front_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_left" />
<geom name="shin_front_left" />
<body name="ankle_front_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_left" />
<geom name="foot_front_left" />
<body name="toe_front_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_left" />
<site name="toe_front_left" />
</body>
</body>
</body>
</body>
<body name="hip_front_right" pos=".2 -.2 0" euler="0 0 -45" childclass="hip">
<joint name="yaw_front_right" class="yaw" />
<joint name="pitch_front_right" class="pitch" />
<geom name="thigh_front_right" />
<body name="knee_front_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_front_right" />
<geom name="shin_front_right" />
<body name="ankle_front_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_front_right" />
<geom name="foot_front_right" />
<body name="toe_front_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_front_right" />
<site name="toe_front_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_right" pos="-.2 -.2 0" euler="0 0 -135" childclass="hip">
<joint name="yaw_back_right" class="yaw" />
<joint name="pitch_back_right" class="pitch" />
<geom name="thigh_back_right" />
<body name="knee_back_right" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_right" />
<geom name="shin_back_right" />
<body name="ankle_back_right" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_right" />
<geom name="foot_back_right" />
<body name="toe_back_right" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_right" />
<site name="toe_back_right" />
</body>
</body>
</body>
</body>
<body name="hip_back_left" pos="-.2 .2 0" euler="0 0 135" childclass="hip">
<joint name="yaw_back_left" class="yaw" />
<joint name="pitch_back_left" class="pitch" />
<geom name="thigh_back_left" />
<body name="knee_back_left" pos=".3 0 .11" childclass="knee">
<joint name="knee_back_left" />
<geom name="shin_back_left" />
<body name="ankle_back_left" pos=".25 0 -.25" childclass="ankle">
<joint name="ankle_back_left" />
<geom name="foot_back_left" />
<body name="toe_back_left" pos="0 0 -.3" childclass="toe">
<geom name="toe_back_left" />
<site name="toe_back_left" />
</body>
</body>
</body>
</body>
</body>
</worldbody>
<tendon>
<fixed name="coupling_front_left">
<joint joint="pitch_front_left" coef=".333" />
<joint joint="knee_front_left" coef=".333" />
<joint joint="ankle_front_left" coef=".333" />
</fixed>
<fixed name="coupling_front_right">
<joint joint="pitch_front_right" coef=".333" />
<joint joint="knee_front_right" coef=".333" />
<joint joint="ankle_front_right" coef=".333" />
</fixed>
<fixed name="coupling_back_right">
<joint joint="pitch_back_right" coef=".333" />
<joint joint="knee_back_right" coef=".333" />
<joint joint="ankle_back_right" coef=".333" />
</fixed>
<fixed name="coupling_back_left">
<joint joint="pitch_back_left" coef=".333" />
<joint joint="knee_back_left" coef=".333" />
<joint joint="ankle_back_left" coef=".333" />
</fixed>
<fixed name="extend_front_left">
<joint joint="pitch_front_left" coef=".25" />
<joint joint="knee_front_left" coef="-.5" />
<joint joint="ankle_front_left" coef=".25" />
</fixed>
<fixed name="lift_front_left">
<joint joint="pitch_front_left" coef=".5" />
<joint joint="ankle_front_left" coef="-.5" />
</fixed>
<fixed name="extend_front_right">
<joint joint="pitch_front_right" coef=".25" />
<joint joint="knee_front_right" coef="-.5" />
<joint joint="ankle_front_right" coef=".25" />
</fixed>
<fixed name="lift_front_right">
<joint joint="pitch_front_right" coef=".5" />
<joint joint="ankle_front_right" coef="-.5" />
</fixed>
<fixed name="extend_back_right">
<joint joint="pitch_back_right" coef=".25" />
<joint joint="knee_back_right" coef="-.5" />
<joint joint="ankle_back_right" coef=".25" />
</fixed>
<fixed name="lift_back_right">
<joint joint="pitch_back_right" coef=".5" />
<joint joint="ankle_back_right" coef="-.5" />
</fixed>
<fixed name="extend_back_left">
<joint joint="pitch_back_left" coef=".25" />
<joint joint="knee_back_left" coef="-.5" />
<joint joint="ankle_back_left" coef=".25" />
</fixed>
<fixed name="lift_back_left">
<joint joint="pitch_back_left" coef=".5" />
<joint joint="ankle_back_left" coef="-.5" />
</fixed>
</tendon>
<equality>
<tendon name="coupling_front_left" tendon1="coupling_front_left" class="coupling" />
<tendon name="coupling_front_right" tendon1="coupling_front_right" class="coupling" />
<tendon name="coupling_back_right" tendon1="coupling_back_right" class="coupling" />
<tendon name="coupling_back_left" tendon1="coupling_back_left" class="coupling" />
</equality>
<actuator>
<general name="yaw_front_left" class="yaw_act" joint="yaw_front_left" />
<general name="lift_front_left" class="lift_act" tendon="lift_front_left" />
<general name="extend_front_left" class="extend_act" tendon="extend_front_left" />
<general name="yaw_front_right" class="yaw_act" joint="yaw_front_right" />
<general name="lift_front_right" class="lift_act" tendon="lift_front_right" />
<general name="extend_front_right" class="extend_act" tendon="extend_front_right" />
<general name="yaw_back_right" class="yaw_act" joint="yaw_back_right" />
<general name="lift_back_right" class="lift_act" tendon="lift_back_right" />
<general name="extend_back_right" class="extend_act" tendon="extend_back_right" />
<general name="yaw_back_left" class="yaw_act" joint="yaw_back_left" />
<general name="lift_back_left" class="lift_act" tendon="lift_back_left" />
<general name="extend_back_left" class="extend_act" tendon="extend_back_left" />
</actuator>
<sensor>
<accelerometer name="imu_accel" site="torso" />
<gyro name="imu_gyro" site="torso" />
<velocimeter name="velocimeter" site="torso" />
<force name="force_toe_front_left" site="toe_front_left" />
<force name="force_toe_front_right" site="toe_front_right" />
<force name="force_toe_back_right" site="toe_back_right" />
<force name="force_toe_back_left" site="toe_back_left" />
<torque name="torque_toe_front_left" site="toe_front_left" />
<torque name="torque_toe_front_right" site="toe_front_right" />
<torque name="torque_toe_back_right" site="toe_back_right" />
<torque name="torque_toe_back_left" site="toe_back_left" />
<subtreecom name="center_of_mass" body="torso" />
</sensor>
</mujoco>

5
motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py
View File

@@ -79,7 +79,6 @@ class Reacher2DEnv(NpEnv):
terminated = np.isnan(obs).any(axis=-1)
rwd[terminated] = 0.0
state.info["target_info"] = {"positions": self._target_xyz.copy(), "size": self._target_size}
state.info["Reward"] = {"distance": dist, "tolerance": rwd.copy()}
return state.replace(obs=obs, reward=rwd, terminated=terminated)
@@ -110,6 +109,8 @@ class Reacher2DEnv(NpEnv):
obs = self._get_obs(data)
rewards = {"distance": np.zeros((num_reset,)), "tolerance": np.zeros((num_reset,))}
info = {"Reward": rewards, "target_info": {"positions": self._target_xyz.copy(), "size": self._target_size}}
info = {
"Reward": rewards,
}
return obs, info

8
motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py
View File

@@ -500,25 +500,25 @@ class AnymalCEnv(NpEnv):
pos_range[0],
pos_range[2], # x_min, x_max
num_envs,
)
).astype(np.float32)
robot_init_y = np.random.uniform(
pos_range[1],
pos_range[3], # y_min, y_max
num_envs,
)
).astype(np.float32)
robot_init_pos = np.stack([robot_init_x, robot_init_y], axis=1) # [num_envs, 2]
# Generate target position: offset relative to robot initial position
# pose_command_range now represents offset range relative to robot
target_offset = np.random.uniform(
low=cfg.commands.pose_command_range[:2], high=cfg.commands.pose_command_range[3:5], size=(num_envs, 2)
)
).astype(np.float32)
target_positions = robot_init_pos + target_offset # Target position in world coordinates
# Generate target heading (absolute heading, random in horizontal direction)
target_headings = np.random.uniform(
low=cfg.commands.pose_command_range[2], high=cfg.commands.pose_command_range[5], size=(num_envs, 1)
)
).astype(np.float32)
pose_commands = np.concatenate([target_positions, target_headings], axis=1)

4
motrix_envs/src/motrix_envs/locomotion/go1/cfg.py
View File

@@ -92,6 +92,7 @@ class Sensor:
feet = ["FR", "FL", "RR", "RL"]
# -- docs-tag-start: go1-reward-config --
@dataclass
class RewardConfig:
scales: dict[str, float] = field(
@@ -119,6 +120,9 @@ class RewardConfig:
max_foot_height: float = 0.1
# -- docs-tag-end: go1-reward-config --
@registry.envcfg("go1-flat-terrain-walk")
@dataclass
class Go1WalkNpEnvCfg(EnvCfg):

9
motrix_envs/src/motrix_envs/locomotion/go1/walk_np.py
View File

@@ -244,7 +244,7 @@ class Go1WalkTask(NpEnv):
high=self.cfg.commands.vel_limit[1],
size=(num_envs, 3),
)
return commands
return commands.astype(np.float32)
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data
@@ -266,12 +266,9 @@ class Go1WalkTask(NpEnv):
def reset(self, data) -> tuple[np.ndarray, dict]:
num_reset = data.shape[0]
dof_pos = np.tile(self._init_dof_pos, (num_reset, 1))
dof_vel = np.tile(self._init_dof_vel, (num_reset, 1))
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
data.set_dof_vel(self._init_dof_vel)
data.set_dof_pos(self._init_dof_pos, self._model)
self._model.forward_kinematic(data)
info = {

2
motrix_envs/src/motrix_envs/locomotion/go1/walk_rough_terrain.py
View File

@@ -271,7 +271,7 @@ class Go1WalkRoughTask(NpEnv):
size=(num_envs, 3),
)
# commands[:, 2] = 0
return commands
return commands.astype(np.float32)
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data

2
motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py
View File

@@ -287,7 +287,7 @@ class Go1WalkStairsTask(NpEnv):
high=self.cfg.commands.vel_limit[1],
size=(num_envs, 3),
)
return commands
return commands.astype(np.float32)
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data

2
motrix_envs/src/motrix_envs/locomotion/go2/walk_np.py
View File

@@ -234,7 +234,7 @@ class Go2WalkTask(NpEnv):
high=self.cfg.commands.vel_limit[1],
size=(num_envs, 3),
)
return commands
return commands.astype(np.float32)
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data

7
motrix_envs/src/motrix_envs/manipulation/__init__.py
View File

@@ -13,4 +13,9 @@
# limitations under the License.
# ==============================================================================
from . import franka_lift_cube, franka_open_cabinet, shadow_hand # noqa: F401 import to register envs
from . import (
franka_lift_cube,
franka_open_cabinet,
rm65_open_cabinet,
shadow_hand,
)

49
motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/xmls/mjx_panda.xml
View File

@@ -117,8 +117,7 @@
<worldbody>
<body name="link0" childclass="panda">
<inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974"
fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
<inertial mass="0.629769" pos="-0.041018 -0.00014 0.049974" fullinertia="0.00315 0.00388 0.004285 8.2904e-7 0.00015 8.2299e-6"/>
<geom mesh="link0_0" material="off_white" class="visual"/>
<geom mesh="link0_1" material="black" class="visual"/>
<geom mesh="link0_2" material="off_white" class="visual"/>
@@ -132,29 +131,25 @@
<geom mesh="link0_11" material="white" class="visual"/>
<geom mesh="link0_c" class="collision"/>
<body name="link1" pos="0 0 0.333">
<inertial mass="4.970684" pos="0.003875 0.002081 -0.04762"
fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
<inertial mass="4.970684" pos="0.003875 0.002081 -0.04762" fullinertia="0.70337 0.70661 0.0091170 -0.00013900 0.0067720 0.019169"/>
<joint name="joint1" damping="40"/>
<geom material="white" mesh="link1" class="visual"/>
<geom mesh="link1_c" class="collision"/>
<body name="link2" quat="1 -1 0 0">
<inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495"
fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
<inertial mass="0.646926" pos="-0.003141 -0.02872 0.003495" fullinertia="0.0079620 2.8110e-2 2.5995e-2 -3.925e-3 1.0254e-2 7.04e-4"/>
<joint name="joint2" range="-1.7628 1.7628" damping="40"/>
<geom material="white" mesh="link2" class="visual"/>
<geom mesh="link2_c" class="collision"/>
<body name="link3" pos="0 -0.316 0" quat="1 1 0 0">
<joint name="joint3" damping="40"/>
<inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2"
fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
<inertial mass="3.228604" pos="2.7518e-2 3.9252e-2 -6.6502e-2" fullinertia="3.7242e-2 3.6155e-2 1.083e-2 -4.761e-3 -1.1396e-2 -1.2805e-2"/>
<geom mesh="link3_0" material="white" class="visual"/>
<geom mesh="link3_1" material="white" class="visual"/>
<geom mesh="link3_2" material="white" class="visual"/>
<geom mesh="link3_3" material="black" class="visual"/>
<geom mesh="link3_c" class="collision"/>
<body name="link4" pos="0.0825 0 0" quat="1 1 0 0">
<inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2"
fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
<inertial mass="3.587895" pos="-5.317e-2 1.04419e-1 2.7454e-2" fullinertia="2.5853e-2 1.9552e-2 2.8323e-2 7.796e-3 -1.332e-3 8.641e-3"/>
<joint name="joint4" range="-3.0718 -0.0698" damping="40"/>
<geom mesh="link4_0" material="white" class="visual"/>
<geom mesh="link4_1" material="white" class="visual"/>
@@ -162,8 +157,7 @@
<geom mesh="link4_3" material="white" class="visual"/>
<geom mesh="link4_c" class="collision"/>
<body name="link5" pos="-0.0825 0.384 0" quat="1 -1 0 0">
<inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2"
fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
<inertial mass="1.225946" pos="-1.1953e-2 4.1065e-2 -3.8437e-2" fullinertia="3.5549e-2 2.9474e-2 8.627e-3 -2.117e-3 -4.037e-3 2.29e-4"/>
<joint name="joint5" damping="2"/>
<geom mesh="link5_0" material="black" class="visual"/>
<geom mesh="link5_1" material="white" class="visual"/>
@@ -172,8 +166,7 @@
<geom mesh="link5_c1" class="collision"/>
<geom mesh="link5_c2" class="collision"/>
<body name="link6" quat="1 1 0 0">
<inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2"
fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
<inertial mass="1.666555" pos="6.0149e-2 -1.4117e-2 -1.0517e-2" fullinertia="1.964e-3 4.354e-3 5.433e-3 1.09e-4 -1.158e-3 3.41e-4"/>
<joint name="joint6" range="-0.0175 3.7525" damping="2"/>
<geom mesh="link6_0" material="off_white" class="visual"/>
<geom mesh="link6_1" material="white" class="visual"/>
@@ -194,8 +187,7 @@
<geom mesh="link6_16" material="white" class="visual"/>
<geom mesh="link6_c" class="collision"/>
<body name="link7" pos="0.088 0 0" quat="1 1 0 0">
<inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2"
fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
<inertial mass="7.35522e-01" pos="1.0517e-2 -4.252e-3 6.1597e-2" fullinertia="1.2516e-2 1.0027e-2 4.815e-3 -4.28e-4 -1.196e-3 -7.41e-4"/>
<joint name="joint7" damping="2"/>
<geom mesh="link7_0" material="white" class="visual"/>
<geom mesh="link7_1" material="black" class="visual"/>
@@ -214,9 +206,7 @@
<geom mesh="hand_3" material="white" class="visual"/>
<geom mesh="hand_4" material="off_white" class="visual"/>
<geom mesh="hand_c" class="collision"/>
<geom name="hand_capsule" type="capsule"
class="collision" conaffinity="1" size="0.04 0.06"
quat="1 1 0 0" pos="0 0 0.03"/>
<geom name="hand_capsule" type="capsule" class="collision" conaffinity="1" size="0.04 0.06" quat="1 1 0 0" pos="0 0 0.03"/>
<site name="gripper" pos="0 0 0.1"/>
<body name="left_finger" pos="0 0 0.0584">
<inertial mass="0.015" pos="0 0 0" diaginertia="2.375e-6 2.375e-6 7.5e-7"/>
@@ -258,21 +248,14 @@
</equality>
<actuator>
<position class="panda" name="actuator1" joint="joint1" kp="1000" kv="20"
ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator2" joint="joint2" kp="1000" kv="20"
ctrlrange="-1.7628 1.7628"/>
<position class="panda" name="actuator3" joint="joint3" kp="750" kv="4"
ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator4" joint="joint4" kp="750" kv="4"
ctrlrange="-3.0718 -0.0698"/>
<position class="panda" name="actuator5" joint="joint5" kp="300" kv="2"
forcerange="-12 12" ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator6" joint="joint6" kp="300" kv="2" forcerange="-12 12"
ctrlrange="-0.0175 3.7525"/>
<position class="panda" name="actuator1" joint="joint1" kp="1000" kv="20" ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator2" joint="joint2" kp="1000" kv="20" ctrlrange="-1.7628 1.7628"/>
<position class="panda" name="actuator3" joint="joint3" kp="750" kv="4" ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator4" joint="joint4" kp="750" kv="4" ctrlrange="-3.0718 -0.0698"/>
<position class="panda" name="actuator5" joint="joint5" kp="300" kv="2" forcerange="-12 12" ctrlrange="-2.8973 2.8973"/>
<position class="panda" name="actuator6" joint="joint6" kp="300" kv="2" forcerange="-12 12" ctrlrange="-0.0175 3.7525"/>
<position class="panda" name="actuator7" joint="joint7" kp="300" kv="2" forcerange="-12 12"/>
<general class="panda" name="actuator8" joint="finger_joint1"
ctrlrange="0 0.04" gainprm="350 0 0" biasprm="0 -350 -10" forcerange="-200 200"/>
<general class="panda" name="actuator8" joint="finger_joint1" ctrlrange="0 0.04" gainprm="350 0 0" biasprm="0 -350 -10" forcerange="-200 200"/>
</actuator>
<contact>

28
motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/xmls/mjx_scene.xml
View File

@@ -15,43 +15,33 @@
<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" />
<global azimuth="120" elevation="-20" />
<scale contactwidth="0.075" contactheight="0.025" forcewidth="0.05" com="0.05" framewidth="0.01"
framelength="0.2" />
<scale contactwidth="0.075" contactheight="0.025" forcewidth="0.05" com="0.05" framewidth="0.01" framelength="0.2" />
</visual>
<asset>
<material name="Transport" rgba="0 1 0 0" reflectance="0.5" metallic="0.0" roughness="0.5" />
<texture type="2d" colorspace="sRGB" name="small_meta_table_diffuse"
file="small_meta_table_diffuse.png" />
<texture type="2d" colorspace="linear" name="small_meta_table_nrm"
file="small_meta_table_nrm.png" />
<texture type="2d" colorspace="linear" name="small_meta_table_orm"
file="small_meta_table_orm.png" />
<texture type="2d" colorspace="sRGB" name="small_meta_table_diffuse" file="small_meta_table_diffuse.png" />
<texture type="2d" colorspace="linear" name="small_meta_table_nrm" file="small_meta_table_nrm.png" />
<texture type="2d" colorspace="linear" name="small_meta_table_orm" file="small_meta_table_orm.png" />
<material name="wood" rgba="0.8 0.8 0.8 1" reflectance="0.25" metallic="0.0" roughness=".95">
<layer texture="small_meta_table_diffuse" role="rgb" />
<layer texture="small_meta_table_nrm" role="normal" />
<layer texture="small_meta_table_orm" role="orm" />
</material>
<!-- Table dimensions (length x width x height in meters): 1.21x0.76x0.75 -->
<material name="chrome" rgba="0.8 0.8 0.8 1" reflectance="0.5" metallic="1.0"
roughness="0.44999998807907104" />
<material name="chrome" rgba="0.8 0.8 0.8 1" reflectance="0.5" metallic="1.0" roughness="0.44999998807907104" />
<mesh file="tablelegs.obj" />
<mesh file="tabletop.obj" />
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0" width="512" height="512" />
<texture name="motphys-ground" type="2d" file="../../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true" texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light diffuse="0.6 0.6 0.6" pos="0 0 1.5"
dir="-0.49835488200187683 0.2925136387348175 -0.8161361217498779" directional="true" />
<geom name="floor" size="0 0 0.05" pos="0 0 -0.75" type="plane" material="motphys-ground"
contype="1" />
<light diffuse="0.6 0.6 0.6" pos="0 0 1.5" dir="-0.49835488200187683 0.2925136387348175 -0.8161361217498779" directional="true" />
<geom name="floor" size="0 0 0.05" pos="0 0 -0.75" type="plane" material="motphys-ground" contype="1" />
<geom mesh="tabletop" material="wood" class="visual" pos="0.3 0 -0.75" quat="1 0 0 1" />
<geom mesh="tablelegs" material="chrome" class="visual" pos="0.3 0 -0.75" quat="1 0 0 1" />

16
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/__init__.py Executable file
View File

@@ -0,0 +1,16 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
from . import rm65_open_cabinet_np # noqa: F401

153
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/cfg.py Normal file
View File

@@ -0,0 +1,153 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass, field
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/xmls/scene.xml"
@dataclass
class ResetConfig:
joint_pos_noise_scale: float = 0
@dataclass
class ArmControlConfig:
action_mode: str = "joint_target"
action_in_degrees: bool = False
target_action_normalized: bool = True
max_joint_speed: float = 1.15
max_joint_acc: float = 10.47
use_speed_limit: bool = True
use_acc_limit: bool = True
target_smoothing_alpha: float = 0.0
action_delay_steps: int = 6
actuator_lag_alpha: float = 0.062
delay_lag_randomization_enabled: bool = True
action_delay_steps_min: int = 6
action_delay_steps_max: int = 7
actuator_lag_alpha_min: float = 0.059
actuator_lag_alpha_max: float = 0.064
speed_acc_randomization_enabled: bool = True
max_joint_speed_min: float = 0.95
max_joint_speed_max: float = 1.35
max_joint_acc_min: float = 8.5
max_joint_acc_max: float = 12.0
@dataclass
class GripperControlConfig:
action_mode: str = "binary"
use_sigmoid: bool = True
close_threshold: float = 0.7
close_on_threshold: float = 0.78
open_off_threshold: float = 0.62
min_switch_interval_s: float = 0.25
max_speed: float = 4.0
use_speed_limit: bool = True
actuator_lag_alpha: float = 0.062
@dataclass
class RewardConfig:
dist_std: float = 0.4
dist_scale: float = 15.0
gripper_close_dist: float = 0.035
gripper_close_reward: float = 140.0
gripper_close_penalty: float = -8.0
grasp_dist: float = 0.03
grasp_close_ratio: float = 0.7
grasp_hold_steps: int = 6
open_reward_scale: float = 420.0
open_delta_reward_scale: float = 260.0
open_reward_strict_dist: float = 0.08
grasp_hold_reward_scale: float = 8.0
grasp_hold_open_scale: float = 10.0
open_bonus_dist_1: float = 0.15
open_bonus_reward_1: float = 35.0
open_bonus_dist_2: float = 0.22
open_bonus_reward_2: float = 70.0
slip_penalty: float = 20.0
slip_penalty_open_scale: float = 65.0
slip_open_dist_thresh: float = 0.002
action_penalty_rate_late: float = 4e-3
finger_penalty_weight: float = 6.0
finger_penalty_dist: float = 0.6
finger_align_reward: float = 8.0
finger_align_close_amount_thresh: float = 0.05
gripper_switch_penalty: float = 0.8
gripper_switch_penalty_dist: float = 0.10
quat_reward_scale: float = 12
quat_reward_dist_thresh: float = 0.5
wrong_open_dist: float = 0.032
action_penalty_switch_step: int = 12000
action_penalty_rate_early: float = 8e-4
joint_vel_penalty_rate_early: float = 0.0
joint_vel_penalty_rate_late: float = 5e-3
truncation_penalty: float = 10.0
@dataclass
class TerminationConfig:
tcp_behind_handle_threshold: float = -0.02
max_joint_vel: float = 3.93
@dataclass
class ObservationNoiseConfig:
enabled: bool = True
joint_noise_enabled: bool = True
handle_pose_noise_enabled: bool = True
joint_pos_std: float = 2e-5
joint_vel_std: float = 1e-4
target_pos_std: float = 0.01
target_rot_std: float = 0.017
target_pos_bias_std: float = 0.015
target_rot_bias_std: float = 0.03
bias_resample_prob: float = 0.0
dropout_prob: float = 0.091
latency_steps: int = 0
hold_last_on_dropout: bool = True
@registry.envcfg("rm65-open-cabinet")
@dataclass
class RM65OpenCabinetEnvCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 30.0
sim_dt: float = 0.005
ctrl_dt: float = 0.025
render_spacing: float = 2.0
action_scale = (0.05, 0.05, 0.05, 0.05, 0.05, 0.05)
action_history_len: int = 9
reset: ResetConfig = field(default_factory=ResetConfig)
reward: RewardConfig = field(default_factory=RewardConfig)
termination: TerminationConfig = field(default_factory=TerminationConfig)
arm_control: ArmControlConfig = field(default_factory=ArmControlConfig)
gripper_control: GripperControlConfig = field(default_factory=GripperControlConfig)
observation_noise: ObservationNoiseConfig = field(default_factory=ObservationNoiseConfig)

62
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/gripper_logic.py Normal file
View File

@@ -0,0 +1,62 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
from __future__ import annotations
import numpy as np
def raw_action_to_close_ratio(raw_gripper_action: np.ndarray, use_sigmoid: bool) -> np.ndarray:
raw = np.asarray(raw_gripper_action, dtype=np.float32)
if bool(use_sigmoid):
return 1.0 / (1.0 + np.exp(-raw))
return np.clip((raw + 1.0) * 0.5, 0.0, 1.0)
def binary_hysteresis_step(
*,
close_ratio: np.ndarray,
prev_closed: np.ndarray,
steps_since_switch: np.ndarray,
close_on_threshold: float,
open_off_threshold: float,
min_switch_interval_steps: int,
) -> tuple[np.ndarray, np.ndarray]:
close_ratio = np.asarray(close_ratio, dtype=np.float32)
prev_closed = np.asarray(prev_closed, dtype=bool)
steps_since_switch = np.asarray(steps_since_switch, dtype=np.int32)
close_on = float(np.clip(close_on_threshold, 0.0, 1.0))
open_off = float(np.clip(open_off_threshold, 0.0, 1.0))
if open_off > close_on:
open_off = close_on
min_steps = max(int(min_switch_interval_steps), 0)
can_switch = steps_since_switch >= min_steps
want_close = close_ratio > close_on
want_open = close_ratio < open_off
next_closed = prev_closed.copy()
next_closed = np.where(
np.logical_and(np.logical_not(prev_closed), np.logical_and(can_switch, want_close)),
True,
next_closed,
)
next_closed = np.where(
np.logical_and(prev_closed, np.logical_and(can_switch, want_open)),
False,
next_closed,
)
switched = next_closed != prev_closed
return next_closed.astype(bool), switched.astype(bool)

1132
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/rm65_open_cabinet_np.py Executable file
View File

File diff suppressed because it is too large Load Diff

176
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/LICENSE Executable file
View File

@@ -0,0 +1,176 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/box.obj Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:e9d29e9a8cd2187d4cc54c17d261a8f0dba4fd75f43c3913194b1d4ad87c74b4
size 444

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/cabinet.obj Normal file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d2416faaf6f93732f15dc8748a830621ceef393822f388a2f1e5335f05128a35
size 16120

13
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left.mtl Executable file
View File

@@ -0,0 +1,13 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl Material.001
Ns 96.078431
Ka 1.000000 1.000000 1.000000
Kd 0.840000 0.80000 0.640000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2
# map_Kd WoodFine0035_1_S.jpg

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left.obj Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:faac6910a79c21389ed9ad3cb4177c51bb674b77b30b1717bdbbf6ca2ee23227
size 65480

9
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left.obj.mtl Executable file
View File

@@ -0,0 +1,9 @@
# File produced by Open Asset Import Library (http://www.assimp.sf.net)
# (assimp v3.1.187496374)
newmtl DefaultMaterial
Kd 0.6 0.6 0.6
Ka 0.05 0.05 0.05
Ks 0.6 0.6 0.6
illum 1

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left.stl Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:5883b7c6f94df7ec293b4f56f9cdbf1cb78cde84b7ffc8d20154e1d8f0c8cf93
size 17184

553
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left.wrl Executable file
View File

@@ -0,0 +1,553 @@
#VRML V2.0 utf8
Group {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0.975 0.486 0.975
specularColor 0.7 0.7 1.0
}
}
geometry IndexedFaceSet {
solid FALSE
coord Coordinate {
point [
-0.010282 -0.189491 0.188934,
-0.008882 -0.190891 0.188934,
-0.006123 -0.190891 0.188934,
-0.010282 -0.184131 0.188934,
-0.006123 -0.190891 0.210934,
-0.010282 -0.184131 0.210934,
-0.006123 -0.184131 0.210934,
-0.010282 -0.189491 -0.189757,
-0.010282 -0.184131 -0.189757,
-0.006123 -0.190891 -0.189757,
-0.008882 -0.190891 -0.189757,
-0.006123 -0.190891 -0.211757,
-0.010282 -0.184131 -0.211757,
-0.010282 -0.189491 -0.211757,
-0.008882 -0.190891 -0.211757,
-0.008882 -0.190891 0.210934,
-0.010282 -0.189491 0.210934,
-0.006123 -0.184131 0.188934,
-0.006123 -0.184131 -0.189757,
-0.006123 -0.184131 -0.211757,
0.009747 -0.106958 -0.165943,
0.009747 0.104562 -0.165943,
0.009747 0.104562 0.165120,
0.009747 -0.106958 0.165120,
-0.010282 -0.139733 0.195593,
-0.010282 0.137337 0.195593,
-0.010282 0.137337 -0.196416,
-0.010282 -0.139733 -0.196416,
0.003847 -0.139733 0.195593,
0.003847 0.137337 0.195593,
0.003847 0.137337 -0.196416,
0.003847 -0.139733 -0.196416,
0.003847 -0.116058 0.175151,
0.009324 -0.107677 0.166763,
0.009324 0.105281 0.166763,
0.003847 0.113662 0.175151,
0.003847 -0.119309 -0.172721,
0.009324 -0.110927 -0.164332,
0.009324 -0.110927 0.163509,
0.003847 -0.119309 0.171898,
0.003847 0.113662 -0.175974,
0.009324 0.105281 -0.167586,
0.009324 -0.107677 -0.167586,
0.003847 -0.116058 -0.175974,
0.003847 0.116913 0.171898,
0.009324 0.108531 0.163509,
0.009324 0.108531 -0.164332,
0.003847 0.116913 -0.172721,
0.003847 -0.117480 0.174948,
0.009324 -0.109099 0.166560,
0.003847 -0.118496 0.174338,
0.009324 -0.110115 0.165949,
0.009324 0.106703 0.166560,
0.003847 0.115084 0.174948,
0.009324 0.107719 0.165949,
0.003847 0.116100 0.174338,
0.003847 -0.119106 -0.174144,
0.009324 -0.110724 -0.165755,
0.003847 -0.118496 -0.175161,
0.009324 -0.110115 -0.166772,
0.009324 -0.110724 0.164933,
0.003847 -0.119106 0.173321,
0.003847 0.115084 -0.175771,
0.009324 0.106703 -0.167382,
0.003847 0.116100 -0.175161,
0.009324 0.107719 -0.166772,
0.009324 -0.109099 -0.167382,
0.003847 -0.117480 -0.175771,
0.003847 0.116710 0.173321,
0.009324 0.108328 0.164933,
0.009324 0.108328 -0.165755,
0.003847 0.116710 -0.174144,
0.009747 -0.108112 0.164955,
0.009747 -0.108742 0.164576,
0.009747 -0.109121 0.163945,
0.009747 -0.109286 0.162790,
0.009747 -0.108743 -0.165399,
0.009747 -0.109121 -0.164767,
0.009747 -0.108112 -0.165778,
0.009747 0.105716 -0.165778,
0.009747 0.106347 -0.165399,
0.009747 0.106725 -0.164767,
0.009747 0.106890 -0.163613,
0.009747 0.106890 0.162790,
0.009747 0.106725 0.163945,
0.009747 0.106347 0.164576,
0.009747 0.105716 0.164955,
0.008363 0.136073 0.253550,
-0.008882 0.136073 0.253550,
-0.008882 -0.138469 0.253550,
0.008363 -0.138469 0.253550,
-0.008882 0.136073 -0.254373,
0.008363 0.136073 -0.254373,
0.008363 -0.138469 -0.254373,
-0.008882 -0.138469 -0.254373,
0.009763 -0.138469 -0.252973,
0.009763 0.136073 -0.252973,
0.009763 0.136073 -0.197681,
0.009763 -0.138469 -0.197681,
-0.010282 -0.138469 -0.252973,
-0.010282 -0.138469 -0.197681,
-0.010282 0.136073 -0.197681,
-0.010282 0.136073 -0.252973,
0.009763 0.136073 0.252150,
0.009763 -0.138469 0.252150,
0.009763 -0.138469 0.196858,
0.009763 0.136073 0.196858,
-0.010282 0.136073 0.252150,
-0.010282 0.136073 0.196858,
-0.010282 -0.138469 0.196858,
-0.010282 -0.138469 0.252150,
0.008363 -0.139733 0.252150,
-0.008882 -0.139733 0.252150,
-0.008882 -0.139733 0.196858,
0.008363 -0.139733 0.196858,
-0.008882 0.137337 0.252150,
0.008363 0.137337 0.252150,
0.008363 0.137337 0.196858,
-0.008882 0.137337 0.196858,
-0.008882 0.136073 0.195593,
0.008363 0.136073 0.195593,
0.008363 -0.138469 0.195593,
-0.008882 -0.138469 0.195593,
0.008363 0.137337 -0.252973,
-0.008882 0.137337 -0.252973,
-0.008882 0.137337 -0.197681,
0.008363 0.137337 -0.197681,
-0.008882 -0.139733 -0.252973,
0.008363 -0.139733 -0.252973,
0.008363 -0.139733 -0.197681,
-0.008882 -0.139733 -0.197681,
-0.008882 -0.138469 -0.196416,
0.008363 -0.138469 -0.196416,
0.008363 0.136073 -0.196416,
-0.008882 0.136073 -0.196416,
0.008363 0.190111 0.252150,
0.008363 0.190111 -0.252973,
-0.008882 0.190111 -0.252973,
-0.008882 0.190111 0.252150,
0.008363 0.188711 0.253550,
-0.008882 0.188711 0.253550,
-0.008882 0.138601 0.253550,
0.008363 0.138601 0.253550,
-0.008882 0.188711 -0.254373,
0.008363 0.188711 -0.254373,
0.008363 0.138601 -0.254373,
-0.008882 0.138601 -0.254373,
0.009763 0.138601 -0.252973,
0.009763 0.188711 -0.252973,
0.009763 0.188711 0.252150,
0.009763 0.138601 0.252150,
-0.010282 0.138601 0.252150,
-0.010282 0.188711 0.252150,
-0.010282 0.188711 -0.252973,
-0.010282 0.138601 -0.252973,
0.009763 -0.189491 0.252150,
0.008363 -0.189491 0.253550,
0.008363 -0.190891 0.252150,
-0.008882 -0.189491 0.253550,
-0.010282 -0.189491 0.252150,
-0.008882 -0.190891 0.252150,
0.009763 -0.140997 0.252150,
0.008363 -0.140997 0.253550,
-0.008882 -0.140997 0.253550,
-0.010282 -0.140997 0.252150,
0.008363 -0.190891 0.188934,
0.009763 -0.189491 0.188934,
0.009763 -0.140997 0.188934,
0.008363 -0.139733 0.188934,
-0.008882 -0.139733 0.188934,
-0.010282 -0.140997 0.188934,
0.009763 -0.140997 -0.189757,
0.008363 -0.139733 -0.189757,
0.009763 -0.189491 -0.189757,
0.009763 -0.189491 -0.252973,
0.008363 -0.190891 -0.252973,
0.008363 -0.189491 -0.254373,
-0.008882 -0.189491 -0.254373,
-0.008882 -0.190891 -0.252973,
-0.010282 -0.189491 -0.252973,
0.009763 -0.140997 -0.252973,
0.008363 -0.140997 -0.254373,
-0.008882 -0.140997 -0.254373,
-0.010282 -0.140997 -0.252973,
0.008363 -0.190891 -0.189757,
-0.008882 -0.139733 -0.189757,
-0.010282 -0.140997 -0.189757,
]
}
coordIndex [
0, 1, 2, -1,
0, 2, 3, -1,
4, 5, 6, -1,
7, 8, 9, -1,
7, 9, 10, -1,
11, 12, 13, -1,
11, 13, 14, -1,
4, 15, 16, -1,
4, 16, 5, -1,
2, 17, 3, -1,
8, 18, 9, -1,
11, 19, 12, -1,
20, 21, 22, -1,
20, 22, 23, -1,
24, 25, 26, -1,
24, 26, 27, -1,
24, 28, 29, -1,
24, 29, 25, -1,
25, 29, 30, -1,
25, 30, 26, -1,
26, 30, 31, -1,
26, 31, 27, -1,
27, 31, 28, -1,
27, 28, 24, -1,
29, 28, 31, -1,
29, 31, 30, -1,
32, 33, 34, -1,
32, 34, 35, -1,
36, 37, 38, -1,
36, 38, 39, -1,
40, 41, 42, -1,
40, 42, 43, -1,
44, 45, 46, -1,
44, 46, 47, -1,
32, 48, 49, -1,
32, 49, 33, -1,
48, 50, 51, -1,
48, 51, 49, -1,
35, 34, 52, -1,
35, 52, 53, -1,
53, 52, 54, -1,
53, 54, 55, -1,
36, 56, 57, -1,
36, 57, 37, -1,
56, 58, 59, -1,
56, 59, 57, -1,
39, 38, 60, -1,
39, 60, 61, -1,
61, 60, 51, -1,
61, 51, 50, -1,
40, 62, 63, -1,
40, 63, 41, -1,
62, 64, 65, -1,
62, 65, 63, -1,
43, 42, 66, -1,
43, 66, 67, -1,
67, 66, 59, -1,
67, 59, 58, -1,
44, 68, 69, -1,
44, 69, 45, -1,
68, 55, 54, -1,
68, 54, 69, -1,
47, 46, 70, -1,
47, 70, 71, -1,
71, 70, 65, -1,
71, 65, 64, -1,
72, 23, 33, -1,
72, 33, 49, -1,
73, 72, 49, -1,
73, 49, 51, -1,
74, 73, 51, -1,
74, 51, 60, -1,
75, 74, 60, -1,
75, 60, 38, -1,
76, 77, 57, -1,
76, 57, 59, -1,
78, 76, 59, -1,
78, 59, 66, -1,
20, 78, 66, -1,
20, 66, 42, -1,
21, 20, 42, -1,
21, 42, 41, -1,
79, 21, 41, -1,
79, 41, 63, -1,
80, 79, 63, -1,
80, 63, 65, -1,
81, 80, 65, -1,
81, 65, 70, -1,
82, 81, 70, -1,
82, 70, 46, -1,
83, 82, 46, -1,
83, 46, 45, -1,
84, 83, 45, -1,
84, 45, 69, -1,
85, 84, 69, -1,
85, 69, 54, -1,
86, 85, 54, -1,
86, 54, 52, -1,
86, 52, 34, -1,
86, 34, 22, -1,
23, 22, 34, -1,
23, 34, 33, -1,
84, 86, 22, -1,
84, 22, 83, -1,
79, 81, 82, -1,
79, 82, 21, -1,
82, 83, 22, -1,
82, 22, 21, -1,
72, 74, 75, -1,
72, 75, 23, -1,
77, 76, 78, -1,
72, 73, 74, -1,
84, 85, 86, -1,
79, 80, 81, -1,
87, 88, 89, -1,
87, 89, 90, -1,
91, 92, 93, -1,
91, 93, 94, -1,
95, 96, 97, -1,
95, 97, 98, -1,
99, 100, 101, -1,
99, 101, 102, -1,
103, 104, 105, -1,
103, 105, 106, -1,
107, 108, 109, -1,
107, 109, 110, -1,
111, 112, 113, -1,
111, 113, 114, -1,
115, 116, 117, -1,
115, 117, 118, -1,
119, 120, 121, -1,
119, 121, 122, -1,
123, 124, 125, -1,
123, 125, 126, -1,
127, 128, 129, -1,
127, 129, 130, -1,
131, 132, 133, -1,
131, 133, 134, -1,
97, 126, 133, -1,
120, 117, 106, -1,
105, 114, 121, -1,
132, 129, 98, -1,
131, 100, 130, -1,
113, 109, 122, -1,
119, 108, 118, -1,
125, 101, 134, -1,
116, 87, 103, -1,
104, 90, 111, -1,
112, 89, 110, -1,
107, 88, 115, -1,
128, 93, 95, -1,
96, 92, 123, -1,
124, 91, 102, -1,
99, 94, 127, -1,
103, 87, 90, -1,
103, 90, 104, -1,
110, 89, 88, -1,
110, 88, 107, -1,
95, 93, 92, -1,
95, 92, 96, -1,
102, 91, 94, -1,
102, 94, 99, -1,
105, 121, 120, -1,
105, 120, 106, -1,
97, 133, 132, -1,
97, 132, 98, -1,
108, 119, 122, -1,
108, 122, 109, -1,
100, 131, 134, -1,
100, 134, 101, -1,
87, 116, 115, -1,
87, 115, 88, -1,
89, 112, 111, -1,
89, 111, 90, -1,
91, 124, 123, -1,
91, 123, 92, -1,
93, 128, 127, -1,
93, 127, 94, -1,
98, 129, 128, -1,
98, 128, 95, -1,
99, 127, 130, -1,
99, 130, 100, -1,
101, 125, 124, -1,
101, 124, 102, -1,
96, 123, 126, -1,
96, 126, 97, -1,
106, 117, 116, -1,
106, 116, 103, -1,
107, 115, 118, -1,
107, 118, 108, -1,
109, 113, 112, -1,
109, 112, 110, -1,
104, 111, 114, -1,
104, 114, 105, -1,
113, 122, 121, -1,
113, 121, 114, -1,
117, 120, 119, -1,
117, 119, 118, -1,
125, 134, 133, -1,
125, 133, 126, -1,
129, 132, 131, -1,
129, 131, 130, -1,
135, 136, 137, -1,
135, 137, 138, -1,
139, 140, 141, -1,
139, 141, 142, -1,
143, 144, 145, -1,
143, 145, 146, -1,
147, 148, 149, -1,
147, 149, 150, -1,
151, 152, 153, -1,
151, 153, 154, -1,
116, 115, 124, -1,
116, 124, 123, -1,
148, 144, 136, -1,
135, 139, 149, -1,
138, 152, 140, -1,
143, 153, 137, -1,
142, 116, 150, -1,
151, 115, 141, -1,
147, 123, 145, -1,
146, 124, 154, -1,
143, 137, 136, -1,
143, 136, 144, -1,
148, 136, 135, -1,
148, 135, 149, -1,
152, 138, 137, -1,
152, 137, 153, -1,
139, 135, 138, -1,
139, 138, 140, -1,
149, 139, 142, -1,
149, 142, 150, -1,
153, 143, 146, -1,
153, 146, 154, -1,
151, 141, 140, -1,
151, 140, 152, -1,
141, 115, 116, -1,
141, 116, 142, -1,
147, 145, 144, -1,
147, 144, 148, -1,
145, 123, 124, -1,
145, 124, 146, -1,
154, 124, 115, -1,
154, 115, 151, -1,
150, 116, 123, -1,
150, 123, 147, -1,
155, 156, 157, -1,
158, 159, 160, -1,
161, 111, 162, -1,
163, 112, 164, -1,
155, 157, 165, -1,
155, 165, 166, -1,
160, 159, 16, -1,
160, 16, 15, -1,
111, 161, 167, -1,
111, 167, 168, -1,
164, 112, 169, -1,
164, 169, 170, -1,
160, 157, 156, -1,
160, 156, 158, -1,
162, 111, 112, -1,
162, 112, 163, -1,
160, 15, 4, -1,
160, 4, 157, -1,
168, 169, 112, -1,
168, 112, 111, -1,
163, 164, 159, -1,
163, 159, 158, -1,
156, 155, 161, -1,
156, 161, 162, -1,
164, 5, 16, -1,
164, 16, 159, -1,
166, 167, 161, -1,
166, 161, 155, -1,
163, 158, 156, -1,
163, 156, 162, -1,
3, 17, 6, -1,
3, 6, 5, -1,
6, 17, 2, -1,
6, 2, 4, -1,
1, 0, 7, -1,
1, 7, 10, -1,
167, 171, 172, -1,
167, 172, 168, -1,
2, 1, 10, -1,
2, 10, 9, -1,
167, 166, 173, -1,
167, 173, 171, -1,
174, 175, 176, -1,
177, 178, 179, -1,
180, 181, 128, -1,
182, 183, 127, -1,
184, 175, 174, -1,
184, 174, 173, -1,
13, 179, 178, -1,
13, 178, 14, -1,
171, 180, 128, -1,
171, 128, 172, -1,
185, 127, 183, -1,
185, 183, 186, -1,
176, 175, 178, -1,
176, 178, 177, -1,
127, 128, 181, -1,
127, 181, 182, -1,
11, 14, 178, -1,
11, 178, 175, -1,
11, 175, 184, -1,
11, 184, 9, -1,
127, 185, 172, -1,
127, 172, 128, -1,
179, 183, 182, -1,
179, 182, 177, -1,
180, 174, 176, -1,
180, 176, 181, -1,
13, 12, 183, -1,
13, 183, 179, -1,
180, 171, 173, -1,
180, 173, 174, -1,
176, 177, 182, -1,
176, 182, 181, -1,
19, 18, 8, -1,
19, 8, 12, -1,
9, 18, 19, -1,
9, 19, 11, -1,
166, 165, 184, -1,
166, 184, 173, -1,
169, 185, 186, -1,
169, 186, 170, -1,
2, 9, 184, -1,
2, 184, 165, -1,
172, 185, 169, -1,
172, 169, 168, -1,
3, 170, 186, -1,
3, 186, 8, -1,
3, 8, 7, -1,
3, 7, 0, -1,
4, 2, 165, -1,
4, 165, 157, -1,
170, 3, 5, -1,
170, 5, 164, -1,
12, 8, 186, -1,
12, 186, 183, -1
]
}
}
]
}

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left_nob.obj Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:c9f7a054ca51219b672cf78fcd3caa405b9e6b7cf77f2249f2b2e04452c7433c
size 25719

9
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left_nob.obj.mtl Executable file
View File

@@ -0,0 +1,9 @@
# File produced by Open Asset Import Library (http://www.assimp.sf.net)
# (assimp v3.1.187496374)
newmtl DefaultMaterial
Kd 0.6 0.6 0.6
Ka 0.05 0.05 0.05
Ks 0.6 0.6 0.6
illum 1

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left_nob.stl Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:2d67bb603014782235a64ae851089f7d867c64ba1fa4c699283d2e91a812bd8a
size 9984

331
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_left_nob.wrl Executable file
View File

@@ -0,0 +1,331 @@
#VRML V2.0 utf8
Group {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0.975 0.486 0.975
specularColor 0.7 0.7 1.0
}
}
geometry IndexedFaceSet {
solid FALSE
coord Coordinate {
point [
0.010346 0.176606 0.184551,
0.010346 0.175985 0.188075,
0.016218 0.175985 0.188075,
0.016218 0.176606 0.184551,
0.010346 0.174195 0.191174,
0.016218 0.174195 0.191174,
0.010346 0.171454 0.193474,
0.016218 0.171454 0.193474,
0.010346 0.168092 0.194698,
0.016218 0.168092 0.194698,
0.010346 0.164514 0.194698,
0.016218 0.164514 0.194698,
0.010346 0.161151 0.193474,
0.016218 0.161151 0.193474,
0.010346 0.158410 0.191174,
0.016218 0.158410 0.191174,
0.010346 0.156621 0.188075,
0.016218 0.156621 0.188075,
0.010346 0.156000 0.184551,
0.016218 0.156000 0.184551,
0.010346 0.156621 0.181028,
0.016218 0.156621 0.181028,
0.010346 0.158410 0.177929,
0.016218 0.158410 0.177929,
0.010346 0.161151 0.175629,
0.016218 0.161151 0.175629,
0.010346 0.164514 0.174405,
0.016218 0.164514 0.174405,
0.010346 0.168092 0.174405,
0.016218 0.168092 0.174405,
0.010346 0.171454 0.175629,
0.016218 0.171454 0.175629,
0.010346 0.174195 0.177929,
0.016218 0.174195 0.177929,
0.010346 0.175985 0.181027,
0.016218 0.175985 0.181027,
0.021149 0.177090 0.188478,
0.021149 0.177782 0.184551,
0.021149 0.175097 0.191930,
0.021149 0.172043 0.194493,
0.021149 0.168296 0.195857,
0.021149 0.164309 0.195857,
0.021149 0.160563 0.194493,
0.021149 0.157509 0.191930,
0.021149 0.155515 0.188478,
0.021149 0.154823 0.184551,
0.021149 0.155515 0.180625,
0.021149 0.157509 0.177172,
0.021149 0.160563 0.174610,
0.021149 0.164309 0.173246,
0.021149 0.168296 0.173246,
0.021149 0.172043 0.174610,
0.021149 0.175097 0.177172,
0.021149 0.177090 0.180625,
0.026081 0.181180 0.184551,
0.026081 0.180283 0.189640,
0.036057 0.180283 0.189640,
0.036057 0.181180 0.184551,
0.026081 0.177700 0.194114,
0.036057 0.177700 0.194114,
0.026081 0.173742 0.197436,
0.036057 0.173742 0.197436,
0.026081 0.168886 0.199203,
0.036057 0.168886 0.199203,
0.026081 0.163719 0.199203,
0.036057 0.163719 0.199203,
0.026081 0.158864 0.197436,
0.036057 0.158864 0.197436,
0.026081 0.154906 0.194114,
0.036057 0.154906 0.194114,
0.026081 0.152322 0.189640,
0.036057 0.152322 0.189640,
0.026081 0.151425 0.184551,
0.036057 0.151425 0.184551,
0.026081 0.152322 0.179463,
0.036057 0.152322 0.179463,
0.026081 0.154906 0.174988,
0.036057 0.154906 0.174988,
0.026081 0.158864 0.171667,
0.036057 0.158864 0.171667,
0.026081 0.163719 0.169900,
0.036057 0.163719 0.169900,
0.026081 0.168886 0.169900,
0.036057 0.168886 0.169900,
0.026081 0.173741 0.171667,
0.036057 0.173741 0.171667,
0.026081 0.177700 0.174988,
0.036057 0.177700 0.174988,
0.026081 0.180283 0.179463,
0.036057 0.180283 0.179463,
0.036843 0.179751 0.189446,
0.036843 0.180614 0.184551,
0.036843 0.177266 0.193750,
0.036843 0.173458 0.196945,
0.036843 0.168788 0.198645,
0.036843 0.163818 0.198645,
0.036843 0.159147 0.196945,
0.036843 0.155340 0.193750,
0.036843 0.152855 0.189446,
0.036843 0.151992 0.184551,
0.036843 0.152855 0.179657,
0.036843 0.155340 0.175352,
0.036843 0.159147 0.172158,
0.036843 0.163818 0.170458,
0.036843 0.168788 0.170458,
0.036843 0.173458 0.172158,
0.036843 0.177266 0.175352,
0.036843 0.179751 0.179657,
0.036843 0.166303 0.184551,
]
}
coordIndex [
0, 1, 2, -1,
0, 2, 3, -1,
1, 4, 5, -1,
1, 5, 2, -1,
4, 6, 7, -1,
4, 7, 5, -1,
6, 8, 9, -1,
6, 9, 7, -1,
8, 10, 11, -1,
8, 11, 9, -1,
10, 12, 13, -1,
10, 13, 11, -1,
12, 14, 15, -1,
12, 15, 13, -1,
14, 16, 17, -1,
14, 17, 15, -1,
16, 18, 19, -1,
16, 19, 17, -1,
18, 20, 21, -1,
18, 21, 19, -1,
20, 22, 23, -1,
20, 23, 21, -1,
22, 24, 25, -1,
22, 25, 23, -1,
24, 26, 27, -1,
24, 27, 25, -1,
26, 28, 29, -1,
26, 29, 27, -1,
28, 30, 31, -1,
28, 31, 29, -1,
30, 32, 33, -1,
30, 33, 31, -1,
32, 34, 35, -1,
32, 35, 33, -1,
34, 0, 3, -1,
34, 3, 35, -1,
2, 36, 37, -1,
2, 37, 3, -1,
5, 38, 36, -1,
5, 36, 2, -1,
7, 39, 38, -1,
7, 38, 5, -1,
9, 40, 39, -1,
9, 39, 7, -1,
11, 41, 40, -1,
11, 40, 9, -1,
13, 42, 41, -1,
13, 41, 11, -1,
15, 43, 42, -1,
15, 42, 13, -1,
17, 44, 43, -1,
17, 43, 15, -1,
19, 45, 44, -1,
19, 44, 17, -1,
21, 46, 45, -1,
21, 45, 19, -1,
23, 47, 46, -1,
23, 46, 21, -1,
25, 48, 47, -1,
25, 47, 23, -1,
27, 49, 48, -1,
27, 48, 25, -1,
29, 50, 49, -1,
29, 49, 27, -1,
31, 51, 50, -1,
31, 50, 29, -1,
33, 52, 51, -1,
33, 51, 31, -1,
35, 53, 52, -1,
35, 52, 33, -1,
3, 37, 53, -1,
3, 53, 35, -1,
54, 55, 56, -1,
54, 56, 57, -1,
55, 58, 59, -1,
55, 59, 56, -1,
58, 60, 61, -1,
58, 61, 59, -1,
60, 62, 63, -1,
60, 63, 61, -1,
62, 64, 65, -1,
62, 65, 63, -1,
64, 66, 67, -1,
64, 67, 65, -1,
66, 68, 69, -1,
66, 69, 67, -1,
68, 70, 71, -1,
68, 71, 69, -1,
70, 72, 73, -1,
70, 73, 71, -1,
72, 74, 75, -1,
72, 75, 73, -1,
74, 76, 77, -1,
74, 77, 75, -1,
76, 78, 79, -1,
76, 79, 77, -1,
78, 80, 81, -1,
78, 81, 79, -1,
80, 82, 83, -1,
80, 83, 81, -1,
82, 84, 85, -1,
82, 85, 83, -1,
84, 86, 87, -1,
84, 87, 85, -1,
86, 88, 89, -1,
86, 89, 87, -1,
88, 54, 57, -1,
88, 57, 89, -1,
56, 90, 91, -1,
56, 91, 57, -1,
59, 92, 90, -1,
59, 90, 56, -1,
61, 93, 92, -1,
61, 92, 59, -1,
63, 94, 93, -1,
63, 93, 61, -1,
65, 95, 94, -1,
65, 94, 63, -1,
67, 96, 95, -1,
67, 95, 65, -1,
69, 97, 96, -1,
69, 96, 67, -1,
71, 98, 97, -1,
71, 97, 69, -1,
73, 99, 98, -1,
73, 98, 71, -1,
75, 100, 99, -1,
75, 99, 73, -1,
77, 101, 100, -1,
77, 100, 75, -1,
79, 102, 101, -1,
79, 101, 77, -1,
81, 103, 102, -1,
81, 102, 79, -1,
83, 104, 103, -1,
83, 103, 81, -1,
85, 105, 104, -1,
85, 104, 83, -1,
87, 106, 105, -1,
87, 105, 85, -1,
89, 107, 106, -1,
89, 106, 87, -1,
57, 91, 107, -1,
57, 107, 89, -1,
91, 90, 108, -1,
90, 92, 108, -1,
92, 93, 108, -1,
93, 94, 108, -1,
94, 95, 108, -1,
95, 96, 108, -1,
96, 97, 108, -1,
97, 98, 108, -1,
98, 99, 108, -1,
99, 100, 108, -1,
100, 101, 108, -1,
101, 102, 108, -1,
102, 103, 108, -1,
103, 104, 108, -1,
104, 105, 108, -1,
105, 106, 108, -1,
106, 107, 108, -1,
107, 91, 108, -1,
55, 54, 37, -1,
55, 37, 36, -1,
58, 55, 36, -1,
58, 36, 38, -1,
60, 58, 38, -1,
60, 38, 39, -1,
62, 60, 39, -1,
62, 39, 40, -1,
64, 62, 40, -1,
64, 40, 41, -1,
66, 64, 41, -1,
66, 41, 42, -1,
68, 66, 42, -1,
68, 42, 43, -1,
70, 68, 43, -1,
70, 43, 44, -1,
72, 70, 44, -1,
72, 44, 45, -1,
74, 72, 45, -1,
74, 45, 46, -1,
76, 74, 46, -1,
76, 46, 47, -1,
78, 76, 47, -1,
78, 47, 48, -1,
80, 78, 48, -1,
80, 48, 49, -1,
82, 80, 49, -1,
82, 49, 50, -1,
84, 82, 50, -1,
84, 50, 51, -1,
86, 84, 51, -1,
86, 51, 52, -1,
88, 86, 52, -1,
88, 52, 53, -1,
54, 88, 53, -1,
54, 53, 37, -1
]
}
}
]
}

13
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_right.mtl Executable file
View File

@@ -0,0 +1,13 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl Material.002
Ns 96.078431
Ka 1.000000 1.000000 1.000000
Kd 0.840000 0.80000 0.640000
Ks 0.500000 0.500000 0.500000
Ke 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2
# map_Kd WoodFine0035_1_S.jpg

3
motrix_envs/src/motrix_envs/manipulation/rm65_open_cabinet/xmls/assets/door_right.obj Executable file
View File

@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:acf69c4665732fbec4fd8c1fee1956575a645394fd6ed12070870e8f13249fd8
size 64676

Some files were not shown because too many files have changed in this diff Show More