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

chore: release v0.2.0

Browse Source
This commit is contained in:
motphys-developers
2026-02-10 08:08:11 +00:00
parent dbfa9e31fa
commit b568ac5600
123 changed files with 9732 additions and 497 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
.dockerignore Normal file
View File

@@ -0,0 +1,10 @@
# Exclude all files and directories.
*
# Unexclude specified files/directories.
!motrix_envs/**/*
!motrix_rl/**/*
!scripts/**/*
!pyproject.toml
!uv.lock
!uv.toml

124
.github/workflows/README.md vendored Normal file
View File

@@ -0,0 +1,124 @@
# GitHub Actions Workflows
This directory contains GitHub Actions workflows for automating CI/CD processes.
## Docker Image Build Workflow
### File: `docker-build.yml`
Automatically builds and pushes Docker images to Docker Hub when a new version tag is pushed.
#### Trigger Conditions
The workflow is triggered only when you push a git tag that matches the pattern `v*`:
```bash
git tag v0.1.0
git push origin v0.1.0
```
#### What It Does
1. **Extracts Version**: Reads the version from `pyproject.toml` (currently `0.1.0`)
2. **Builds Docker Image**: Uses the Dockerfile in `docker/Dockerfile`
3. **Pushes Multiple Tags**:
- `motphys/motrixlab:0.1.0` (version from pyproject.toml)
- `motphys/motrixlab:latest` (always points to the latest version)
- `motphys/motrixlab:0.1` (major.minor version)
#### Required Secrets
You need to configure the following secrets in your GitHub repository settings:
1. **`DOCKER_USERNAME`**: Your Docker Hub username
2. **`DOCKER_PASSWORD`**: Your Docker Hub password or access token
To add secrets:
1. Go to your repository on GitHub
2. Click **Settings** → **Secrets and variables** → **Actions**
3. Click **New repository secret**
4. Add the secrets listed above
#### Usage Example
```bash
# 1. Update version in pyproject.toml if needed
# 2. Commit your changes
git add .
git commit -m "Release v0.1.0"
# 3. Create and push a version tag
git tag v0.1.0
git push origin v0.1.0
# 4. The workflow will automatically build and push the Docker image
# 5. Monitor the build at: https://github.com/Motphys/MotrixLab/actions
```
#### Built Image Tags
After the workflow completes, the following Docker images will be available:
```bash
# Pull the latest version
docker pull motphys/motrixlab:latest
# Pull a specific version
docker pull motphys/motrixlab:0.1.0
# Pull major.minor version
docker pull motphys/motrixlab:0.1
```
#### Workflow Features
- ✅ **Optimized Caching**: Uses GitHub Actions cache to speed up builds
- ✅ **Multi-tag Support**: Automatically tags with version, major.minor, and latest
- ✅ **Version Extraction**: Automatically reads version from pyproject.toml
- ✅ **Docker Layer Caching**: Uses UV cache mounts for faster dependency installation
- ✅ **Tag-based Trigger**: Only builds on version tags, not on every commit
#### Docker Image Contents
The resulting Docker image includes:
- Base: NVIDIA CUDA 12.8.1 Runtime + Ubuntu 24.04
- UV package manager
- MotrixLab with all dependencies
- SKRL (both JAX and PyTorch backends)
- TensorBoard
- MotrixSim physics engine
#### Testing the Docker Image Locally
Before tagging a release, you can test the Docker build locally:
```bash
cd docker
docker build -t motphys/motrixlab:test .
docker run --gpus all motphys/motrixlab:test scripts/view.py --env cartpole
```
#### Troubleshooting
**Build fails with authentication error:**
- Verify Docker Hub credentials are correctly set in GitHub secrets
- Ensure your Docker Hub account has permission to push to the `motphys/motrixlab` repository
**Version extraction fails:**
- Ensure `pyproject.toml` has a valid `version = "x.y.z"` line
- Check the workflow logs for the exact extraction command output
**Tag not triggering the workflow:**
- Ensure the tag starts with `v` (e.g., `v0.1.0`, not `0.1.0`)
- Verify the tag was pushed to the correct branch: `git push origin v0.1.0`
#### See Also
- [Docker Hub Repository](https://hub.docker.com/r/motphys/motrixlab)
- [Container Deployment Documentation](../../docs/source/zh_CN/user_guide/getting_started/container_deployment.md)
- [Dockerfile](../../docker/Dockerfile)

60
.github/workflows/docker-build.yml vendored Normal file
View File

@@ -0,0 +1,60 @@
name: Build and Push Docker Image
on:
push:
tags:
- "v*"
env:
DOCKER_IMAGE: motphys/motrixlab
DOCKERFILE_PATH: docker
jobs:
build-and-push:
runs-on: ubuntu-latest
permissions:
contents: read
packages: write
steps:
- name: Checkout repository
uses: actions/checkout@v4
- name: Extract version from pyproject.toml
id: version
run: |
VERSION=$(grep '^version' pyproject.toml | head -1 | sed 's/version = "\(.*\)"/\1/' | tr -d '" ')
echo "version=${VERSION}" >> $GITHUB_OUTPUT
echo "Extracted version: ${VERSION}"
- name: Set up Docker Buildx
uses: docker/setup-buildx-action@v3
- name: Log in to Docker Hub
uses: docker/login-action@v3
with:
username: ${{ secrets.DOCKER_USERNAME }}
password: ${{ secrets.DOCKER_PASSWORD }}
- name: Extract metadata for Docker
id: meta
uses: docker/metadata-action@v5
with:
images: ${{ env.DOCKER_IMAGE }}
tags: |
type=semver,pattern={{version}},prefix=v
type=semver,pattern={{major}}.{{minor}},prefix=v
type=raw,value=${{ steps.version.outputs.version }}
type=raw,value=latest
- name: Build and push Docker image
uses: docker/build-push-action@v6
with:
context: .
file: ${{ env.DOCKERFILE_PATH }}/Dockerfile
push: true
tags: ${{ steps.meta.outputs.tags }}
labels: ${{ steps.meta.outputs.labels }}
cache-from: type=gha
cache-to: type=gha,mode=max
provenance: false

19
.readthedocs.yaml
View File

@@ -1,7 +1,7 @@
version: 2 version: 2
build: build:
os: ubuntu-22.04 os: ubuntu-24.04
tools: tools:
python: "3.10" python: "3.10"
jobs: jobs:
@@ -22,13 +22,14 @@ build:
# Make local files to have the real content on them # Make local files to have the real content on them
- ./tools/git-lfs checkout - ./tools/git-lfs checkout
- rm -rf ./tools - rm -rf ./tools
pre_create_environment:
- asdf plugin add uv
- asdf install uv latest
- asdf global uv latest
create_environment:
- uv venv "${READTHEDOCS_VIRTUALENV_PATH}"
install:
- UV_PROJECT_ENVIRONMENT="${READTHEDOCS_VIRTUALENV_PATH}" uv sync --frozen --extra docs
sphinx: sphinx:
configuration: docs/source/conf.py configuration: docs/source/conf.py
python:
install:
- method: pip
path: .
extra_requirements:
- docs

58
docker/Dockerfile Normal file
View File

@@ -0,0 +1,58 @@
FROM nvidia/cuda:12.8.1-runtime-ubuntu24.04
ENV DEBIAN_FRONTEND=noninteractive \
USERNAME=root \
HOMEDIR=/root \
UV_LINK_MODE=copy \
TZ=Asia/Shanghai
RUN apt-get update \
&& apt-get install -y --no-install-recommends \
curl ca-certificates tzdata \
&& ln -fs /usr/share/zoneinfo/${TZ} /etc/localtime \
&& echo ${TZ} > /etc/timezone \
&& apt-get clean \
&& rm -rf /var/lib/apt/lists/*
COPY --from=ghcr.io/astral-sh/uv:0.9.14 /uv /uvx /bin/
WORKDIR ${HOMEDIR}/motrixlab
# =============================================================================
# Two-Stage Dependency Installation Strategy
# =============================================================================
#
# This Dockerfile uses a two-stage installation approach to optimize Docker layer
# caching and reduce rebuild times when source code changes.
#
# Stage 1 (below): Install heavy framework dependencies
# - Installs large ML frameworks (SKRL, JAX, PyTorch, TensorFlow) (~3GB)
# - Skips workspace members (motrix-envs, motrixsim) that depend on source code
# - These dependencies rarely change, so this layer is heavily cached
# - Mounts only configuration files (pyproject.toml, uv.lock), no source code
#
# Stage 2 (after source copy): Install workspace members and remaining dependencies
# - Installs all workspace members (motrix-envs, motrix-rl, motrixsim)
# - Source code changes only invalidate this stage, not Stage 1
# - Significantly faster rebuilds during development
#
# Benefits:
# - Source code changes don't require re-downloading 3GB+ of dependencies
# - Dependencies are cached across builds using UV cache
# =============================================================================
RUN --mount=type=cache,target=/root/.cache/uv \
--mount=type=bind,source=pyproject.toml,target=pyproject.toml \
--mount=type=bind,source=motrix_envs/pyproject.toml,target=motrix_envs/pyproject.toml \
--mount=type=bind,source=motrix_rl/pyproject.toml,target=motrix_rl/pyproject.toml \
--mount=type=bind,source=uv.toml,target=uv.toml \
--mount=type=bind,source=uv.lock,target=uv.lock \
uv sync --frozen --no-install-workspace --package motrix-rl --no-install-package motrix-envs --no-install-package motrixsim --extra skrl-jax --extra skrl-torch
COPY --chown=${USERNAME}:${USERNAME} . .
# Stage 2: Install workspace members with source code
RUN --mount=type=cache,target=/root/.cache/uv \
uv sync --all-packages --extra skrl-jax --extra skrl-torch
ENTRYPOINT [ "uv", "run" ]

41
docker/docker-compose.yml Normal file
View File

@@ -0,0 +1,41 @@
volumes:
motrixlab-data:
name: motrixlab-data
services:
motrixlab-training:
image: motphys/motrixlab:latest
container_name: motrixlab-training
volumes:
- motrixlab-data:/root/motrixlab/runs
deploy:
resources:
reservations:
devices:
- driver: nvidia
count: all
capabilities: [gpu]
command:
- scripts/train.py
- --train-backend
- ${MOTRIX_TRAIN_BACKEND:-jax}
- --num-envs
- ${MOTRIX_NUM_ENVS:-2048}
- --env
- ${MOTRIX_ENV:-cartpole}
restart: no
motrixlab-tensorboard:
image: motphys/motrixlab:latest
container_name: motrixlab-tensorboard
ports:
- "6006:6006"
volumes:
- motrixlab-data:/root/motrixlab/runs:ro
command:
- tensorboard
- --host
- 0.0.0.0
- --logdir
- runs/${MOTRIX_ENV:-cartpole}
restart: unless-stopped

17
docs/gen_poster.sh
View File

@@ -78,21 +78,10 @@ for video_file in "$SOURCE_DIR"/*; do
echo "⏳ Processing '$filename'..." echo "⏳ Processing '$filename'..."
# Extract first frame using ffmpeg with timeout if ffmpeg -i "$video_file" -vframes 1 -q:v 2 "$output_file" -y -loglevel error 2>&1; then
if [ -n "$FFMPEG_TIMEOUT" ]; then ffmpeg_success=true
# Use timeout if available
if timeout 30 ffmpeg -i "$video_file" -vframes 1 -q:v 2 "$output_file" -y -loglevel error 2>&1; then
ffmpeg_success=true
else
ffmpeg_success=false
fi
else else
# No timeout available, run ffmpeg directly ffmpeg_success=false
if ffmpeg -i "$video_file" -vframes 1 -q:v 2 "$output_file" -y -loglevel error 2>&1; then
ffmpeg_success=true
else
ffmpeg_success=false
fi
fi fi
if $ffmpeg_success; then if $ffmpeg_success; then

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

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

4
docs/source/_static/images/poster/bounce_ball.jpg
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:4dd7992801e14afc2f4b71b29ce1618b04499c58702d2c151859b9005bd1b1e5 oid sha256:8bde5a04286fbc690130f7051c623080ac4ee4dbe3891990444970a1e1449a14
size 325888 size 293358

3
docs/source/_static/images/poster/bring-ball.png Normal file
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4
docs/source/_static/videos/bounce_ball.mp4
View File

@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1 version https://git-lfs.github.com/spec/v1
oid sha256:f72f90d07336c646c8e32fb108b5e57afde29b69917e34892641853f3701075c oid sha256:70b8d4d31a5694e2484904030c23d28c18ac90d85fcb151526214d14182f39d5
size 2784595 size 1519041

3
docs/source/_static/videos/bring-ball.mp4 Normal file
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -161,6 +161,20 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:::{grid-item-card} :::{grid-item-card}
```{video} _static/videos/dm_humanoid_run.mp4
:poster: _static/images/poster/dm_humanoid_run.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/bounce_ball.mp4 ```{video} _static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg :poster: _static/images/poster/bounce_ball.jpg
:nocontrols: :nocontrols:

154
docs/source/en/user_guide/demo/acrobot.md Normal file
View File

@@ -0,0 +1,154 @@
# Acrobot
Acrobot is a two-link swing-up and balance task. The goal is to swing both arms up and reach a target position using one motor torque.
```{video} /_static/videos/acrobot.mp4
:poster: _static/images/poster/acrobot.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Task Description
A two-link acrobot with one hinge joint is driven by a single motor. The motor is installed at the elbow joint, which is the only actuated joint in the system. The motor's torque rotates the rods in a plane, enabling swing-up from arbitrary initial angles and reaching a target position. Torque is limited by the actuator ctrlrange; by modulating its magnitude and direction, the policy must accumulate energy to swing up and reach the target while maintaining stability.
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (1,), float32)` |
| **Dimension** | 1 |
---
## Observation Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (6,), float32)` |
| **Dimension** | 6 |
Order: `upper_arm_horizontal, lower_arm_horizontal, upper_arm_vertical, lower_arm_vertical, shoulder_velocity, elbow_velocity`.
---
## Reward Function Design
- Base sparse reward: encourages the tip to enter the target region (radius = 0.2)
- Continuous reward: provides 0.1 reward per step for staying in the target region
- Distance shaping: 0.3 \* (1.0 - clip(distance / 2.0, 0, 1.0)) to encourage movement towards target
- Velocity penalty: 0.01 \* max(0, velocity_magnitude - 2.0) to penalize excessive velocities
---
## Initial State
- Shoulder angle randomized in `[-pi, pi]`
- Elbow angle randomized in `[-pi, pi]`
- Angular velocities initialized to zero
## Episode Termination Conditions
- Episode length limited by `max_episode_seconds`
- NaN check for observation values
---
### 1. Environment Preview
```bash
uv run scripts/view.py --env acrobot
```
### 2. Start Training
```bash
# Train with default parameters
uv run scripts/train.py --env acrobot
# Customize parallel environments
uv run scripts/train.py --env acrobot --num-envs 1024
# Enable rendering during training
uv run scripts/train.py --env acrobot --render
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/acrobot
```
### 4. Test Training Results
```bash
# Auto-discover best policy (recommended)
uv run scripts/play.py --env acrobot
# Manually specify a policy file
uv run scripts/play.py --env acrobot --policy runs/acrobot/nn/best_policy.pickle
```
> **Tip**: Policies are auto-selected from `runs/acrobot/`. You can override with `--policy`.
---
## Configuration Parameters
### Environment Configuration
```python
@dataclass
class AcrobotEnvCfg(EnvCfg):
model_file: str = ".../acrobot.xml" # MJCF model
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
```
### Training Configuration (PPO example)
```python
@rlcfg("acrobot", backend="jax")
@dataclass
class AcrobotPPO(PPOCfg):
max_env_steps: int = 60_000_000
check_point_interval: int = 500
# Override PPO configuration
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
```
---
## Expected Training Results
1. Acrobot can swing up both arms to reach the target position
2. The tip can stay within the target region with stability
3. Excessive oscillations are reduced by velocity penalty
4. The policy efficiently approaches the target with smooth movements

2
docs/source/en/user_guide/demo/anymal_c.md
View File

@@ -1,4 +1,4 @@
# ANYmal-C Locomotion # ANYmal-C Navigation
## Overview ## Overview

373
docs/source/en/user_guide/demo/bounce_ball.md
View File

@@ -42,46 +42,357 @@ The joints correspond as follows:
| Item | Details | | Item | Details |
| ------------- | -------------------------------- | | ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (25,), float32)` | | **Type** | `Box(-inf, inf, (29,), float32)` |
| **Dimension** | 25 | | **Dimension** | 29 |
The observation space consists of the following parts (in order): The observation space consists of the following parts (in order):
| Part | Content Description | Dimension | Remarks | | Part | Content Description | Dimension | Remarks |
| ----------- | ----------------------------------------------- | --------- | -------------------------------------------------------------------------------- | | ----------------- | ----------------------------------------------- | --------- | -------------------------------------------------------------------------------- |
| **dof_pos** | Position information for each degree of freedom | 13 | First 6 are arm joints, last 7 are ball's free joint (3 position + 4 quaternion) | | **dof_pos** | Position information for each degree of freedom | 13 | First 6 are arm joints, last 7 are ball's free joint (3 position + 4 quaternion) |
| **dof_vel** | Velocity information for each degree of freedom | 12 | Velocity is derivative of position | | **dof_vel** | Velocity information for each degree of freedom | 12 | Velocity is derivative of position |
| **paddle_pos** | Paddle position information | 3 | x, y, z coordinates of paddle center |
| **target_height** | Target height | 1 | Target height for current environment |
| Index | Observation | Min Value | Max Value | XML Name | Type (Unit) | | Index | Observation | Min Value | Max Value | XML Name | Type (Unit) |
| ----- | --------------------------- | --------- | --------- | ----------- | ------------------------- | | ----- | ------------------------------------- | --------- | --------- | --------- | ------------------------ |
| 0-5 | Arm Joint Angles | -Inf | Inf | Joint1-6 | Angle (rad) | | 0-5 | Arm Joint Angles | -Inf | Inf | Joint1-6 | Angle (rad) |
| 6 | Ball x-coordinate | -Inf | Inf | ball_x | Position (m) | | 6-8 | Ball Position [x, y, z] | -Inf | Inf | ball_link | Position (m) |
| 7 | Ball y-coordinate | -Inf | Inf | ball_y | Position (m) | | 9-12 | Ball Orientation Quaternion [w,x,y,z] | -Inf | Inf | ball_link | Quaternion |
| 8 | Ball z-coordinate | -Inf | Inf | ball_z | Position (m) | | 13-18 | Arm Joint Angular Velocities | -Inf | Inf | Joint1-6 | Angular Velocity (rad/s) |
| 9-12 | Ball Orientation Quaternion | -Inf | Inf | ball_qw/xyz | Quaternion (w,x,y,z) | | 19-24 | Ball Velocity [vx,vy,vz,wx,wy,wz] | -Inf | Inf | ball_link | Velocity (m/s, rad/s) |
| 13-24 | Joint and Ball Velocities | -Inf | Inf | - | Velocity/Angular Velocity | | 25-27 | Paddle Position [x, y, z] | -Inf | Inf | blocker | Position (m) |
| 28 | Target Height | -Inf | Inf | - | Position (m) |
--- ---
## Reward Function Design ## Reward Function
The reward function consists of the following components: The reward function uses a composite design with multiple reward and penalty terms to guide the robot to learn a stable ball bouncing strategy. All reward parameters can be adjusted through the configuration file.
```python ### Main Reward Terms
# Position Control Reward: Keep ball above paddle center
# Controlled Upward Velocity Reward: Reward moderate upward velocity when ball is well-positioned #### 1. Horizontal Position Reward
# Height Accuracy Reward: Ball is close to target height
# Consecutive Bounces Reward: Reward consecutive successful bounces **Design Rationale**: This is the core reward term, ensuring the ball stays directly above the paddle. Through a vertical distance weighting mechanism, when the ball is close to the paddle (about to hit), the horizontal position requirement is stricter, guiding the strategy to align precisely at critical moments.
# Total Reward = Weighted combination of all components
``` **Formula**:
$$
\begin{aligned}
\text{err}_{xy} &= \sqrt{(x_{ball} - x_{target})^2 + (y_{ball} - y_{target})^2} &&\text{(Horizontal position error)} \\
d_{vert} &= |z_{ball} - z_{paddle}| &&\text{(Vertical distance)} \\
w_{vert} &= e^{-d_{vert} / \sigma_{vert}} &&\text{(Vertical distance weight)} \\
\sigma_{pos} &= \sigma_{base} \times (1.0 + k_{weight} \times w_{vert}) &&\text{(Adaptive scale)} \\
r_{pos} &= e^{-\frac{\text{err}_{xy}^2}{2\sigma_{pos}^2}} &&\text{(Gaussian reward)} \\
\\
\text{Where:} \quad &\sigma_{vert} = 0.15 \text{ m} &&\text{(Vertical distance scale)} \\
&\sigma_{base} = 0.1 \text{ m} &&\text{(Base horizontal scale)} \\
&k_{weight} = 3.0 &&\text{(Weight factor)} \\
&x_{target} = 0.58856 \text{ m}, \, y_{target} = 0.0 \text{ m} &&\text{(Target position)}
\end{aligned}
$$
**Weight**: 2.0
#### 2. Out of Position Penalty
**Design Rationale**: Applies strong penalty for severe deviation from target position to prevent the ball from flying out of control range. Uses sigmoid function for smooth transition, avoiding discontinuous reward function.
**Formula**:
$$
r_{out} = -\frac{2.0}{1 + e^{-(\text{err}_{xy} - 0.05) / 0.03}} \qquad \text{(Sigmoid penalty)}
$$
**Weight**: 1.0
#### 3. Velocity Matching Reward
**Design Rationale**: Based on projectile motion physics, encourages the ball's trajectory to have the desired velocity (0.5 m/s) at target height. This ensures the ball doesn't pass through the target height too fast or too slow, facilitating stable control.
**Formula**:
$$
\begin{aligned}
\Delta h &= h_{target} - z_{ball} &&\text{(Height difference)} \\
v_{desired} &= 0.5 \text{ m/s} &&\text{(Desired velocity)} \\
\\
\text{Case 1:}&\text{ Ball moving upward and below target height} \\
v_{z,up}^2 &= v_z^2 - 2g\Delta h &&\text{(Energy conservation)} \\
v_{at\_target,up} &= \sqrt{\max(0, v_{z,up}^2)} &&\text{(Upward arrival velocity)} \\
\\
\text{Case 2:}&\text{ Ball moving downward and above target height} \\
v_{z,down}^2 &= v_z^2 + 2g|\Delta h| &&\text{(Energy conservation)} \\
v_{at\_target,down} &= -\sqrt{\max(0, v_{z,down}^2)} &&\text{(Downward arrival velocity)} \\
\\
\text{Case 3:}&\text{ Ball near target height (} |\Delta h| < 0.05 \text{ m)} \\
v_{at\_target,near} &= v_z &&\text{(Current velocity)} \\
\\
\text{Smooth combination:}& \\
\sigma_{up} &= \frac{1}{1 + e^{-v_z / 0.2}} &&\text{(Upward motion weight)} \\
\sigma_{below} &= \frac{1}{1 + e^{-\Delta h / 0.02}} &&\text{(Below target weight)} \\
\sigma_{down} &= 1 - \sigma_{up} &&\text{(Downward motion weight)} \\
\sigma_{above} &= 1 - \sigma_{below} &&\text{(Above target weight)} \\
w_{near} &= e^{-\frac{\Delta h^2}{2 \times 0.01^2}} &&\text{(Near target weight)} \\
\\
v_{at\_target} &= v_{at\_target,up} \cdot \sigma_{up} \cdot \sigma_{below} \\
&\quad + v_{at\_target,down} \cdot \sigma_{down} \cdot \sigma_{above} \\
&\quad + v_{at\_target,near} \cdot w_{near} &&\text{(Weighted combination)} \\
\\
\text{err}_{vel} &= |v_{at\_target} - v_{desired}| &&\text{(Velocity error)} \\
r_{vel} &= e^{-\frac{\text{err}_{vel}^2}{2 \times 0.8^2}} &&\text{(Gaussian reward)}
\end{aligned}
$$
**Weight**: 2.0
#### 4. Height Reward
**Design Rationale**: Directly encourages the ball to approach target height, one of the core task objectives. Higher weight (4.5) ensures the strategy prioritizes height control. Target height is randomly sampled (0.3-0.6 m) in each environment to improve policy generalization.
**Formula**:
$$
\begin{aligned}
\text{err}_h &= |z_{ball} - h_{target}| &&\text{(Height error)} \\
r_h &= e^{-\frac{\text{err}_h^2}{2 \times 0.15^2}} &&\text{(Gaussian reward)}
\end{aligned}
$$
**Weight**: 4.5
#### 5. Height Progress Reward
**Design Rationale**: Encourages the ball to reach higher positions, helping the strategy quickly learn to hit the ball upward in early training, avoiding the "no-hit" local optimum.
**Formula**:
$$
r_{progress} = \max(0, z_{ball} - 0.2) \times 2.0 \qquad \text{(Linear reward)}
$$
**Weight**: 1.0
#### 6. Controlled Upward Velocity Reward
**Design Rationale**: Only rewards upward velocity when the ball's horizontal position is good, avoiding "random hitting" behavior. Ideal velocity is calculated from physics formula, ensuring the ball can exactly reach target height. This reward guides the strategy to learn precise hitting force.
**Formula**:
$$
\begin{aligned}
q_{pos} &= e^{-\frac{\text{err}_{xy}^2}{2 \times 0.02^2}} &&\text{(Position quality)} \\
v_{ideal} &= \sqrt{2g \times \max(0, \Delta h)} &&\text{(Ideal launch velocity)} \\
v_{ideal} &\in [0.5, 3.0] \text{ m/s} &&\text{(Limit range)} \\
q_{vel} &= e^{-\frac{(v_z - v_{ideal})^2}{2 \times 0.5^2}} &&\text{(Velocity quality)} \\
\sigma_{up} &= \frac{1}{1 + e^{-v_z / 0.1}} &&\text{(Upward mask)} \\
r_{controlled} &= q_{pos} \times q_{vel} \times \sigma_{up} \times \text{clip}(v_z, 0, 1.5) &&\text{(Combined reward)}
\end{aligned}
$$
**Weight**: 1.5
#### 7. Consecutive Bounces Reward
**Design Rationale**: Encourages multiple consecutive successful bounces, guiding the strategy to learn stable long-term control. Uses logarithmic function to avoid infinite reward growth, while requiring good ball position to give reward.
**Formula**:
$$
\begin{aligned}
q_{bounce} &= e^{-\frac{\text{err}_{xy}^2}{2 \times 0.05^2}} &&\text{(Bounce position quality)} \\
r_{bounce\_log} &= 0.5 \times \log(n_{bounces} + 1) &&\text{(Logarithmic reward)} \\
r_{bounce} &= r_{bounce\_log} \times q_{bounce} \times \mathbb{1}_{n_{bounces} > 0} &&\text{(Conditional reward)}
\end{aligned}
$$
**Weight**: 0.8
#### 8. High Bounce Count Reward
**Design Rationale**: Gives extra reward for high bounce counts (≥3), further encouraging long-term stable control. Uses sigmoid activation function for smooth reward growth.
**Formula**:
$$
\begin{aligned}
\sigma_{high} &= \frac{1}{1 + e^{-(n_{bounces} - 2.0) / 0.5}} &&\text{(High bounce activation)} \\
r_{high} &= n_{bounces} \times 0.15 \times q_{bounce} \times \sigma_{high} &&\text{(Extra reward)}
\end{aligned}
$$
**Weight**: 0.3
#### 9. Paddle-Ball Horizontal Alignment Reward
**Design Rationale**: Encourages the paddle to actively move directly below the ball rather than waiting for the ball to fall. The closer the vertical distance, the greater the weight, requiring more precise alignment at the moment of hitting. Extra reward (boost) is given at bounce moment to reinforce correct hitting behavior.
**Formula**:
$$
\begin{aligned}
\text{err}_{align} &= \|(\mathbf{x}_{ball}, \mathbf{y}_{ball}) - (\mathbf{x}_{paddle}, \mathbf{y}_{paddle})\|_2 &&\text{(Alignment error)} \\
w_{prox} &= e^{-d_{vert} / 0.1} &&\text{(Vertical proximity weight)} \\
q_{align} &= e^{-\frac{\text{err}_{align}^2}{2 \times 0.03^2}} \times (1.0 + 2.0 \times w_{prox}) &&\text{(Alignment quality)} \\
k_{boost} &= \begin{cases} 3.0 & \text{if bounce detected} \\ 1.0 & \text{otherwise} \end{cases} &&\text{(Bounce boost)} \\
r_{align} &= q_{align} \times k_{boost} \times 0.3 &&\text{(Alignment reward)}
\end{aligned}
$$
**Weight**: 0.6
#### 10. Paddle Home Position Reward
**Design Rationale**: Encourages the paddle to return to home position when the ball is far away, avoiding the paddle staying at high position for long time. Uses distance dynamic factor: when ball is far from paddle ($d_{vert} > 0.15$ m), increase reward to encourage quick return; when ball is close, decrease reward to allow paddle to move up for hitting. This design makes paddle motion more energy-efficient and natural.
**Formula**:
$$
\begin{aligned}
\text{err}_{home} &= |z_{paddle} - z_{home}| &&\text{(Home deviation)} \\
k_{dist} &= 1.0 + \frac{0.5}{1 + e^{-(d_{vert} - 0.15) / 0.03}} &&\text{(Distance factor)} \\
q_{home} &= e^{-\frac{\text{err}_{home}^2}{2 \times 0.05^2}} &&\text{(Home quality)} \\
r_{home} &= q_{home} \times k_{dist} &&\text{(Home reward)} \\
\\
\text{Where:} \quad &z_{home} = 0.05 \text{ m} &&\text{(Paddle home height)}
\end{aligned}
$$
**Weight**: 1.5
### Penalty Terms
#### 1. Excessive Upward Velocity Penalty
**Design Rationale**: Prevents ball velocity from being too fast (>3.5 m/s) and losing control, ensuring ball motion stays within controllable range.
**Formula**:
$$
r_{excess} = -\frac{1.0}{1 + e^{-(v_z - 3.5) / 0.3}} \qquad \text{(Sigmoid penalty)}
$$
**Weight**: 1.0
#### 2. Downward Velocity Penalty
**Design Rationale**: Penalizes ball moving downward ($v_z < -0.2$ m/s), encouraging the strategy to hit the ball in time, avoiding free fall.
**Formula**:
$$
\begin{aligned}
\text{mag}_{down} &= -v_z \times \text{clip}(-v_z \times 0.3, 0, 0.5) &&\text{(Downward magnitude)} \\
\sigma_{down} &= \frac{1}{1 + e^{(v_z + 0.2) / 0.2}} &&\text{(Downward activation)} \\
r_{down} &= \text{mag}_{down} \times \sigma_{down} &&\text{(Downward penalty)}
\end{aligned}
$$
**Weight**: 1.0
#### 3. Paddle Height Violation Penalty
**Design Rationale**: Applies strong penalty when paddle deviates too far from home position (>0.1 m), ensuring paddle doesn't stay at high position for long time.
**Formula**:
$$
\begin{aligned}
\text{viol}_{height} &= \max(0, \text{err}_{home} - 0.1) &&\text{(Violation amount)} \\
r_{violation} &= -20.0 \times \text{viol}_{height} &&\text{(Strong penalty)}
\end{aligned}
$$
**Weight**: 1.0
#### 4. Action Change Rate Penalty
**Design Rationale**: Penalizes drastic action changes, encouraging smooth control strategy.
**Formula**:
$$
\begin{aligned}
r_{action} &= -\|\mathbf{a}_t - \mathbf{a}_{t-1}\|^2 &&\text{(L2 penalty)} \\
\\
\text{Where:} \quad &\mathbf{a}_t \in \mathbb{R}^6 &&\text{(Current action vector)}
\end{aligned}
$$
**Weight**: $10^{-4}$
#### 5. Joint Velocity Penalty
**Design Rationale**: Penalizes excessive joint velocities, encouraging energy-efficient and smooth motion.
**Formula**:
$$
\begin{aligned}
r_{joint\_vel} &= -\sum_{i=1}^{6} \dot{q}_i^2 &&\text{(Velocity squared sum penalty)} \\
\\
\text{Where:} \quad &\dot{\mathbf{q}} = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4, \dot{q}_5, \dot{q}_6]^T &&\text{(Joint angular velocity vector)}
\end{aligned}
$$
**Weight**: $10^{-4}$
### Total Reward Calculation
$$
\begin{aligned}
R_{total} = &\ 2.0 \cdot r_{pos} \\
&+ 1.0 \cdot r_{out} \\
&+ 2.0 \cdot r_{vel} \\
&+ 4.5 \cdot r_h \\
&+ 1.0 \cdot r_{progress} \\
&+ 1.5 \cdot r_{controlled} \\
&+ 0.8 \cdot r_{bounce} \\
&+ 0.3 \cdot r_{high} \\
&+ 0.6 \cdot r_{align} \\
&+ 1.5 \cdot r_{home} \\
&+ 1.0 \cdot r_{excess} \\
&+ 1.0 \cdot r_{down} \\
&+ 1.0 \cdot r_{violation} \\
&+ 10^{-4} \cdot r_{action} \\
&+ 10^{-4} \cdot r_{joint\_vel}
\end{aligned}
$$
--- ---
## Initial State ## Initial State
- **Arm Initial Position**: [0, 40, 110, 0, -60, 0] degrees, with random noise ### Robot Initialization
- **Ball Initial Position**: Above paddle center, with random noise
- **Ball Initial Velocity**: [0.0, 0.0, 0.0] m/s **Joint Angles**:
- Default angles: [0°, 40°, 110°, 0°, -60°, 0°]
- Random noise: Uniform random noise in [-0.1, 0.1] radians added to each joint
**Joint Velocities**:
- Initialized to zero with small random noise
### Ball Initialization
**Position**:
- Base position: `ball_init_pos` from config file (default [0.58856, 0, 0.45] m)
- Random noise: Uniform random noise in [-0.01, 0.01] m
**Velocity**:
- Initialized to zero
**Orientation**:
- Quaternion: [0, 0, 0, 1] (identity quaternion, no rotation)
### Target Height
Target height for each environment is randomly sampled in [0.4, 0.6] m range to improve policy generalization.
--- ---
@@ -89,7 +400,9 @@ The reward function consists of the following components:
- **Ball Falls**: Ball z-coordinate < 0.05m (near ground) - **Ball Falls**: Ball z-coordinate < 0.05m (near ground)
- **Ball Too High**: Ball z-coordinate > target height + 1.0m (lost control) - **Ball Too High**: Ball z-coordinate > target height + 1.0m (lost control)
- **Horizontal Deviation Too Far**: Absolute value of ball x-coordinate > 1.5m - **Horizontal Deviation Too Far**: Ball x or y coordinate absolute value > 1.5m
- **Joint Velocity Too High**: Any joint angular velocity > 2π rad/s (360°/s)
- **Timeout**: Episode duration exceeds maximum allowed time
--- ---
@@ -128,3 +441,9 @@ uv run scripts/play.py --env bounce_ball
3. Height Control: Ball's height stable within target height 0.8 ± 0.1m range 3. Height Control: Ball's height stable within target height 0.8 ± 0.1m range
4. Velocity Control: Ball's upward velocity maintained within reasonable range (0.1-1.5 m/s) 4. Velocity Control: Ball's upward velocity maintained within reasonable range (0.1-1.5 m/s)
5. Stable Control: Capable of maintaining stable bouncing for 20 seconds without dropping 5. Stable Control: Capable of maintaining stable bouncing for 20 seconds without dropping
---
## Known Issues
- **JAX Backend Training Performance**: The JAX version currently shows suboptimal training performance. For better results, it is recommended to use the PyTorch backend for this environment.

167
docs/source/en/user_guide/demo/bring_ball.md Normal file
View File

@@ -0,0 +1,167 @@
# Manipulator Bring Ball
Bring Ball is a classic Manipulator task from the DeepMind Control Suite. A planar hand with a thumb and finger must grasp a ball and move it to a target ball position. MotrixLab currently provides one Bring Ball environment:
- `dm-manipulator-bring-ball`: grasp the ball and move it to the target position
```{video} /_static/videos/bring-ball.mp4
:poster: _static/images/poster/bring-ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Task Description
Bring Ball is a planar (x-z) grasp-and-transport task:
- The hand has 4 arm joints (`arm_root`, `arm_shoulder`, `arm_elbow`, `arm_wrist`) plus thumb/finger joints
- Grasping is driven by the `grasp` tendon, coupling the `thumb` and `finger` joints
- The ball can slide in the plane (`ball_x`, `ball_z`) and rotate about the y-axis (`ball_y`)
- The target ball is a mocap body, sampled at reset
---
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (5,), float32)` |
| **Dimension** | 5 |
Actions correspond to the following actuators:
| Index | Action Meaning | Min Control | Max Control | XML Name |
| ----: | ---------------------------------- | :---------: | :---------: | :--------: |
| 0 | Root joint drive | -1 | 1 | `root` |
| 1 | Shoulder joint drive | -1 | 1 | `shoulder` |
| 2 | Elbow joint drive | -1 | 1 | `elbow` |
| 3 | Wrist joint drive | -1 | 1 | `wrist` |
| 4 | Grasp drive (thumb/finger coupled) | -1 | 1 | `grasp` |
---
## Observation Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (41,), float32)` |
| **Dimension** | 41 |
The observation vector is composed of the following parts (in order):
| Part | Content Description | Dim | Notes |
| -------------- | ----------------------------- | --- | ------------------------------------ |
| **arm_pos** | `sin`/`cos` of 8 joint angles | 16 | Joint order listed below |
| **arm_vel** | 8 joint velocities | 8 | Same order as arm_pos |
| **touch** | `log(1 + touch)` sensors | 5 | palm/finger/thumb/fingertip/thumbtip |
| **hand_pos** | Grasp site world position | 3 | x, y, z |
| **object_pos** | Ball position | 3 | x, y, z |
| **target_pos** | Target ball position | 3 | x, y, z |
| **rel** | `object_pos - target_pos` | 3 | Relative position |
| Index | Observation Range | Dim | Notes |
| ----- | ----------------------------------------------- | --- | --------------------------------- |
| 0-15 | `sin`/`cos` of 8 joint angles | 16 | Joint order: arm_root -> thumbtip |
| 16-23 | 8 joint velocities | 8 | Same order as above |
| 24-28 | Touch: palm, finger, thumb, fingertip, thumbtip | 5 | `log(1 + touch)` |
| 29-31 | Grasp position (x, y, z) | 3 | hand_pos |
| 32-34 | Ball position (x, y, z) | 3 | object_pos |
| 35-37 | Target ball position (x, y, z) | 3 | target_pos |
| 38-40 | Ball relative position (x, y, z) | 3 | rel |
Joint order: `arm_root`, `arm_shoulder`, `arm_elbow`, `arm_wrist`, `finger`, `fingertip`, `thumb`, `thumbtip`.
---
## Reward Function Design
Bring Ball uses shaped rewards with multiple components and penalties:
```python
# R1: Reach - fingertips approach the ball
r_reach = tolerance(avg_tip_dist)
# R2: Orient - palm points toward the ball
r_orient = clip(1 - orient_bound + dot(hand_dir, unit_vec_to_ball), 0..1)
# R3: Pause - reduce arm jitter when close to the ball
r_pause = tolerance(arm_speed_step) * is_close_to_ball
# R4: Close - grasp intent and contact together
r_close = r_close_intent * (approach_or_grasp)
# R5: Lift & Transport - height and target distance
r_lift_height = tolerance(ball_z)
r_transport = tolerance(move_dist_to_target)
r_lift = mix(r_lift_height, r_transport)
# Precision/Progress
r_precision = tolerance(move_dist_to_target, gaussian)
r_progress = (prev_dist - curr_dist) * scale
# Penalties
penalty_side + penalty_hover
```
Default weights (from `BringBallCfg`):
- reach 1.0, orient 1.5, pause 0.5, close 2.0, lift 6.0, precision 1.0
- lift mixes `lift_height_weight` and `transport_weight`
- progress reward is controlled by `transport_progress_scale`
---
## Initial State
- **Arm initialization**: uses the default model pose (`randomize_arm=False`), thumb/finger are symmetric
- **Target position**: `x in [-0.4, 0.4]`, `z in [0.1, 0.4]`, `y = 0.001`
- **Ball position**: `x in [-0.4, 0.4]`, `z in [0.2, 0.7]`, with a minimum hand distance
- **Physics settling**: performs settle steps after reset (`settle_steps=300`)
---
## Episode Termination Conditions
- Terminate if any `NaN` appears in observations
---
## Usage Guide
### 1. Environment Preview (random actions)
```bash
uv run scripts/view.py --env dm-manipulator-bring-ball
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-manipulator-bring-ball --train-backend torch
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-manipulator-bring-ball
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-manipulator-bring-ball
```
---
## Expected Training Results
1. The hand consistently reaches and grasps the ball
2. The ball is lifted off the ground and held at a stable height
3. The ball is reliably moved close to the target ball position

5
docs/source/en/user_guide/demo/dm_cheetah.md
View File

@@ -127,5 +127,6 @@ uv run scripts/play.py --env dm-cheetah
## Expected Training Results ## Expected Training Results
1. Stable horizontal speed approaching or exceeding 30.0 m/s 1. Run at a stable horizontal speed close to or exceeding 10.0 m/s
2. Maintaining an upright torso and coordinated gait 2. Maintain torso stability and coordinated gait, running long distances without falling
3. Running posture close to that of a real cheetah, with a sense of extension during the run

179
docs/source/en/user_guide/demo/dm_finger.md Normal file
View File

@@ -0,0 +1,179 @@
# Finger Manipulation
Finger is a classic manipulation task from the DeepMind Control Suite. A two-link “finger” applies torques to interact with a rotating spinner. MotrixLab currently provides three Finger environments:
```{video} /_static/videos/dm_finger_spin.mp4
:poster: _static/images/poster/dm_finger_spin.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
```{video} /_static/videos/dm_finger_turn.mp4
:poster: _static/images/poster/dm_finger_turn.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
- `dm-finger-spin`: make the spinner rotate continuously in the target direction
- `dm-finger-turn-easy`: align the spinner tip (`tip`) with a target point (larger target radius)
- `dm-finger-turn-hard`: same as Turn, but with a smaller target radius
---
## Task Description
Finger is a planar (x-z) interaction task:
- The finger has 2 actuated hinge joints: `proximal` and `distal`
- The spinner rotates around joint `hinge`, and `tip` denotes the spinner tip position
- For Turn tasks, a target point is sampled around the spinner at the beginning of each episode
---
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (2,), float32)` |
| **Dimension** | 2 |
The actions correspond to:
| Index | Action Description | Min Control | Max Control | XML Name | Joint Type |
| ----- | ---------------------------------- | ----------- | ----------- | -------- | ---------- |
| 0 | Torque applied to `proximal` joint | -1 | 1 | proximal | hinge |
| 1 | Torque applied to `distal` joint | -1 | 1 | distal | hinge |
---
## Observation Space
MotrixLab follows dm_control-style observations, but flattens them into a single vector.
### Spin Observation Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (9,), float32)` |
| **Dimension** | 9 |
The observation vector contains (in order):
- **position (4)**: `qpos(proximal, distal)` + `tip_xz` (tip position relative to the spinner in x-z)
- **velocity (3)**: `qvel(proximal, distal, hinge)` (hinge velocity is used by Spin reward)
- **touch (2)**: `log(1 + touchtop)`, `log(1 + touchbottom)`
### Turn Observation Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (12,), float32)` |
| **Dimension** | 12 |
Compared to Spin, Turn adds:
- **target_position (2)**: target position relative to the spinner in x-z
- **dist_to_target (1)**: signed distance from tip to the target sphere surface (negative means “inside”)
---
## Reward Function Design
### Spin
In dm_control, Spin is typically defined with a sparse threshold on spinner angular velocity. MotrixLab defaults to a dense/shaped reward for easier training, while also logging the sparse version:
```text
spin_sparse = 1 if hinge_velocity <= -15 else 0
spin = clip(-hinge_velocity / 15, 0, 1)
```
### Turn (Easy / Hard)
Turn aims to bring the spinner tip into a target sphere around the spinner:
- `turn_sparse = 1` when `dist_to_target <= 0`
- MotrixLab defaults to a shaped reward based on distance-to-target (exponential decay), and adds auxiliary terms to reduce “no-contact” failure modes and action jitter:
- approach-to-spinner shaping
- touch bonus
- action magnitude / action change penalties
The final shaped reward is clipped to `[0, 1]`.
---
## Initial State
- `proximal`, `distal` joint angles are sampled uniformly within joint limits
- spinner `hinge` angle is sampled uniformly in `[-pi, pi]`
- for Turn tasks, the target is sampled around the spinner on the x-z plane at reset
---
## Episode Termination Conditions
### Termination
If `NaN` appears in the observations
---
## Usage Guide
### 1. Environment Preview (random actions)
```bash
uv run scripts/view.py --env dm-finger-spin
```
```bash
uv run scripts/view.py --env dm-finger-turn-easy
```
```bash
uv run scripts/view.py --env dm-finger-turn-hard
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-finger-spin --train-backend torch
```
```bash
uv run scripts/train.py --env dm-finger-turn-easy --train-backend torch
```
```bash
uv run scripts/train.py --env dm-finger-turn-hard --train-backend torch
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-finger-spin
```
### 4. Test Training Results
`scripts/play.py` will auto-discover the latest `best_agent.*` under `runs/{env-name}/` (or you can pass `--policy` explicitly):
```bash
uv run scripts/play.py --env dm-finger-turn-hard
```
---
## Expected Training Results
1. `dm-finger-spin`: stable continuous rotation in the target direction
2. `dm-finger-turn-easy`: consistent contact and alignment with the (larger) target region, with reduced jitter
3. `dm-finger-turn-hard`: successful alignment with a smaller target region, typically requiring more training and better contact behavior

236
docs/source/en/user_guide/demo/dm_humanoid.md Normal file
View File

@@ -0,0 +1,236 @@
# 3D Humanoid Robot
The 3D Humanoid Robot (Humanoid) is a classic bipedal locomotion task from DeepMind Control Suite. The goal is to train a simulated 3D humanoid robot to achieve standing, walking, and running by controlling joint torques.
```{video} /_static/videos/dm_humanoid_run.mp4
:poster: _static/images/poster/dm_humanoid_run.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
Humanoid is a 3D bipedal humanoid robot task. The robot consists of a head, torso, two arms, and two legs, with 21 controlled joints (actuators). The agent controls the robot by applying torques to these joints to achieve standing balance, forward walking, or fast running. This task requires coordinated bipedal gait, balance control, and 3D spatial posture stability.
---
## Action Space
| Item | Details |
| -------- | -------------------------------- |
| **Type** | `Box(-1.0, 1.0, (21,), float32)` |
| **Dim** | 21 |
Actions correspond to the following:
| Index | Action (Joint Torque) | Min | Max | XML Name |
| ----: | ------------------------------ | :--: | :-: | :---------------- |
| 0 | Abdomen Y-axis rotation torque | -1.0 | 1.0 | `abdomen_y` |
| 1 | Abdomen Z-axis rotation torque | -1.0 | 1.0 | `abdomen_z` |
| 2 | Abdomen X-axis rotation torque | -1.0 | 1.0 | `abdomen_x` |
| 3 | Right hip X-axis torque | -1.0 | 1.0 | `right_hip_x` |
| 4 | Right hip Z-axis torque | -1.0 | 1.0 | `right_hip_z` |
| 5 | Right hip Y-axis torque | -1.0 | 1.0 | `right_hip_y` |
| 6 | Right knee torque | -1.0 | 1.0 | `right_knee` |
| 7 | Right ankle X-axis torque | -1.0 | 1.0 | `right_ankle_x` |
| 8 | Right ankle Y-axis torque | -1.0 | 1.0 | `right_ankle_y` |
| 9 | Left hip X-axis torque | -1.0 | 1.0 | `left_hip_x` |
| 10 | Left hip Z-axis torque | -1.0 | 1.0 | `left_hip_z` |
| 11 | Left hip Y-axis torque | -1.0 | 1.0 | `left_hip_y` |
| 12 | Left knee torque | -1.0 | 1.0 | `left_knee` |
| 13 | Left ankle X-axis torque | -1.0 | 1.0 | `left_ankle_x` |
| 14 | Left ankle Y-axis torque | -1.0 | 1.0 | `left_ankle_y` |
| 15 | Right shoulder 1 torque | -1.0 | 1.0 | `right_shoulder1` |
| 16 | Right shoulder 2 torque | -1.0 | 1.0 | `right_shoulder2` |
| 17 | Right elbow torque | -1.0 | 1.0 | `right_elbow` |
| 18 | Left shoulder 1 torque | -1.0 | 1.0 | `left_shoulder1` |
| 19 | Left shoulder 2 torque | -1.0 | 1.0 | `left_shoulder2` |
| 20 | Left elbow torque | -1.0 | 1.0 | `left_elbow` |
---
## Observation Space
| Item | Details |
| -------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (73,), float32)` |
| **Dim** | 73 |
The observation space of the Humanoid environment consists of the following components (in order):
| Component | Description | Dim | Notes |
| ------------------ | --------------------------------------- | --- | --------------------------------------------------------------------- |
| **joint_angles** | Joint angles (excluding root's 7 DOF) | 22 | Angles of 22 joints |
| **head_height** | Head height | 1 | Height of head relative to ground |
| **extremities** | Extremity positions (relative to torso) | 12 | Left hand, left foot, right hand, right foot (3D each, in that order) |
| **torso_vertical** | Torso vertical direction vector | 3 | Vertical direction in local coordinates |
| **com_vel** | Center of mass linear velocity | 3 | Linear velocity of torso subtree |
| **qvel** | Velocities of all joints and root | 29 | Including root's 6 DOF |
| **target_local** | Target direction (local coordinates) | 3 | Target direction in torso local frame |
---
## Reward Function Design
The Humanoid reward function varies according to task type (standing, walking, running), but all include the following core components:
### Posture Reward
```python
# Head height reward: keep head above target height (95% of stand_height, ~1.33m)
stand_reward = tolerance(head_height, bounds=(stand_height * 0.95, inf), margin=0.5)
# Torso upright reward: keep torso upright
upright_reward = tolerance(torso_upright, bounds=(0.9, inf), sigmoid="linear", margin=0.9)
# Pelvis height reward: keep pelvis at reasonable height (60% of stand_height, ~0.84m)
pelvis_height_reward = tolerance(pelvis_height, bounds=(stand_height * 0.6, inf), sigmoid="linear", margin=stand_height * 0.6)
# Posture reward = head height reward × torso upright reward × pelvis height reward
posture_reward = stand_reward * upright_reward * pelvis_height_reward
```
### Speed Reward
The speed reward calculation differs by task type:
**Standing Task (move_speed <= 0)**:
```python
# Speed reward: maintain near-zero speed
speed_reward = tolerance(actual_speed, bounds=(0, 0), margin=1.0, value_at_margin=0.01)
```
**Walking Task (0 < move_speed <= 3.0)**:
```python
# Speed reward: achieve target speed (default 1.0 m/s) in target direction (positive X-axis)
actual_speed = dot(com_vel[:2], target_direction[:2]) # Projection of velocity onto target direction
speed_reward = tolerance(actual_speed, bounds=(move_speed, move_speed), margin=move_speed, sigmoid="linear")
```
**Running Task (move_speed > 3.0)**:
```python
# Speed reward: achieve target speed (default 10.0 m/s) or above in target direction
actual_speed = dot(com_vel[:2], target_direction[:2])
speed_reward = tolerance(actual_speed, bounds=(move_speed, inf), margin=move_speed, sigmoid="linear")
```
### Energy Reward
```python
energy_reward = exp(-energy_coef * mean(ctrls ^ 2))
```
### Gait Reward
```python
# Torso heading reward: torso faces target direction
torso_heading_reward = tolerance(dot(torso_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# Head heading reward: head faces target direction
head_heading_reward = tolerance(dot(head_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# Pelvis yaw reward: pelvis faces target direction
pelvis_yaw_reward = tolerance(dot(pelvis_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# Pelvis level reward: pelvis remains level
pelvis_level_reward = tolerance(pelvis_up, bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# Feet height reward: feet stay close to ground
feet_height_reward = tolerance(max_foot_height, bounds=(0.0, 0.3), margin=0.5, sigmoid="quadratic")
# Gait reward = product of all heading and posture rewards
gait_reward = torso_heading_reward * head_heading_reward * pelvis_yaw_reward * pelvis_level_reward * feet_height_reward
```
### Total Reward
```python
total_reward = posture_reward * speed_reward * energy_reward * gait_reward
```
---
## Initial State
- **Robot Position**: Torso initial height is 1.33 meters (95% of standard stand height)
- **Robot Orientation**: Torso remains upright, quaternion set to (1.0, 0.0, 0.0, 0.0)
- **Joint Angles**: Randomly initialized within joint limits
- Torso/hip base joints: Randomized in small range (±15 degrees)
- Leg joints: Symmetrically initialized, ensuring left and right legs are independently randomized, knees initially bent
- Arm joints: Symmetrically initialized, using middle 80% of joint limit ranges
- **Initial Velocities**: All joint velocities and linear velocities initialized to small random values near zero (-0.01 to 0.01)
- **Initial Controls**: All actuator controls initialized to small random values near zero (-0.02 to 0.02)
## Episode Termination Conditions
- Robot state observations contain abnormal values (NaN or Inf)
- Head too low: Head height below 50% of standard stand height (0.7 meters)
- Torso too tilted: Torso vertical component less than 0.2 (severe torso tilt)
- Extreme velocity: Absolute value of any joint velocity exceeds 200.0 rad/s or m/s
- Maximum episode duration: 25 seconds
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-humanoid-stand
uv run scripts/view.py --env dm-humanoid-walk
uv run scripts/view.py --env dm-humanoid-run
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-humanoid-stand
uv run scripts/train.py --env dm-humanoid-walk
uv run scripts/train.py --env dm-humanoid-run
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-humanoid-walk
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-humanoid-stand
uv run scripts/play.py --env dm-humanoid-walk
uv run scripts/play.py --env dm-humanoid-run
```
---
## Expected Training Results
### Standing Task (dm-humanoid-stand)
1. Head height maintained in 1.3-1.5m range
2. Torso upright angle deviation less than 15 degrees
3. Able to stand stably without falling
4. Speed near zero, no significant movement
### Walking Task (dm-humanoid-walk)
1. Actual walking speed close to 1.0 m/s
2. Coordinated gait, no obvious falls
3. Able to walk continuously and stably
4. Torso and head facing target direction
### Running Task (dm-humanoid-run)
1. Running speed reaches 5.0-10.0 m/s
2. Flight phase appears (both feet off ground simultaneously)
3. Coordinated and stable gait
4. Able to maintain high-speed running posture

136
docs/source/en/user_guide/demo/pendulum.md Normal file
View File

@@ -0,0 +1,136 @@
# Pendulum
Pendulum is a single-joint swing-up and balance task. The goal is to swing the pole up and keep it inverted using one motor torque.
```{video} /_static/videos/pendulum.mp4
:poster: _static/images/poster/pendulum.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Task Description
A single-link pendulum with one hinge joint is driven by a single motor (configurable gear). The motor’s torque rotates the rod in a plane, enabling swing-up from arbitrary initial angles, inverted balance, and maintenance. Torque is limited by the actuator ctrlrange; by modulating its magnitude and direction, the policy must accumulate energy to swing up and stabilize near the inverted position while damping angular-velocity-induced oscillations.
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (1,), float32)` |
| **Dimension** | 1 |
---
## Observation Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (3,), float32)` |
| **Dimension** | 3 |
Order: `cos(theta), sin(theta), angular velocity`.
---
## Reward Function Design
- Upright reward: encourages angle near π (inverted)
- Energy shaping: target energy near inverted position
- Penalties: `ang_vel^2`, `ctrl^2`, `(ctrl - prev_ctrl)^2` to reduce oscillation and aggressive actuation
---
## Initial State
- Angle randomized in `[-pi, pi]`
- Angular velocity small random noise (if configured)
- Control history (`prev_ctrl`) reset to zero
## Episode Termination Conditions
- No fall/angle termination; only NaN check
- Episode length limited by `max_episode_seconds`
---
### 1. Environment Preview
```bash
uv run scripts/view.py --env pendulum
```
### 2. Start Training
```bash
# Train with default parameters
uv run scripts/train.py --env pendulum
# Customize parallel environments
uv run scripts/train.py --env pendulum --num-envs 1024
# Enable rendering during training
uv run scripts/train.py --env pendulum --render
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/pendulum
```
### 4. Test Training Results
```bash
# Auto-discover best policy (recommended)
uv run scripts/play.py --env pendulum
# Manually specify a policy file
uv run scripts/play.py --env pendulum --policy runs/pendulum/nn/best_policy.pickle
```
> **Tip**: Policies are auto-selected from `runs/pendulum/`. You can override with `--policy`.
---
## Configuration Parameters
### Environment Configuration
```python
@dataclass
class PendulumEnvCfg(EnvCfg):
model_file: str = ".../pendulum.xml" # MJCF model (gear=5)
max_episode_seconds: float = 20.0
sim_dt: float = 0.0125
ctrl_dt: float = 0.025
```
### Training Configuration (PPO example)
```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)
```
---
## Expected Training Results
1. Pendulum can swing up and stay near inverted
2. Oscillation around upright is reduced by angular-velocity and control-change penalties

381
docs/source/en/user_guide/demo/shadow_hand_repose.md Normal file
View File

@@ -0,0 +1,381 @@
# Shadow Hand Cube Repose
## Overview
This document describes in detail the Shadow Hand dexterous manipulation cube reorientation task environment. This is a classic benchmark test in the field of robotic manipulation, requiring the robot to reorient a cube in-hand to match a randomly sampled target orientation.
```{video} /_static/videos/shadow_hand_repose.mp4
:poster: _static/images/poster/shadow_hand_repose.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Environment Description
The Shadow Hand cube reorientation task is built based on the realistic Shadow Hand 24-DOF dexterous hand, designed to train robots to rotate a cube in-hand to a target pose through fine multi-finger coordination.
### Robot Structure
Shadow Hand is a high-fidelity 24-DOF dexterous hand composed of the following main parts:
- **Palm**: The base structure of the hand, fixed to the base
- **5 Fingers**:
- **Thumb**: 5 degrees of freedom, including CMC rotation, MCP, IP joints
- **Index**: 4 degrees of freedom, including MCP, PIP, DIP joints
- **Middle**: 4 degrees of freedom, including MCP, PIP, DIP joints
- **Ring**: 4 degrees of freedom, including MCP, PIP, DIP joints
- **Little**: 4 degrees of freedom, including MCP, PIP, DIP joints
- **Actuator Configuration**: 20 actuated joints (4 coupled joints driven by other joints)
### Finger Joint Details
Each finger (except thumb) contains:
- **MCP Joint**: Metacarpophalangeal joint, 2 degrees of freedom (abduction/adduction + flexion)
- **PIP Joint**: Proximal interphalangeal joint, 1 degree of freedom (flexion)
- **DIP Joint**: Distal interphalangeal joint, 1 degree of freedom (flexion)
Thumb contains:
- **CMC Joint**: Carpometacarpal joint, 2 degrees of freedom
- **MCP Joint**: Metacarpophalangeal joint, 1 degree of freedom
- **IP Joint**: Interphalangeal joint, 1 degree of freedom
### Environment Objects
- **Cube**: 50mm \* 50mm \* 50mm cube
- Mass: approximately 0.028 kg
- Friction coefficient: 1.2
- Initial position: above palm center `(0.33, 0.00, 0.295)` meters
- **Target Visualization**: Semi-transparent target pose indicator (mocap body)
### Task Goals
The robot needs to complete the following operation goals:
1. **Maintain Grasp**: Maintain stable grip of the cube in-hand
2. **Perceive Goal**: Observe target pose (visualization indicator)
3. **Fine Manipulation**: Rotate cube through multi-finger coordination
4. **Pose Alignment**: Rotate cube pose to target pose (tolerance <20>0.1 radian)
---
## Action Space
Action space is `Box(-1, 1, (20,), float32)`, representing position control commands applied to 20 actuated joints (normalized).
### Control Mode
The environment uses position control mode, actions are converted to joint target positions through:
```
1. Scale from [-1, 1] to actuator control range
2. Optional: action smoothing (moving average filter)
3. Clip to joint limits
4. Apply to simulator actuators
```
### Action Processing Flow
```python
# 1. Scale to actuator limits
targets = scale(actions, lower_limits, upper_limits)
# 2. Action smoothing (optional)
if act_moving_average < 1.0:
targets = α * targets + (1-α) * prev_actions
# 3. Clip to limits
targets = clip(targets, lower_limits, upper_limits)
# 4. Apply control
actuator_ctrls = targets
```
### Action Dimension Details
| Index | Finger | Joint | DOF | Description |
| ----- | ------ | ----- | --- | ------------- |
| 0-4 | Thumb | J0-J4 | 5 | CMC, MCP, IP |
| 5-8 | Index | J0-J3 | 4 | MCP, PIP, DIP |
| 9-12 | Middle | J0-J3 | 4 | MCP, PIP, DIP |
| 13-16 | Ring | J0-J3 | 4 | MCP, PIP, DIP |
| 17-20 | Little | J0-J3 | 4 | MCP, PIP, DIP |
---
## Observation Space
Observation space is `Box(-inf, inf, (157,), float32)`, containing robot proprioceptive information, cube state, target state, and fingertip state.
### Observation Components
Observation vector consists of the following parts (in order):
#### 1. Hand Joint State (48 dimensions)
- **Joint Position (24 dims)**: Unscaled raw joint angles
- **Joint Velocity (24 dims)**: Joint angular velocities scaled by 0.2
#### 2. Cube State (17 dimensions)
- **Position (3 dims)**: Cube position in world coordinates `(x, y, z)`
- **Orientation (4 dims)**: Quaternion `(x, y, z, w)`
- **Linear Velocity (3 dims)**: Cube linear velocity
- **Angular Velocity (3 dims)**: Angular velocity scaled by 0.2
- **Normalization Factor**: Velocity observations multiplied by `vel_obs_scale = 0.2`
#### 3. Goal State (11 dimensions)
- **Goal Position (3 dims)**: Fixed at `(0.33, 0.00, 0.295)`
- **Goal Orientation (4 dims)**: Randomly sampled target quaternion
- **Relative Rotation (4 dims)**: Relative quaternion from cube to goal
#### 4. Fingertip State (65 dimensions)
State of 5 fingertips, 13 dimensions per fingertip:
- **Position (3 dims)**: Fingertip position in Cartesian space
- **Orientation (4 dims)**: Fingertip quaternion
- **Velocity (6 dims)**: Linear and angular velocities
**Fingertip Link Names**:
- `rh_ffdistal`: Index fingertip
- `rh_mfdistal`: Middle fingertip
- `rh_rfdistal`: Ring fingertip
- `rh_lfdistal`: Little fingertip
- `rh_thdistal`: Thumb fingertip
#### 5. Action History (20 dimensions)
- Previous action values, for temporal context in the policy
### Observation Details
| Index | Observation Content | Dimension | Range | Unit |
| ------- | -------------------------------- | --------- | ------------ | ----- |
| 0-23 | Hand joint position (unscaled) | 24 | Joint limits | rad |
| 24-47 | Hand joint velocity (0.2) | 24 | ±π/2 | rad/s |
| 48-50 | Cube position | 3 | Real | m |
| 51-54 | Cube orientation (quat w,x,y,z) | 4 | Unit norm | - |
| 55-57 | Cube linear velocity | 3 | Real | m/s |
| 58-60 | Cube angular velocity (0.2) | 3 | Real | rad/s |
| 61-63 | Goal position | 3 | Fixed | m |
| 64-67 | Goal orientation (quat w,x,y,z) | 4 | Unit norm | - |
| 68-71 | Relative rotation (quat w,x,y,z) | 4 | Unit norm | - |
| 72-136 | Fingertip state (5\*13) | 65 | - | - |
| 137-156 | Previous action | 20 | [-1, 1] | - |
---
## Reward Function
The reward function uses a composite design with multiple reward and penalty terms.
### Main Reward Terms
1. **Rotation Alignment Reward** (core objective)
```
rot_reward = rot_reward_scale / (|rot_dist| + rot_eps)
```
- **Scaling factor**: `1.0`
- **Epsilon**: `0.1`
- **Rotation distance calculation**: Using quaternion rotation distance formula
- **Incentive**: Reward grows inversely as cube orientation approaches target
2. **Position Distance Penalty**
```
dist_reward = dist_reward_scale <20> goal_dist
```
- **Scaling factor**: `-10.0`
- **Distance calculation**: Euclidean distance from cube to goal position
- **Incentive**: Prevent cube from dropping, keep near target position
3. **Action Regularization Penalty**
```
action_penalty = action_penalty_scale <20> ||actions||<7C>
```
- **Scaling factor**: `-0.0002`
- **Purpose**: Encourage smooth, energy-efficient motion
### Conditional Rewards
4. **Success Reward**
```
if |rot_dist| d success_tolerance:
reward += reach_goal_bonus
```
- **Reward value**: `2.0`
- **Tolerance**: `0.1` radian (approximately 5.7<EFBFBD>)
- **Purpose**: Sparse reward for achieving goal alignment
5. **Drop Penalty**
```
if goal_dist e fall_dist:
reward += fall_penalty
terminated = True
```
- **Penalty value**: `0.0` (termination only, no additional penalty)
- **Distance threshold**: `0.24` meters
- **Purpose**: Terminate episode when cube is dropped
---
## Initial State
### Hand Initialization
**Position Initialization:**
The palm is fixed in the world coordinate system, position determined by the model file.
**Joint Angle Initialization:**
- Use model default joint positions
- Add uniform random noise: `[-0.2, 0.2]` radians
- Range: All 24 hand degrees of freedom
**Velocity Initialization:**
All joint velocities are initialized to zero.
### Cube Initialization
**Position Initialization:**
- Fixed position: `(0.33, 0.00, 0.295)` above palm center
- Add uniform random noise: `[-0.01, 0.01]` (<28>1cm)
**Orientation Initialization:**
- Use Shoemake method to generate uniformly distributed random quaternions
- Ensure uniform sampling on SO(3) space
**Velocity Initialization:**
All linear and angular velocities are initialized to zero.
### Goal Initialization
**Position Initialization:**
- Fixed position: `(0.33, 0.00, 0.295)` (same as cube initial position)
**Orientation Initialization:**
- Use Shoemake method to generate uniformly distributed random target quaternions
- Resampled each reset
---
## Termination Conditions
Episodes terminate under the following conditions:
1. **Drop Termination**: Cube distance from goal position e `fall_dist` (0.24m)
2. **Timeout Termination**: Reaching `max_episode_steps` (default 1000 steps)
3. **NaN Protection**: Detecting rotation distance or position distance as NaN
### Success Holding Mechanism
The environment uses a consecutive success counter:
- When rotation tolerance is satisfied, counter increments
- When `max_consecutive_successes` (50) is reached, trigger success termination and reset goal
- Rotation tolerance: `0.1` radian
---
## Usage
### Training
```bash
uv run scripts/train.py --env shadow-hand-repose
```
### Policy Evaluation
```bash
uv run scripts/play.py --env shadow-hand-repose
```
### Environment Visualization
```bash
uv run scripts/view.py --env shadow-hand-repose
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/shadow-hand-repose
```
---
## Configuration Parameters
### Environment Parameters
| Parameter | Default | Description |
| --------------------- | ------- | -------------------------- |
| `max_episode_seconds` | 10.0 | Maximum episode length (s) |
| `ctrl_dt` | 0.01 | Control timestep (s) |
| `max_episode_steps` | 1000 | Maximum episode steps |
| `num_hand_dofs` | 24 | Total hand DOFs |
| `num_actuators` | 20 | Number of actuated joints |
### Reward Parameters
| Parameter | Default | Description |
| ---------------------- | ------- | ------------------------- |
| `dist_reward_scale` | -10.0 | Position distance reward |
| `rot_reward_scale` | 1.0 | Rotation alignment reward |
| `rot_eps` | 0.1 | Rotation reward epsilon |
| `action_penalty_scale` | -0.0002 | Action regularization |
| `success_tolerance` | 0.1 | Success tolerance (rad) |
| `reach_goal_bonus` | 2.0 | Success reward |
| `fall_dist` | 0.24 | Drop distance threshold |
| `fall_penalty` | 0.0 | Drop penalty |
### Reset Noise Parameters
| Parameter | Default | Description |
| ---------------------- | ------- | ---------------------------- |
| `reset_position_noise` | 0.01 | Cube position noise (m) |
| `reset_dof_pos_noise` | 0.2 | Joint position noise (rad) |
| `reset_dof_vel_noise` | 0.0 | Joint velocity noise (rad/s) |
### Observation Scaling Parameters
| Parameter | Default | Description |
| --------------- | ------- | -------------------------- |
| `vel_obs_scale` | 0.2 | Velocity observation scale |
---
## References
This environment is based on the following classic works:
- **OpenAI Dactyl** (2018): First successful in-hand manipulation sim-to-real transfer
- **Isaac Gym** (2021): High-performance GPU-accelerated physics simulation
- **Isaac Lab** (2023): Modular robot learning framework

189
docs/source/en/user_guide/getting_started/container_deployment.md Normal file
View File

@@ -0,0 +1,189 @@
# Container Deployment
This document describes how to deploy MotrixLab using Docker containers to simplify environment configuration and enable rapid deployment.
## Prerequisites
- **Docker** and **Docker Compose**: [Installation Docs](https://docs.docker.com/engine/install/)
- **NVIDIA Container Toolkit** (for GPU support): [Installation Docs](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html#installation)
- **NVIDIA GPU**: Graphics card supporting CUDA 12.8
## Quick Start
### 1. Clone the Repository
```bash
git clone https://github.com/Motphys/MotrixLab.git
cd MotrixLab/docker
```
### 2. Start with Docker Compose
We provide a complete `docker-compose.yml` configuration file that supports one-click startup of training and TensorBoard visualization services.
```bash
# Start training and TensorBoard services
docker compose up -d
```
This will start the following services:
- **motrixlab-training**: Training container that executes reinforcement learning training tasks
- **motrixlab-tensorboard**: TensorBoard visualization service accessible via browser at http://localhost:6006
### 3. Configure Training Parameters
You can customize training configurations through environment variables:
```bash
# Set training backend (jax or torch)
export MOTRIX_TRAIN_BACKEND=jax
# Set number of parallel environments
export MOTRIX_NUM_ENVS=2048
# Set training environment name
export MOTRIX_ENV=cartpole
# Start services
docker compose up -d
```
| Environment Variable | Default | Description |
| :--------------------- | :--------- | :--------------------------------- |
| `MOTRIX_TRAIN_BACKEND` | `jax` | Training backend: `jax` or `torch` |
| `MOTRIX_NUM_ENVS` | `2048` | Number of parallel environments |
| `MOTRIX_ENV` | `cartpole` | Training environment name |
### 4. Monitor Training Progress
Training logs are automatically saved to the Docker Volume `motrixlab-data`. You can view them through:
```bash
# View training container logs
docker logs -f motrixlab-training
# Access TensorBoard
# Open in browser: http://localhost:6006
```
### 5. Stop Services
```bash
# Stop all services
docker compose down
# Stop services and remove data volumes
docker compose down -v
```
## Advanced Usage
### Building Docker Images
If you need to customize the image, you can build from source:
```bash
# Execute from project root directory
cd docker
docker build -t motphys/motrixlab:latest .
```
### Running Single Container
If you only want to run the training container without using Docker Compose:
```bash
docker run --gpus all \
-v $(pwd)/runs:/root/motrixlab/runs \
motphys/motrixlab:latest \
scripts/train.py --train-backend jax --num-envs 2048 --env cartpole
```
### Persisting Training Results
The default configuration uses the Docker Volume `motrixlab-data` to save training results. You can mount it to a host directory:
```bash
# Modify volumes configuration in docker-compose.yml
volumes:
- ./runs:/root/motrixlab/runs
```
## Image Details
Our Docker image is based on {bdg-primary-line}`NVIDIA CUDA 12.8.1` runtime environment and comes pre-installed with the following components:
- **UV Package Manager**: Fast, reliable dependency management
- **MotrixLab**: Complete reinforcement learning training framework
- **SKRL**: Reinforcement learning library supporting both JAX and PyTorch backends
- **TensorBoard**: Training process visualization tool
- **MotrixSim**: High-performance physics simulation engine
Image layer build process:
1. **Base Environment**: NVIDIA CUDA 12.8.1 Runtime + Ubuntu 24.04
2. **System Dependencies**: Install UV package manager and necessary system tools
3. **Python Dependencies**: Use UV cache mechanism for fast Python package installation
4. **Project Code**: Copy MotrixLab source code and complete dependency installation
## Troubleshooting
### GPU Not Available
If the container cannot access the GPU:
```bash
# Check NVIDIA Docker Runtime
docker run --rm --gpus all nvidia/cuda:12.8.1-base-ubuntu24.04 nvidia-smi
# Verify NVIDIA Container Toolkit is correctly installed
which nvidia-container-cli
```
### Insufficient Storage Space
Clean Docker cache and unused images:
```bash
# Clean build cache
docker builder prune
# Delete unused images
docker image prune -a
# Clean all unused resources
docker system prune -a
```
## Performance Optimization
### Accelerate Builds with UV Cache
The Dockerfile uses UV's cache mount feature, which can significantly speed up rebuilds:
```bash
# Rebuild using UV cache
docker build --cache-from motphys/motrixlab:latest -t motphys/motrixlab:latest .
```
### GPU Resource Allocation
You can specify the number of GPUs to use in `docker-compose.yml`:
```yaml
deploy:
resources:
reservations:
devices:
- driver: nvidia
device_ids: ["0", "1"] # Use GPU 0 and 1
capabilities: [gpu]
```
## Next Steps
- Check out the [Quick Start Tutorial](hello_motrixlab.md) to learn basic MotrixLab usage
- Read [Training Examples](../demo/cartpole.md) for more training tasks
- Explore [Basic Framework](../tutorial/basic_frame.md) for in-depth understanding of the framework architecture

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

@@ -5,6 +5,7 @@
:maxdepth: 1 :maxdepth: 1
getting_started/installation getting_started/installation
getting_started/container_deployment
getting_started/hello_motrixlab getting_started/hello_motrixlab
``` ```
@@ -25,16 +26,22 @@ tutorial/training_and_result
:caption: Training Examples :caption: Training Examples
:maxdepth: 1 :maxdepth: 1
demo/acrobot
demo/cartpole demo/cartpole
demo/pendulum
demo/bounce_ball demo/bounce_ball
demo/dm_walker demo/dm_walker
demo/dm_cheetah demo/dm_cheetah
demo/dm_hopper demo/dm_hopper
demo/dm_reacher demo/dm_reacher
demo/dm_finger
demo/dm_humanoid
demo/bring_ball
demo/locomotion_unitree_go1 demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c demo/anymal_c
demo/franka_lift_cube demo/franka_lift_cube
demo/franka_open_cabinet demo/franka_open_cabinet
demo/shadow_hand_repose
``` ```

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

@@ -161,6 +161,20 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:::{grid-item-card} :::{grid-item-card}
```{video} _static/videos/dm_humanoid_run.mp4
:poster: _static/images/poster/dm_humanoid_run.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/bounce_ball.mp4 ```{video} _static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg :poster: _static/images/poster/bounce_ball.jpg
:nocontrols: :nocontrols:

154
docs/source/zh_CN/user_guide/demo/acrobot.md Normal file
View File

@@ -0,0 +1,154 @@
# Acrobot 双连杆
Acrobot 是一个双连杆摆动和平衡任务。目标是使用一个电机扭矩摆动双臂并到达目标位置。
```{video} /_static/videos/acrobot.mp4
:poster: _static/images/poster/acrobot.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 任务描述
双连杆 Acrobot 由一个铰链关节驱动，该关节由单个电机控制。电机安装在肘关节处，这是系统中唯一的驱动关节。电机的扭矩使连杆在平面内旋转，实现从任意初始角度摆起并到达目标位置。扭矩受限于执行器的 ctrlrange；通过调节其大小和方向，策略必须积累能量以摆起并到达目标，同时保持稳定性。
## 动作空间
| 项目 | 详情 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (1,), float32)` |
| **维度** | 1 |
---
## 观察空间
| 项目 | 详情 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (6,), float32)` |
| **维度** | 6 |
顺序：`upper_arm_horizontal, lower_arm_horizontal, upper_arm_vertical, lower_arm_vertical, shoulder_velocity, elbow_velocity`。
---
## 奖励函数设计
- 基础稀疏奖励：鼓励末端进入目标区域（半径 = 0.2）
- 持续奖励：在目标区域内每步提供 0.1 的奖励
- 距离奖励：0.3 \* (1.0 - clip(distance / 2.0, 0, 1.0)) 鼓励向目标移动
- 速度惩罚：0.01 \* max(0, velocity_magnitude - 2.0) 惩罚过高的速度
---
## 初始状态
- 肩关节角度随机于 `[-pi, pi]`
- 肘关节角度随机于 `[-pi, pi]`
- 角速度初始化为零
## Episode 终止条件
- Episode 长度由 `max_episode_seconds` 限制
- 对观察值进行 NaN 检查
---
### 1. 环境预览
```bash
uv run scripts/view.py --env acrobot
```
### 2. 开始训练
```bash
# 使用默认参数训练
uv run scripts/train.py --env acrobot
# 自定义并行环境数
uv run scripts/train.py --env acrobot --num-envs 1024
# 开启训练时渲染
uv run scripts/train.py --env acrobot --render
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/acrobot
```
### 4. 测试训练结果
```bash
# 自动发现最佳策略（推荐）
uv run scripts/play.py --env acrobot
# 手动指定策略文件
uv run scripts/play.py --env acrobot --policy runs/acrobot/nn/best_policy.pickle
```
> **提示**：策略会从 `runs/acrobot/` 中自动选择。你可以使用 `--policy` 参数覆盖。
---
## 配置参数
### 环境配置
```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
```
### 训练配置（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
```
---
## 预期训练结果
1. Acrobot 能够摆动双臂到达目标位置
2. 末端能够稳定地停留在目标区域内
3. 过高的震荡通过速度惩罚得到减少
4. 策略能够以平滑的动作高效地接近目标

2
docs/source/zh_CN/user_guide/demo/anymal_c.md
View File

@@ -1,4 +1,4 @@
# ANYmal-C Locomotion # ANYmal-C 导航
## 概述 ## 概述

387
docs/source/zh_CN/user_guide/demo/bounce_ball.md
View File

@@ -42,46 +42,323 @@ Bounce Ball 是一个单臂机器人操作任务，使用 6 自由度的配天 A
| 项目 | 详细信息 | | 项目 | 详细信息 |
| -------- | -------------------------------- | | -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (25,), float32)` | | **类型** | `Box(-inf, inf, (29,), float32)` |
| **维度** | 25 | | **维度** | 29 |
观察空间由以下部分组成（按顺序）： 观察空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 | | 部分 | 内容说明 | 维度 | 备注 |
| ----------- | -------------------- | ---- | --------------------------------------------------------------- | | ----------------- | -------------------- | ---- | --------------------------------------------------------------- |
| **dof_pos** | 各关节自由度位置信息 | 13 | 前 6 个为机械臂关节，后 7 个为球的自由关节（3 位置 + 4 四元数） | | **dof_pos** | 各关节自由度位置信息 | 13 | 前 6 个为机械臂关节，后 7 个为球的自由关节（3 位置 + 4 四元数） |
| **dof_vel** | 各关节自由度速度信息 | 12 | 速度为位置导数 | | **dof_vel** | 各关节自由度速度信息 | 12 | 速度为位置导数 |
| **paddle_pos** | 挡板位置信息 | 3 | 挡板中心的 x, y, z 坐标 |
| **target_height** | 目标高度 | 1 | 当前环境的目标高度 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 类型 (单位) | | 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 类型 (单位) |
| ----- | -------------- | ------ | ------ | ----------- | ---------------- | | ----- | -------------------------- | ------ | ------ | --------- | ----------------- |
| 0-5 | 机械臂关节角度 | -Inf | Inf | Joint1-6 | 角度 (rad) | | 0-5 | 机械臂关节角度 | -Inf | Inf | Joint1-6 | 角度 (rad) |
| 6 | 球 x 坐标 | -Inf | Inf | ball_x | 位置 (m) | | 6-8 | 球位置 [x, y, z] | -Inf | Inf | ball_link | 位置 (m) |
| 7 | 球 y 坐标 | -Inf | Inf | ball_y | 位置 (m) | | 9-12 | 球姿态四元数 [w,x,y,z] | -Inf | Inf | ball_link | 四元数 |
| 8 | 球 z 坐标 | -Inf | Inf | ball_z | 位置 (m) | | 13-18 | 机械臂关节角速度 | -Inf | Inf | Joint1-6 | 角速度 (rad/s) |
| 9-12 | 球姿态四元数 | -Inf | Inf | ball_qw/xyz | 四元数 (w,x,y,z) | | 19-24 | 球速度 [vx,vy,vz,wx,wy,wz] | -Inf | Inf | ball_link | 速度 (m/s, rad/s) |
| 13-24 | 关节和球速度 | -Inf | Inf | - | 速度/角速度 | | 25-27 | 挡板位置 [x, y, z] | -Inf | Inf | blocker | 位置 (m) |
| 28 | 目标高度 | -Inf | Inf | - | 位置 (m) |
--- ---
## 奖励函数设计 ## 奖励函数
奖励函数由以下几个部分组成： 奖励函数采用复合设计，包含多个奖励和惩罚项，引导机器人学习稳定的颠球策略。所有奖励参数可通过配置文件调整。
```python ### 主要奖励项
# 位置控制奖励：保持球在挡板中心上方
# 受控向上速度奖励：在球位置良好时奖励适中的向上速度
# 高度精度奖励：球接近目标高度
# 连续弹跳奖励：奖励连续成功的弹跳
# 总奖励 = 各项加权组合
```
--- #### 1. 水平位置奖励
## 初始状态 **设计意义**：这是最核心的奖励项，确保球始终保持在挡板正上方。通过垂直距离加权机制，当球接近挡板时（即将击打时刻）对水平位置的要求更严格，引导策略在关键时刻精确对齐。
- **机械臂初始位置**：[0, 40, 110, 0, -60, 0] 度，带有随机噪声 **计算公式**：
- **球的初始位置**：挡板中心上方，带有随机噪声
- **球的初始速度**：[0.0, 0.0, 0.0] m/s $$
\begin{aligned}
\text{err}_{xy} &= \sqrt{(x_{ball} - x_{target})^2 + (y_{ball} - y_{target})^2} &&\text{（水平位置误差）} \\
d_{vert} &= |z_{ball} - z_{paddle}| &&\text{（垂直距离）} \\
w_{vert} &= e^{-d_{vert} / \sigma_{vert}} &&\text{（垂直距离权重）} \\
\sigma_{pos} &= \sigma_{base} \times (1.0 + k_{weight} \times w_{vert}) &&\text{（自适应尺度）} \\
r_{pos} &= e^{-\frac{\text{err}_{xy}^2}{2\sigma_{pos}^2}} &&\text{（高斯奖励）} \\
\\
\text{其中：} \quad &\sigma_{vert} = 0.15 \text{ m} &&\text{（垂直距离尺度）} \\
&\sigma_{base} = 0.1 \text{ m} &&\text{（基础水平尺度）} \\
&k_{weight} = 3.0 &&\text{（权重因子）} \\
&x_{target} = 0.58856 \text{ m}, \, y_{target} = 0.0 \text{ m} &&\text{（目标位置）}
\end{aligned}
$$
**权重**：2.0
#### 2. 位置偏离惩罚
**设计意义**：对严重偏离目标位置的情况施加强惩罚，防止球飞出控制范围。使用 sigmoid 函数实现平滑过渡，避免奖励函数不连续。
**计算公式**：
$$
r_{out} = -\frac{2.0}{1 + e^{-(\text{err}_{xy} - 0.05) / 0.03}} \qquad \text{（sigmoid 惩罚）}
$$
**权重**：1.0
#### 3. 速度匹配奖励
**设计意义**：基于抛体运动物理规律，激励球的运动轨迹能够在目标高度时具有期望的速度（0.5 m/s）。这确保球不会过快或过慢地通过目标高度，有利于稳定控制。
**计算公式**：
$$
\begin{aligned}
\Delta h &= h_{target} - z_{ball} &&\text{（高度差）} \\
v_{desired} &= 0.5 \text{ m/s} &&\text{（期望速度）} \\
\\
\text{情况1：}&\text{球向上运动且低于目标高度} \\
v_{z,up}^2 &= v_z^2 - 2g\Delta h &&\text{（能量守恒）} \\
v_{at\_target,up} &= \sqrt{\max(0, v_{z,up}^2)} &&\text{（向上到达速度）} \\
\\
\text{情况2：}&\text{球向下运动且高于目标高度} \\
v_{z,down}^2 &= v_z^2 + 2g|\Delta h| &&\text{（能量守恒）} \\
v_{at\_target,down} &= -\sqrt{\max(0, v_{z,down}^2)} &&\text{（向下到达速度）} \\
\\
\text{情况3：}&\text{球接近目标高度（} |\Delta h| < 0.05 \text{ m）} \\
v_{at\_target,near} &= v_z &&\text{（当前速度）} \\
\\
\text{平滑组合：}& \\
\sigma_{up} &= \frac{1}{1 + e^{-v_z / 0.2}} &&\text{（向上运动权重）} \\
\sigma_{below} &= \frac{1}{1 + e^{-\Delta h / 0.02}} &&\text{（低于目标权重）} \\
\sigma_{down} &= 1 - \sigma_{up} &&\text{（向下运动权重）} \\
\sigma_{above} &= 1 - \sigma_{below} &&\text{（高于目标权重）} \\
w_{near} &= e^{-\frac{\Delta h^2}{2 \times 0.01^2}} &&\text{（接近目标权重）} \\
\\
v_{at\_target} &= v_{at\_target,up} \cdot \sigma_{up} \cdot \sigma_{below} \\
&\quad + v_{at\_target,down} \cdot \sigma_{down} \cdot \sigma_{above} \\
&\quad + v_{at\_target,near} \cdot w_{near} &&\text{（加权组合）} \\
\\
\text{err}_{vel} &= |v_{at\_target} - v_{desired}| &&\text{（速度误差）} \\
r_{vel} &= e^{-\frac{\text{err}_{vel}^2}{2 \times 0.8^2}} &&\text{（高斯奖励）}
\end{aligned}
$$
**权重**：2.0
#### 4. 高度奖励
**设计意义**：直接激励球接近目标高度，这是任务的核心目标之一。较高的权重（4.5）确保策略优先考虑高度控制。目标高度在每个环境中随机采样（0.3-0.6 m），提升策略的泛化能力。
**计算公式**：
$$
\begin{aligned}
\text{err}_h &= |z_{ball} - h_{target}| &&\text{（高度误差）} \\
r_h &= e^{-\frac{\text{err}_h^2}{2 \times 0.15^2}} &&\text{（高斯奖励）}
\end{aligned}
$$
**权重**：4.5
#### 5. 高度进步奖励
**设计意义**：激励球达到更高位置，帮助策略在训练初期快速学会向上击打球，避免陷入"不击打"的局部最优。
**计算公式**：
$$
r_{progress} = \max(0, z_{ball} - 0.2) \times 2.0 \qquad \text{（线性奖励）}
$$
**权重**：1.0
#### 6. 受控向上速度奖励
**设计意义**：只有当球水平位置良好时才奖励向上速度，避免"乱打"行为。理想速度根据物理公式计算，确保球能恰好达到目标高度。这个奖励引导策略学习精确的击打力度。
**计算公式**：
$$
\begin{aligned}
q_{pos} &= e^{-\frac{\text{err}_{xy}^2}{2 \times 0.02^2}} &&\text{（位置质量）} \\
v_{ideal} &= \sqrt{2g \times \max(0, \Delta h)} &&\text{（理想发射速度）} \\
v_{ideal} &\in [0.5, 3.0] \text{ m/s} &&\text{（限制范围）} \\
q_{vel} &= e^{-\frac{(v_z - v_{ideal})^2}{2 \times 0.5^2}} &&\text{（速度质量）} \\
\sigma_{up} &= \frac{1}{1 + e^{-v_z / 0.1}} &&\text{（向上掩码）} \\
r_{controlled} &= q_{pos} \times q_{vel} \times \sigma_{up} \times \text{clip}(v_z, 0, 1.5) &&\text{（组合奖励）}
\end{aligned}
$$
**权重**：1.5
#### 7. 连续弹跳奖励
**设计意义**：激励多次连续成功弹跳，引导策略学习稳定的长期控制。使用对数函数避免奖励无限增长，同时要求球位置良好才给予奖励。
**计算公式**：
$$
\begin{aligned}
q_{bounce} &= e^{-\frac{\text{err}_{xy}^2}{2 \times 0.05^2}} &&\text{（弹跳位置质量）} \\
r_{bounce\_log} &= 0.5 \times \log(n_{bounces} + 1) &&\text{（对数奖励）} \\
r_{bounce} &= r_{bounce\_log} \times q_{bounce} \times \mathbb{1}_{n_{bounces} > 0} &&\text{（条件奖励）}
\end{aligned}
$$
**权重**：0.8
#### 8. 高弹跳次数奖励
**设计意义**：对高弹跳次数（≥3 次）给予额外奖励，进一步激励长期稳定控制。使用 sigmoid 激活函数使奖励平滑增长。
**计算公式**：
$$
\begin{aligned}
\sigma_{high} &= \frac{1}{1 + e^{-(n_{bounces} - 2.0) / 0.5}} &&\text{（高弹跳激活）} \\
r_{high} &= n_{bounces} \times 0.15 \times q_{bounce} \times \sigma_{high} &&\text{（额外奖励）}
\end{aligned}
$$
**权重**：0.3
#### 9. 挡板-球水平对齐奖励
**设计意义**：激励挡板主动移动到球的正下方，而不是等待球落下。垂直距离越近权重越大，在即将击打时刻要求更精确的对齐。弹跳时刻给予额外奖励（boost），强化正确的击打行为。
**计算公式**：
$$
\begin{aligned}
\text{err}_{align} &= \|(\mathbf{x}_{ball}, \mathbf{y}_{ball}) - (\mathbf{x}_{paddle}, \mathbf{y}_{paddle})\|_2 &&\text{（对齐误差）} \\
w_{prox} &= e^{-d_{vert} / 0.1} &&\text{（垂直接近权重）} \\
q_{align} &= e^{-\frac{\text{err}_{align}^2}{2 \times 0.03^2}} \times (1.0 + 2.0 \times w_{prox}) &&\text{（对齐质量）} \\
k_{boost} &= \begin{cases} 3.0 & \text{if bounce detected} \\ 1.0 & \text{otherwise} \end{cases} &&\text{（弹跳增益）} \\
r_{align} &= q_{align} \times k_{boost} \times 0.3 &&\text{（对齐奖励）}
\end{aligned}
$$
**权重**：0.6
#### 10. 挡板原位奖励
**设计意义**：激励挡板在球远离时回到原位（home position），避免挡板长时间停留在高位。使用距离动态因子：当球远离挡板时（$d_{vert} > 0.15$ m）增大奖励，鼓励挡板快速回位；当球接近时减小奖励，允许挡板上移准备击打。这种设计使挡板运动更加节能和自然。
**计算公式**：
$$
\begin{aligned}
\text{err}_{home} &= |z_{paddle} - z_{home}| &&\text{（原位偏离）} \\
k_{dist} &= 1.0 + \frac{0.5}{1 + e^{-(d_{vert} - 0.15) / 0.03}} &&\text{（距离因子）} \\
q_{home} &= e^{-\frac{\text{err}_{home}^2}{2 \times 0.05^2}} &&\text{（原位质量）} \\
r_{home} &= q_{home} \times k_{dist} &&\text{（原位奖励）} \\
\\
\text{其中：} \quad &z_{home} = 0.05 \text{ m} &&\text{（挡板原位高度）}
\end{aligned}
$$
**权重**：1.5
### 惩罚项
#### 1. 过高向上速度惩罚
**设计意义**：防止球速度过快（>3.5 m/s）失去控制，确保球的运动在可控范围内。
**计算公式**：
$$
r_{excess} = -\frac{1.0}{1 + e^{-(v_z - 3.5) / 0.3}} \qquad \text{（sigmoid 惩罚）}
$$
**权重**：1.0
#### 2. 向下速度惩罚
**设计意义**：惩罚球向下运动（$v_z < -0.2$ m/s），激励策略及时击打球，避免球自由下落。
**计算公式**：
$$
\begin{aligned}
\text{mag}_{down} &= -v_z \times \text{clip}(-v_z \times 0.3, 0, 0.5) &&\text{（向下幅度）} \\
\sigma_{down} &= \frac{1}{1 + e^{(v_z + 0.2) / 0.2}} &&\text{（向下激活）} \\
r_{down} &= \text{mag}_{down} \times \sigma_{down} &&\text{（向下惩罚）}
\end{aligned}
$$
**权重**：1.0
#### 3. 挡板高度违规惩罚
**设计意义**：对挡板偏离原位过远（>0.1 m）施加强惩罚，确保挡板不会长时间停留在高位。
**计算公式**：
$$
\begin{aligned}
\text{viol}_{height} &= \max(0, \text{err}_{home} - 0.1) &&\text{（违规量）} \\
r_{violation} &= -20.0 \times \text{viol}_{height} &&\text{（强惩罚）}
\end{aligned}
$$
**权重**：1.0
#### 4. 动作变化率惩罚
**设计意义**：惩罚动作的剧烈变化，鼓励平滑的控制策略。
**计算公式**：
$$
\begin{aligned}
r_{action} &= -\|\mathbf{a}_t - \mathbf{a}_{t-1}\|^2 &&\text{（L2 惩罚）} \\
\\
\text{其中：} \quad &\mathbf{a}_t \in \mathbb{R}^6 &&\text{（当前动作向量）}
\end{aligned}
$$
**权重**：$10^{-4}$
#### 5. 关节速度惩罚
**设计意义**：惩罚过高的关节速度，鼓励节能和平滑的运动。
**计算公式**：
$$
\begin{aligned}
r_{joint\_vel} &= -\sum_{i=1}^{6} \dot{q}_i^2 &&\text{（速度平方和惩罚）} \\
\\
\text{其中：} \quad &\dot{\mathbf{q}} = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4, \dot{q}_5, \dot{q}_6]^T &&\text{（关节角速度向量）}
\end{aligned}
$$
**权重**：$10^{-4}$
### 总奖励计算
$$
\begin{aligned}
R_{total} = &\ 2.0 \cdot r_{pos} \\
&+ 1.0 \cdot r_{out} \\
&+ 2.0 \cdot r_{vel} \\
&+ 4.5 \cdot r_h \\
&+ 1.0 \cdot r_{progress} \\
&+ 1.5 \cdot r_{controlled} \\
&+ 0.8 \cdot r_{bounce} \\
&+ 0.3 \cdot r_{high} \\
&+ 0.6 \cdot r_{align} \\
&+ 1.5 \cdot r_{home} \\
&+ 1.0 \cdot r_{excess} \\
&+ 1.0 \cdot r_{down} \\
&+ 1.0 \cdot r_{violation} \\
&+ 10^{-4} \cdot r_{action} \\
&+ 10^{-4} \cdot r_{joint\_vel}
\end{aligned}
$$
--- ---
@@ -89,7 +366,43 @@ Bounce Ball 是一个单臂机器人操作任务，使用 6 自由度的配天 A
- **球掉落**：球的 z 坐标 < 0.05m（接近地面） - **球掉落**：球的 z 坐标 < 0.05m（接近地面）
- **球过高**：球的 z 坐标 > 目标高度 + 1.0m（失去控制） - **球过高**：球的 z 坐标 > 目标高度 + 1.0m（失去控制）
- **水平偏离过远**：球的 x 坐标绝对值 > 1.5m - **水平偏离过远**：球的 x 或 y 坐标绝对值 > 1.5m
- **关节速度过高**：任一关节角速度 > 2π rad/s（360°/s）
- **超时**：Episode 时长超过最大允许时间
---
## 初始状态
### 机器人初始化
**关节角度**：
- 默认角度：[0°, 40°, 110°, 0°, -60°, 0°]
- 随机噪声：每个关节在 [-0.1, 0.1] 弧度范围内添加均匀随机噪声
**关节速度**：
- 初始化为零，带有小幅随机噪声
### 球初始化
**位置**：
- 基准位置：配置文件中的 `ball_init_pos`（默认 [0.58856, 0, 0.45] m）
- 随机噪声：在 [-0.01, 0.01] m 范围内添加均匀随机噪声
**速度**：
- 初始化为零
**姿态**：
- 四元数：[0, 0, 0, 1]（单位四元数，无旋转）
### 目标高度
每个环境的目标高度在 [0.4, 0.6] m 范围内随机采样，提升策略的泛化能力。
--- ---
@@ -123,8 +436,14 @@ uv run scripts/play.py --env bounce_ball
## 预期训练结果 ## 预期训练结果
1. 连续弹跳：能够实现 3 次以上的连续弹跳 1. **连续弹跳**：能够实现 3 次以上的连续弹跳
2. 位置控制：球的水平位置（x 坐标）稳定在目标位置 ± 0.05m 范围内 2. **位置控制**：球的水平位置稳定在目标位置 ± 0.05m 范围内
3. 高度控制：球的高度稳定在目标高度 0.8 ± 0.1m 范围内 3. **高度控制**：球的高度稳定在目标高度 ± 0.1m 范围内
4. 速度控制：球的向上速度保持在合理范围（0.1-1.5 m/s）内 4. **速度控制**：球的向上速度保持在合理范围（0.1-1.5 m/s）内
5. 稳定控制：能够持续 20 秒的稳定弹跳而不掉落 5. **稳定控制**：能够持续 20 秒的稳定颠球而不触发终止条件
---
## 已知问题
- **JAX 后端训练效果不佳**：当前 JAX 版本的训练效果不理想。建议使用 PyTorch 后端进行该环境的训练以获得更好的效果。

169
docs/source/zh_CN/user_guide/demo/bring_ball.md Normal file
View File

@@ -0,0 +1,169 @@
# 机械手抓球
Bring Ball 是 DeepMind Control Suite 中的 Manipulator 经典任务：平面机械手（带拇指与手指）需要抓取球体并将其移动到目标球位置。MotrixLab 当前提供 1 个 Bring Ball 环境：
- `dm-manipulator-bring-ball`：抓取球体并移动到目标位置
```{video} /_static/videos/bring-ball.mp4
:poster: _static/images/poster/bring-ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 任务描述
Bring Ball 是一个二维平面（x-z）内的抓取与搬运任务：
- 机械手包含 4 个手臂关节（`arm_root`、`arm_shoulder`、`arm_elbow`、`arm_wrist`）以及拇指/手指关节
- 夹持通过腱 `grasp` 同时驱动 `thumb` 与 `finger` 关节闭合
- 球体可在平面内滑动（`ball_x`、`ball_z`）并绕 y 轴旋转（`ball_y`）
- 目标球为 mocap 物体，位置在 reset 时随机采样
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (5,), float32)` |
| **维度** | 5 |
动作对应如下（控制信号施加在关节/腱上）：
| 序号 | 动作含义 | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ----------------------------- | :----: | :----: | :-------------: |
| 0 | 根部关节驱动 | -1 | 1 | `root` |
| 1 | 肩部关节驱动 | -1 | 1 | `shoulder` |
| 2 | 肘部关节驱动 | -1 | 1 | `elbow` |
| 3 | 腕部关节驱动 | -1 | 1 | `wrist` |
| 4 | 夹持驱动（拇指/手指耦合闭合） | -1 | 1 | `grasp` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (41,), float32)` |
| **维度** | 41 |
观察向量由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| -------------- | --------------------------- | ---- | ------------------------------------ |
| **arm_pos** | 8 个关节角度的 `sin`/`cos` | 16 | 关节顺序见下表，`sin`/`cos` 交替排列 |
| **arm_vel** | 8 个关节角速度 | 8 | 顺序同 arm_pos |
| **touch** | 触觉传感器 `log(1 + touch)` | 5 | palm/finger/thumb/fingertip/thumbtip |
| **hand_pos** | 抓取点 `grasp` 的世界坐标 | 3 | x, y, z |
| **object_pos** | 球体位置 | 3 | x, y, z |
| **target_pos** | 目标球位置 | 3 | x, y, z |
| **rel** | `object_pos - target_pos` | 3 | 相对位置 |
| 序号 | 观察量范围 | 维度 | 备注 |
| ----- | ---------------------------------------------- | ---- | ----------------------------- |
| 0-15 | 8 个关节角度的 `sin`/`cos` 交替排列 | 16 | 关节顺序：arm_root → thumbtip |
| 16-23 | 8 个关节角速度 | 8 | 顺序同上 |
| 24-28 | 触觉：palm, finger, thumb, fingertip, thumbtip | 5 | `log(1 + touch)` |
| 29-31 | grasp 位置 (x, y, z) | 3 | hand_pos |
| 32-34 | 球体位置 (x, y, z) | 3 | object_pos |
| 35-37 | 目标球位置 (x, y, z) | 3 | target_pos |
| 38-40 | 球体相对目标位置 (x, y, z) | 3 | rel |
关节顺序为：`arm_root`、`arm_shoulder`、`arm_elbow`、`arm_wrist`、`finger`、`fingertip`、`thumb`、`thumbtip`。
---
## 奖励函数设计
Bring Ball 使用 shaped 奖励，由多个子项加权组合，并加入惩罚项：
```python
# R1: Reach - 手指接近球
r_reach = tolerance(avg_tip_dist)
# R2: Orient - 手掌朝向球体
r_orient = clip(1 - orient_bound + dot(hand_dir, unit_vec_to_ball), 0..1)
# R3: Pause - 靠近球体时抑制臂部抖动
r_pause = tolerance(arm_speed_step) * is_close_to_ball
# R4: Close - 夹持动作与接触条件联合
r_close = r_close_intent * (approach_or_grasp)
# R5: Lift & Transport - 抬升高度 + 接近目标
r_lift_height = tolerance(ball_z)
r_transport = tolerance(move_dist_to_target)
r_lift = mix(r_lift_height, r_transport)
# Precision/Progress - 目标精度与移动进度
r_precision = tolerance(move_dist_to_target, gaussian)
r_progress = (prev_dist - curr_dist) * scale
# Penalty - 侧面接触与悬停惩罚
penalty_side + penalty_hover
```
默认权重（`BringBallCfg`）：
- reach 1.0、orient 1.5、pause 0.5、close 2.0、lift 6.0、precision 1.0
- lift 内部由 `lift_height_weight` 与 `transport_weight` 组合
- 进度奖励由 `transport_progress_scale` 控制
---
## 初始状态
- **手臂初始化**：默认使用模型初始姿态（`randomize_arm=False`），拇指/手指对称
- **目标位置**：`x ∈ [-0.4, 0.4]`，`z ∈ [0.1, 0.4]`，`y = 0.001`
- **球体位置**：`x ∈ [-0.4, 0.4]`，`z ∈ [0.2, 0.7]`，并与手部保持最小距离
- **物理稳定**：reset 后会进行若干步 settle（`settle_steps=300`）
---
## Episode 终止条件
- 若观测中出现 `NaN` 值则终止
---
## 使用指南
### 1. 环境预览（随机动作）
```bash
uv run scripts/view.py --env dm-manipulator-bring-ball
```
### 2. 开始训练
建议显式指定训练后端（JAX / PyTorch 二选一）：
```bash
uv run scripts/train.py --env dm-manipulator-bring-ball --train-backend torch
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-manipulator-bring-ball
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-manipulator-bring-ball
```
---
## 预期训练结果
1. 机械手能稳定接近并夹持球体
2. 球体被抬离地面并保持稳定高度
3. 球体最终能稳定到达目标球附近

5
docs/source/zh_CN/user_guide/demo/dm_cheetah.md
View File

@@ -127,5 +127,6 @@ uv run scripts/play.py --env dm-cheetah
## 预期训练结果 ## 预期训练结果
1. 接近或超过 30.0 m/s 的稳定水平速度 1. 以接近或超过 10.0 m/s 的稳定水平速度奔跑
2. 保持躯干直立且步态协调 2. 保持躯体稳定性且步态协调，长距离奔跑不摔倒
3. 奔跑姿态接近真实猎豹，奔跑过程具有伸展感

193
docs/source/zh_CN/user_guide/demo/dm_finger.md Normal file
View File

@@ -0,0 +1,193 @@
# Finger 机械手指
Finger 是 DeepMind Control Suite 中的经典操控任务：一个由两段连杆组成的“手指”通过施加关节力矩与旋转拨片（spinner）交互。MotrixLab 当前提供了 3 个 Finger 相关环境：
```{video} /_static/videos/dm_finger_spin.mp4
:poster: _static/images/poster/dm_finger_spin.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
```{video} /_static/videos/dm_finger_turn.mp4
:poster: _static/images/poster/dm_finger_turn.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
- `dm-finger-spin`：让 spinner 持续向指定方向旋转
- `dm-finger-turn-easy`：将 spinner 的顶端（tip）对准目标点（较大目标半径）
- `dm-finger-turn-hard`：同 Turn，但目标半径更小
---
## 任务描述
Finger 是一个二维平面（x-z）内的手指与旋转拨片交互任务：
- 手指有 2 个受控关节：`proximal`、`distal`
- spinner 通过关节 `hinge` 转动，`tip` 表示 spinner 顶端位置
- Turn 任务会在 spinner 周围随机采样一个目标点（target）
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (2,), float32)` |
| **维度** | 2 |
动作对应如下：
| 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 近端关节 `proximal` 驱动扭矩 | -1 | 1 | `proximal` |
| 1 | 远端关节 `distal` 驱动扭矩 | -1 | 1 | `distal` |
---
## 观察空间
Finger 环境的观测以 dm_control 的 observation dict 为参考，但在 MotrixLab 中被拼成一个向量。
### Spin 观察空间
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (9,), float32)` |
| **维度** | 9 |
组成如下（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| ------------ | --------------------------------------------- | ---- | ----------------------------- |
| **position** | `qpos(proximal, distal)` + `tip_xz` | 4 | tip 相对 spinner 的 x、z 位置 |
| **velocity** | `qvel(proximal, distal, hinge)` | 3 | hinge 角速度用于 Spin 奖励 |
| **touch** | `log(1 + touch_top)`、`log(1 + touch_bottom)` | 2 | 触觉传感器的对数压缩 |
| 序号 | 观察量 | 最小值 | 最大值 | XML/Sensor 名称 | 类型 (单位) |
| ---- | ---------------------------------- | ------ | ------ | ----------------------- | -------------- |
| 0 | `proximal` 关节角度 | -Inf | Inf | `proximal` | 角度 (rad) |
| 1 | `distal` 关节角度 | -Inf | Inf | `distal` | 角度 (rad) |
| 2 | tip 相对 spinner 的 x 位移 | -Inf | Inf | `framepos(tip/spinner)` | 位置 (m) |
| 3 | tip 相对 spinner 的 z 位移 | -Inf | Inf | `framepos(tip/spinner)` | 位置 (m) |
| 4 | `proximal` 角速度 | -Inf | Inf | `proximal_velocity` | 角速度 (rad/s) |
| 5 | `distal` 角速度 | -Inf | Inf | `distal_velocity` | 角速度 (rad/s) |
| 6 | spinner 的 `hinge` 角速度 | -Inf | Inf | `hinge_velocity` | 角速度 (rad/s) |
| 7 | 触觉（上侧）`log(1 + touchtop)` | -Inf | Inf | `touchtop` | 无量纲 |
| 8 | 触觉（下侧）`log(1 + touchbottom)` | -Inf | Inf | `touchbottom` | 无量纲 |
### Turn 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (12,), float32)` |
| **维度** | 12 |
相较 Spin 额外增加：
| 部分 | 内容说明 | 维度 | 备注 |
| ------------------- | -------------------------------- | ---- | ------------------------- |
| **target_position** | target 相对 spinner 的 x、z 坐标 | 2 | target 在 reset 时采样 |
| **dist_to_target** | tip 到 target 球面距离（带符号） | 1 | 负值表示 tip 落在目标球内 |
向量最后 3 个维度为：`target_x`, `target_z`, `dist_to_target`。
---
## 奖励函数设计
### Spin
在 dm_control 中，Spin 的稀疏奖励通常由 spinner 的角速度阈值触发。MotrixLab 默认使用更易训练的 dense/shaped 奖励，并同时在 info 中记录稀疏版本：
```python
# hinge_velocity 为 spinner 关节角速度
spin_sparse = 1 if hinge_velocity <= -15 else 0
# shaped: clip(-hinge_velocity / 15, 0..1)
spin = clip(-hinge_velocity / 15, 0, 1)
```
### Turn（Easy / Hard）
Turn 的核心是 tip 触达并对准目标点：目标点位于 spinner 周围一圈，目标球半径在 easy/hard 中不同。
- `turn_sparse = 1` 当 tip 进入目标球内部（`dist_to_target <= 0`）
- 默认 shaped 奖励以 `dist_to_target` 的指数衰减为主，并额外加入“靠近 spinner、增加接触、抑制动作抖动”等项（最终裁剪到 `[0, 1]`）
---
## 初始状态
- `proximal`、`distal` 关节角：在各自关节限制范围内均匀采样
- spinner 的 `hinge` 角：`[-pi, pi]` 均匀采样
- Turn 任务：每个 episode 在 spinner 周围采样 target（角度均匀采样，位置落在 x-z 平面）
## Episode 终止条件
- 若观测中出现 `NaN` 值则终止
---
## 使用指南
### 1. 环境预览（随机动作）
```bash
uv run scripts/view.py --env dm-finger-spin
```
```bash
uv run scripts/view.py --env dm-finger-turn-easy
```
```bash
uv run scripts/view.py --env dm-finger-turn-hard
```
### 2. 开始训练
建议显式指定训练后端（JAX / PyTorch 二选一）：
```bash
uv run scripts/train.py --env dm-finger-spin --train-backend torch
```
```bash
uv run scripts/train.py --env dm-finger-turn-easy --train-backend torch
```
```bash
uv run scripts/train.py --env dm-finger-turn-hard --train-backend torch
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-finger-spin
```
### 4. 测试训练结果
`scripts/play.py` 默认会自动在 `runs/{env-name}/` 下寻找最新的 `best_agent.*`，也可以用 `--policy` 显式指定：
```bash
uv run scripts/play.py --env dm-finger-turn-hard
```
---
## 预期训练结果
1. `dm-finger-spin`：spinner 能稳定持续向目标方向旋转（hinge_velocity 达到阈值附近）
2. `dm-finger-turn-easy`：手指能稳定接触并将 tip 对准目标点（成功率较高、抖动较小）
3. `dm-finger-turn-hard`：能对准更小目标，但更容易出现“接触不足/动作抖动”的训练难点

236
docs/source/zh_CN/user_guide/demo/dm_humanoid.md Normal file
View File

@@ -0,0 +1,236 @@
# 三维人形机器人
三维人形机器人（Humanoid）是 DeepMind Control Suite 中的经典双足行走任务。其目标是训练一个模拟的三维人形机器人，通过控制其关节力矩，实现站立、行走和奔跑。
```{video} /_static/videos/dm_humanoid_run.mp4
:poster: _static/images/poster/dm_humanoid_run.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
Humanoid 是一个三维空间的双足人形机器人任务。机器人由头部、躯干、双臂和双腿组成，拥有 21 个受控关节（执行器），智能体通过向这些关节施加扭矩作为动作，让机器人实现站立平衡、向前行走或快速奔跑。该任务要求协调的双足步态、平衡控制和三维空间姿态稳定能力。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-1.0, 1.0, (21,), float32)` |
| **维度** | 21 |
动作对应如下：
| 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :---------------- |
| 0 | 腹部 Y 轴旋转关节驱动扭矩 | -1.0 | 1.0 | `abdomen_y` |
| 1 | 腹部 Z 轴旋转关节驱动扭矩 | -1.0 | 1.0 | `abdomen_z` |
| 2 | 腹部 X 轴旋转关节驱动扭矩 | -1.0 | 1.0 | `abdomen_x` |
| 3 | 右髋关节 X 轴驱动扭矩 | -1.0 | 1.0 | `right_hip_x` |
| 4 | 右髋关节 Z 轴驱动扭矩 | -1.0 | 1.0 | `right_hip_z` |
| 5 | 右髋关节 Y 轴驱动扭矩 | -1.0 | 1.0 | `right_hip_y` |
| 6 | 右膝关节驱动扭矩 | -1.0 | 1.0 | `right_knee` |
| 7 | 右踝关节 X 轴驱动扭矩 | -1.0 | 1.0 | `right_ankle_x` |
| 8 | 右踝关节 Y 轴驱动扭矩 | -1.0 | 1.0 | `right_ankle_y` |
| 9 | 左髋关节 X 轴驱动扭矩 | -1.0 | 1.0 | `left_hip_x` |
| 10 | 左髋关节 Z 轴驱动扭矩 | -1.0 | 1.0 | `left_hip_z` |
| 11 | 左髋关节 Y 轴驱动扭矩 | -1.0 | 1.0 | `left_hip_y` |
| 12 | 左膝关节驱动扭矩 | -1.0 | 1.0 | `left_knee` |
| 13 | 左踝关节 X 轴驱动扭矩 | -1.0 | 1.0 | `left_ankle_x` |
| 14 | 左踝关节 Y 轴驱动扭矩 | -1.0 | 1.0 | `left_ankle_y` |
| 15 | 右肩关节 1 驱动扭矩 | -1.0 | 1.0 | `right_shoulder1` |
| 16 | 右肩关节 2 驱动扭矩 | -1.0 | 1.0 | `right_shoulder2` |
| 17 | 右肘关节驱动扭矩 | -1.0 | 1.0 | `right_elbow` |
| 18 | 左肩关节 1 驱动扭矩 | -1.0 | 1.0 | `left_shoulder1` |
| 19 | 左肩关节 2 驱动扭矩 | -1.0 | 1.0 | `left_shoulder2` |
| 20 | 左肘关节驱动扭矩 | -1.0 | 1.0 | `left_elbow` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (73,), float32)` |
| **维度** | 73 |
Humanoid 环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| ------------------ | ------------------------------------- | ---- | ------------------------------------------------- |
| **joint_angles** | 各关节角度（排除根关节的 7 个自由度） | 22 | 22 个关节的角度信息 |
| **head_height** | 头部高度 | 1 | 头部相对于地面的高度 |
| **extremities** | 四肢末端位置（相对于躯干） | 12 | 左手、左脚、右手、右脚的位置（各 3 维，按此顺序） |
| **torso_vertical** | 躯干垂直方向向量 | 3 | 躯干在局部坐标系中的垂直方向 |
| **com_vel** | 质心线速度 | 3 | 躯干子树的线速度 |
| **qvel** | 所有关节和根部的速度信息 | 29 | 包括根关节的 6 个自由度 |
| **target_local** | 目标方向（局部坐标系） | 3 | 目标方向在躯干局部坐标系中的表示 |
---
## 奖励函数设计
Humanoid 的奖励函数根据任务类型（站立、行走、奔跑）有所不同，但都包含以下核心组件：
### 姿态奖励（Posture Reward）
```python
# 头部高度奖励：保持头部在目标高度（标准站立高度的 95%，约 1.33m）以上
stand_reward = tolerance(head_height, bounds=(stand_height * 0.95, inf), margin=0.5)
# 躯干直立奖励：保持躯干直立
upright_reward = tolerance(torso_upright, bounds=(0.9, inf), sigmoid="linear", margin=0.9)
# 盆骨高度奖励：保持盆骨在合理高度（标准站立高度的 60%，约 0.84m）以上
pelvis_height_reward = tolerance(pelvis_height, bounds=(stand_height * 0.6, inf), sigmoid="linear", margin=stand_height * 0.6)
# 姿态奖励 = 头部高度奖励 × 躯干直立奖励 × 盆骨高度奖励
posture_reward = stand_reward * upright_reward * pelvis_height_reward
```
### 速度奖励（Speed Reward）
根据任务类型，速度奖励的计算方式不同：
**站立任务（move_speed <= 0）**：
```python
# 速度奖励：保持接近零速度
speed_reward = tolerance(actual_speed, bounds=(0, 0), margin=1.0, value_at_margin=0.01)
```
**行走任务（0 < move_speed <= 3.0）**：
```python
# 速度奖励：在目标方向（X 轴正方向）上达到目标速度（默认 1.0 m/s）
actual_speed = dot(com_vel[:2], target_direction[:2]) # 速度在目标方向上的投影
speed_reward = tolerance(actual_speed, bounds=(move_speed, move_speed), margin=move_speed, sigmoid="linear")
```
**奔跑任务（move_speed > 3.0）**：
```python
# 速度奖励：在目标方向上达到目标速度（默认 10.0 m/s）以上
actual_speed = dot(com_vel[:2], target_direction[:2])
speed_reward = tolerance(actual_speed, bounds=(move_speed, inf), margin=move_speed, sigmoid="linear")
```
### 能量奖励（Energy Reward）
```python
energy_reward = exp(-energy_coef * mean(ctrls ^ 2))
```
### 步态奖励（Gait Reward）
```python
# 躯干朝向奖励：躯干正对目标方向
torso_heading_reward = tolerance(dot(torso_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# 头部朝向奖励：头部正对目标方向
head_heading_reward = tolerance(dot(head_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# 盆骨朝向奖励：盆骨正对目标方向
pelvis_yaw_reward = tolerance(dot(pelvis_forward, target_dir), bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# 盆骨水平奖励：盆骨保持水平
pelvis_level_reward = tolerance(pelvis_up, bounds=(0.9, 1.0), margin=0.3, sigmoid="linear")
# 足部高度奖励：足部保持贴近地面
feet_height_reward = tolerance(max_foot_height, bounds=(0.0, 0.3), margin=0.5, sigmoid="quadratic")
# 步态奖励 = 所有朝向和姿态奖励的乘积
gait_reward = torso_heading_reward * head_heading_reward * pelvis_yaw_reward * pelvis_level_reward * feet_height_reward
```
### 总奖励
```python
total_reward = posture_reward * speed_reward * energy_reward * gait_reward
```
---
## 初始状态
- **机器人位置**：躯干初始高度为 1.33 米（标准站立高度的 95%）
- **机器人姿态**：躯干保持直立，四元数设置为 (1.0, 0.0, 0.0, 0.0)
- **关节角度**：在关节限位范围内随机初始化
- 躯干/髋部基础关节：在较小范围内随机化（±15 度）
- 腿部关节：对称初始化，确保左右腿独立随机化，膝盖初始弯曲
- 手臂关节：对称初始化，使用关节限位的中间 80% 范围
- **初始速度**：所有关节速度和线速度初始化为接近零的小随机值（-0.01 到 0.01）
- **初始控制**：所有执行器控制量初始化为接近零的小随机值（-0.02 到 0.02）
## Episode 终止条件
- 机器人状态观测值出现异常数值（NaN 或 Inf）
- 头部高度过低：头部高度低于标准站立高度的 50%（0.7 米）
- 躯干倾斜过大：躯干垂直分量小于 0.2（躯干严重倾斜）
- 速度异常：任何关节速度的绝对值超过 200.0 rad/s 或 m/s
- Episode 最大时长：25 秒
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-humanoid-stand
uv run scripts/view.py --env dm-humanoid-walk
uv run scripts/view.py --env dm-humanoid-run
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-humanoid-stand
uv run scripts/train.py --env dm-humanoid-walk
uv run scripts/train.py --env dm-humanoid-run
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-humanoid-walk
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-humanoid-stand
uv run scripts/play.py --env dm-humanoid-walk
uv run scripts/play.py --env dm-humanoid-run
```
---
## 预期训练结果
### 站立任务 (dm-humanoid-stand)
1. 头部高度保持在 1.3-1.5m 范围
2. 躯干直立角度偏差小于 15 度
3. 能够稳定站立不倒
4. 速度接近零，无明显移动
### 行走任务 (dm-humanoid-walk)
1. 实际行走速度接近 1.0 m/s
2. 步态协调，无明显摔倒
3. 能够持续稳定行走
4. 躯干和头部朝向目标方向
### 奔跑任务 (dm-humanoid-run)
1. 奔跑速度达到 4.0-10.0 m/s
2. 出现飞行相（双脚同时离地）
3. 步态协调稳定
4. 能够保持高速奔跑姿态

138
docs/source/zh_CN/user_guide/demo/pendulum.md Normal file
View File

@@ -0,0 +1,138 @@
# 单摆训练示例
单摆（Pendulum）是经典的单关节摆起并倒立保持任务，目标是用一个电机扭矩把摆甩起并稳定在倒立位置。
```{video} /_static/videos/pendulum.mp4
:poster: _static/images/poster/pendulum.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 任务描述
单摆由一段杆体和一个铰接关节组成，关节由单个电机驱动（gear 可配置）。电机施加的扭矩使摆杆在平面内旋转，实现从任意初始角度的摆起、倒立和保持。关节力矩受限于 ctrlrange，通过控制扭矩大小与方向，策略需要完成能量累积（swing-up）并在倒立位置维持平衡，同时抑制角速度带来的震荡。
---
## 动作空间
| 项目 | 详情 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (1,), float32)` |
| **维度** | 1 |
---
## 观察空间
| 项目 | 详情 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (3,), float32)` |
| **维度** | 3 |
顺序：`cos(theta), sin(theta), 角速度`。
---
## 奖励设计
- 倒立奖励：鼓励角度围绕 π（倒立）
- 能量 shaping：能量接近倒立位置
- 惩罚：`角速度^2`、`ctrl^2`、`(ctrl - prev_ctrl)^2`，抑制震荡与过猛动作
---
## 初始状态
- 角度随机于 `[-pi, pi]`
- 角速度小噪声（如配置）
- 控制历史 `prev_ctrl` 初始化为 0
## Episode 终止条件
- 无跌倒终止；仅 NaN 检查
- Episode 长度由 `max_episode_seconds` 限制
---
### 1. 环境预览
```bash
uv run scripts/view.py --env pendulum
```
### 2. 开始训练
```bash
# 默认参数训练
uv run scripts/train.py --env pendulum
# 自定义并行环境数
uv run scripts/train.py --env pendulum --num-envs 1024
# 开启训练时渲染
uv run scripts/train.py --env pendulum --render
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/pendulum
```
### 4. 测试训练结果
```bash
# 自动寻找最新/最优策略（推荐）
uv run scripts/play.py --env pendulum
# 手动指定策略文件
uv run scripts/play.py --env pendulum --policy runs/pendulum/nn/best_policy.pickle
```
> **提示**：策略默认在 `runs/pendulum/` 下自动发现，可用 `--policy` 手动指定。
---
## 配置参数
### 环境配置（示例）
```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
```
### 训练配置（示例 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)
```
---
## 预期训练结果
1. 摆能主动摆起并停留在倒立附近
2. 倒立处的震荡由角速度与控制变化惩罚抑制

370
docs/source/zh_CN/user_guide/demo/shadow_hand_repose.md Normal file
View File

@@ -0,0 +1,370 @@
# Shadow Hand 立方体重定向
## 概述
本文档详细描述了基于 Shadow Hand 灵巧手的立方体重定向任务环境。该任务是机器人操作领域的经典基准测试，要求机器人在手中重新定向立方体以匹配随机目标姿态。
```{video} /_static/videos/shadow_hand_repose.mp4
:poster: _static/images/poster/shadow_hand_repose.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 环境描述
Shadow Hand 立方体重定向任务基于真实的 Shadow Hand 24 自由度灵巧手构建，旨在训练机器人通过精细的多指协调，将手中的立方体旋转到目标姿态。
### 机器人结构
Shadow Hand 是一个高仿真的 24 自由度灵巧手，由以下主要部分组成：
- **手掌（Palm）**：手的基础结构，固定在基座上
- **5 个手指**：
- **拇指（Thumb）**：5 自由度，包括 CMC 旋转、MCP、IP 关节
- **食指（Index）**：4 自由度，包括 MCP、PIP、DIP 关节
- **中指（Middle）**：4 自由度，包括 MCP、PIP、DIP 关节
- **无名指（Ring）**：4 自由度，包括 MCP、PIP、DIP 关节
- **小指（Little）**：4 自由度，包括 MCP、PIP、DIP 关节
- **执行器配置**：20 个驱动关节（4 个耦合关节由其他关节驱动）
### 手指关节详情
每个手指（除拇指外）包含：
- **MCP 关节**：掌指关节，2 自由度（外展/内收 + 屈曲）
- **PIP 关节**：近端指间关节，1 自由度（屈曲）
- **DIP 关节**：远端指间关节，1 自由度（屈曲）
拇指包含：
- **CMC 关节**：腕掌关节，2 自由度
- **MCP 关节**：掌指关节，1 自由度
- **IP 关节**：指间关节，1 自由度
### 环境物体
- **立方体**：50mm × 50mm × 50mm 的正方体
- 质量：约 0.028 kg
- 摩擦系数：1.2
- 初始位置：手掌中心上方 `(0.33, 0.00, 0.295)` 米
- **目标可视化**：半透明目标姿态指示器（mocap body）
### 任务目标
机器人需要完成以下操作目标：
1. **保持抓握**：维持立方体在手中的稳定抓握
2. **感知目标**：观察目标姿态（可视化指示器）
3. **精细操作**：通过多指协调旋转立方体
4. **姿态对齐**：将立方体姿态旋转至目标姿态（容差 ±0.1 弧度）
---
## 动作空间
动作空间为 `Box(-1, 1, (20,), float32)`，表示施加到 20 个驱动关节的位置控制指令（归一化）。
### 动作处理流程
```python
# 1. 缩放到执行器限制
targets = scale(actions, lower_limits, upper_limits)
# 2. 动作平滑（可选）
if act_moving_average < 1.0:
targets = α * targets + (1-α) * prev_actions
# 3. 裁剪到限制
targets = clip(targets, lower_limits, upper_limits)
# 4. 应用控制
actuator_ctrls = targets
```
### 动作维度详细说明
| 编号 | 手指 | 关节 | 自由度 | 说明 |
| ----- | ------ | ----- | ------ | ------------- |
| 0-4 | 拇指 | J0-J4 | 5 | CMC, MCP, IP |
| 5-8 | 食指 | J0-J3 | 4 | MCP, PIP, DIP |
| 9-12 | 中指 | J0-J3 | 4 | MCP, PIP, DIP |
| 13-16 | 无名指 | J0-J3 | 4 | MCP, PIP, DIP |
| 17-20 | 小指 | J0-J3 | 4 | MCP, PIP, DIP |
---
## 观测空间
观测空间为 `Box(-inf, inf, (157,), float32)`，包含机器人的本体感知信息、立方体状态、目标状态和指尖状态。
### 观测组成部分
观测向量由以下部分组成（按顺序）：
#### 1. 手部关节状态（48 维）
- **关节位置（24 维）**：未缩放的原始关节角度
- **关节速度（24 维）**：缩放 0.2 倍的关节角速度
#### 2. 立方体状态（17 维）
- **位置（3 维）**：世界坐标系中的立方体位置 `(x, y, z)`
- **姿态（4 维）**：四元数 `(x, y, z, w)`
- **线速度（3 维）**：立方体线速度
- **角速度（3 维）**：缩放 0.2 倍的角速度
- **归一化因子**：速度观测乘以 `vel_obs_scale = 0.2`
#### 3. 目标状态（11 维）
- **目标位置（3 维）**：固定在 `(0.33, 0.00, 0.295)`
- **目标姿态（4 维）**：随机采样的目标四元数
- **相对旋转（4 维）**：立方体到目标的相对四元数
#### 4. 指尖状态（65 维）
5 个指尖的状态，每个指尖 13 维：
- **位置（3 维）**：指尖在笛卡尔空间的位置
- **姿态（4 维）**：指尖四元数
- **速度（6 维）**：线速度和角速度
**指尖链接名称**：
- `rh_ffdistal`：食指指尖
- `rh_mfdistal`：中指指尖
- `rh_rfdistal`：无名指指尖
- `rh_lfdistal`：小指指尖
- `rh_thdistal`：拇指指尖
#### 5. 动作历史（20 维）
- 上一步的动作值，用于策略的时间上下文
### 观测详细说明
| 编号 | 观测内容 | 维度 | 范围 | 单位 |
| ------- | ---------------------------- | ---- | ------------ | ------ |
| 0-23 | 手部关节位置（未归一化） | 24 | 关节限制范围 | rad |
| 24-47 | 手部关节速度（× 0.2） | 24 | ±π/2 | rad/s |
| 48-50 | 立方体位置 | 3 | 实数 | m |
| 51-54 | 立方体姿态（四元数 w,x,y,z） | 4 | 单位范数 | 无量纲 |
| 55-57 | 立方体线速度 | 3 | 实数 | m/s |
| 58-60 | 立方体角速度（× 0.2） | 3 | 实数 | rad/s |
| 61-63 | 目标位置 | 3 | 固定值 | m |
| 64-67 | 目标姿态（四元数 w,x,y,z） | 4 | 单位范数 | 无量纲 |
| 68-71 | 相对旋转（四元数 w,x,y,z） | 4 | 单位范数 | 无量纲 |
| 72-136 | 指尖状态（5×13） | 65 | - | - |
| 137-156 | 上一步动作 | 20 | [-1, 1] | 无量纲 |
---
## 奖励函数
奖励函数采用复合设计，包含多个奖励和惩罚项。
### 主要奖励项
1. **旋转对齐奖励**（核心目标）
```
rot_reward = rot_reward_scale / (|rot_dist| + rot_eps)
```
- **缩放系数**：`1.0`
- **Epsilon**：`0.1`
- **旋转距离计算**：使用四元数旋转距离公式
- **激励**：随着立方体姿态接近目标，奖励呈反比增长
2. **位置距离惩罚**
```
dist_reward = dist_reward_scale × goal_dist
```
- **缩放系数**：`-10.0`
- **距离计算**：立方体到目标位置的欧氏距离
- **激励**：阻止立方体掉落，保持在目标位置附近
3. **动作正则化惩罚**
```
action_penalty = action_penalty_scale × ||actions||²
```
- **缩放系数**：`-0.0002`
- **目的**：鼓励平滑、节能的运动
### 条件奖励
4. **成功奖励**
```
if |rot_dist| ≤ success_tolerance:
reward += reach_goal_bonus
```
- **奖励值**：`2.0`
- **容差**：`0.1` 弧度（约 5.7°）
- **目的**：达成目标对齐的稀疏奖励
5. **掉落惩罚**
```
if goal_dist ≥ fall_dist:
reward += fall_penalty
terminated = True
```
- **惩罚值**：`0.0`（仅终止，无额外惩罚）
- **距离阈值**：`0.24`
- **目的**：立方体掉落时终止回合
---
## 初始状态
### 手部初始化
**位置初始化：**
手掌固定在世界坐标系中，位置由模型文件确定。
**关节角度初始化：**
- 使用模型默认关节位置
- 添加均匀随机噪声：`[-0.2, 0.2]` 弧度
- 作用范围：所有 24 个手部自由度
**速度初始化：**
所有关节速度初始化为零。
### 立方体初始化
**位置初始化：**
- 固定位置：`(0.33, 0.00, 0.295)` 手掌中心上方）
- 添加均匀随机噪声：`[-0.01, 0.01]` ±1cm）
**姿态初始化：**
- 使用 Shoemake 方法生成均匀分布的随机四元数
- 确保在 SO(3) 空间上的均匀采样
**速度初始化：**
所有线速度和角速度初始化为零。
### 目标初始化
**位置初始化：**
- 固定位置：`(0.33, 0.00, 0.295)`（与立方体初始位置相同）
**姿态初始化：**
- 使用 Shoemake 方法生成均匀分布的随机目标四元数
- 每次重置时重新采样
---
## 终止条件
回合在以下情况下终止：
1. **掉落终止**：立方体距离目标位置 ≥ `fall_dist`（0.24m）
2. **超时终止**：达到 `max_episode_steps`（默认 1000 步）
3. **NaN 保护**：检测到旋转距离或位置距离为 NaN
### 成功保持机制
环境使用连续成功计数器：
- 当满足旋转容差时，计数器递增
- 当达到 `max_consecutive_successes`（50）时，触发成功终止并重置目标
- 旋转容差：`0.1` 弧度
---
## 使用方法
### 训练
```bash
uv run scripts/train.py --env shadow-hand-repose
```
### 策略评估
```bash
uv run scripts/play.py --env shadow-hand-repose
```
### 环境可视化
```bash
uv run scripts/view.py --env shadow-hand-repose
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/shadow-hand-repose
```
---
## 配置参数
### 环境参数
| 参数名 | 默认值 | 说明 |
| --------------------- | ------ | ------------------ |
| `max_episode_seconds` | 10.0 | 最大回合时长（秒） |
| `ctrl_dt` | 0.01 | 控制时间步（秒） |
| `max_episode_steps` | 1000 | 最大回合步数 |
| `num_hand_dofs` | 24 | 手部总自由度 |
| `num_actuators` | 20 | 驱动关节数量 |
### 奖励参数
| 参数名 | 默认值 | 说明 |
| ---------------------- | ------- | ---------------- |
| `dist_reward_scale` | -10.0 | 位置距离奖励 |
| `rot_reward_scale` | 1.0 | 旋转对齐奖励 |
| `rot_eps` | 0.1 | 旋转奖励 epsilon |
| `action_penalty_scale` | -0.0002 | 动作正则化 |
| `success_tolerance` | 0.1 | 成功容差（弧度） |
| `reach_goal_bonus` | 2.0 | 成功奖励 |
| `fall_dist` | 0.24 | 掉落距离阈值 |
| `fall_penalty` | 0.0 | 掉落惩罚 |
### 重置噪声参数
| 参数名 | 默认值 | 说明 |
| ---------------------- | ------ | ----------------------- |
| `reset_position_noise` | 0.01 | 立方体位置噪声（米） |
| `reset_dof_pos_noise` | 0.2 | 关节位置噪声（弧度） |
| `reset_dof_vel_noise` | 0.0 | 关节速度噪声（弧度/秒） |
### 观测缩放参数
| 参数名 | 默认值 | 说明 |
| --------------- | ------ | ---------------- |
| `vel_obs_scale` | 0.2 | 速度观测缩放因子 |
---
## 参考资料
该环境基于以下经典工作实现：
- **OpenAI Dactyl** (2018)：首个成功的手内操作 sim-to-real 迁移
- **Isaac Gym** (2021)：高性能 GPU 加速的物理仿真
- **Isaac Lab** (2023)：模块化机器人学习框架

189
docs/source/zh_CN/user_guide/getting_started/container_deployment.md Normal file
View File

@@ -0,0 +1,189 @@
# 容器部署
本文档介绍如何使用 Docker 容器化部署 MotrixLab，以简化环境配置并实现快速部署。
## 前置要求
- **Docker** 和 **Docker Compose**: [安装文档](https://docs.docker.com/engine/install/)
- **NVIDIA Container Toolkit**: [安装文档](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html#installation)
- **NVIDIA GPU**: 支持 CUDA 12.8 的显卡
## 快速开始
### 1. 克隆项目仓库
```bash
git clone https://github.com/Motphys/MotrixLab.git
cd MotrixLab/docker
```
### 2. 使用 Docker Compose 启动
我们提供了完整的 `docker-compose.yml` 配置文件，支持一键启动训练和 TensorBoard 可视化服务。
```bash
# 启动训练和 TensorBoard 服务
docker compose up -d
```
这将启动以下服务：
- **motrixlab-training**: 训练容器，执行强化学习训练任务
- **motrixlab-tensorboard**: TensorBoard 可视化服务，通过浏览器访问 http://localhost:6006
### 3. 配置训练参数
您可以通过环境变量自定义训练配置：
```bash
# 设置训练后端（jax 或 torch）
export MOTRIX_TRAIN_BACKEND=jax
# 设置并行环境数量
export MOTRIX_NUM_ENVS=2048
# 设置训练环境名称
export MOTRIX_ENV=cartpole
# 启动服务
docker compose up -d
```
| 环境变量 | 默认值 | 说明 |
| :--------------------- | :--------- | :------------------------- |
| `MOTRIX_TRAIN_BACKEND` | `jax` | 训练后端：`jax` 或 `torch` |
| `MOTRIX_NUM_ENVS` | `2048` | 并行环境数量 |
| `MOTRIX_ENV` | `cartpole` | 训练环境名称 |
### 4. 查看训练进度
训练日志会自动保存到 Docker Volume `motrixlab-data` 中。您可以通过以下方式查看：
```bash
# 查看训练容器日志
docker logs -f motrixlab-training
# 访问 TensorBoard
# 浏览器打开: http://localhost:6006
```
### 5. 停止服务
```bash
# 停止所有服务
docker compose down
# 停止服务并删除数据卷
docker compose down -v
```
## 高级用法
### 构建 Docker 镜像
如果您需要自定义镜像，可以从源代码构建：
```bash
# 在项目根目录执行
cd docker
docker build -t motphys/motrixlab:latest .
```
### 运行单个容器
如果您只想运行训练容器而不使用 Docker Compose：
```bash
docker run --gpus all \
-v $(pwd)/runs:/root/motrixlab/runs \
motphys/motrixlab:latest \
scripts/train.py --train-backend jax --num-envs 2048 --env cartpole
```
### 持久化训练结果
默认配置使用 Docker Volume `motrixlab-data` 保存训练结果。您可以将其挂载到主机目录：
```bash
# 修改 docker-compose.yml 中的 volumes 配置
volumes:
- ./runs:/root/motrixlab/runs
```
## 镜像说明
我们的 Docker 镜像基于 {bdg-primary-line}`NVIDIA CUDA 12.8.1` 运行时环境，预装了以下组件：
- **UV 包管理器**: 快速、可靠的依赖管理
- **SKRL**: 支持 JAX 和 PyTorch 后端的强化学习库
- **TensorBoard**: 训练过程可视化工具
- **MotrixSim**: 高性能物理仿真引擎
- **MotrixLab**: 完整的强化学习训练框架
镜像层构建过程：
1. **基础环境**: NVIDIA CUDA 12.8.1 Runtime + Ubuntu 24.04
2. **系统依赖**: 安装 UV 包管理器和必要的系统工具
3. **Python 依赖**: 使用 UV 缓存机制快速安装 Python 包
4. **项目代码**: 复制 MotrixLab 源代码并完成依赖安装
## 故障排查
### GPU 不可用
如果容器无法访问 GPU：
```bash
# 检查 NVIDIA Docker Runtime
docker run --rm --gpus all nvidia/cuda:12.8.1-base-ubuntu24.04 nvidia-smi
# 确认 NVIDIA Container Toolkit 已正确安装
which nvidia-container-cli
```
### 存储空间不足
清理 Docker 缓存和未使用的镜像：
```bash
# 清理构建缓存
docker builder prune
# 删除未使用的镜像
docker image prune -a
# 清理所有未使用的资源
docker system prune -a
```
## 性能优化
### 使用 UV 缓存加速构建
Dockerfile 使用了 UV 的缓存挂载功能，可以显著加速重建过程：
```bash
# 利用 UV 缓存重新构建
docker build --cache-from motphys/motrixlab:latest -t motphys/motrixlab:latest .
```
### GPU 资源分配
您可以在 `docker-compose.yml` 中指定 GPU 使用数量：
```yaml
deploy:
resources:
reservations:
devices:
- driver: nvidia
device_ids: ["0", "1"] # 使用 GPU 0 和 1
capabilities: [gpu]
```
## 下一步
- 查看 [快速入门教程](hello_motrixlab.md) 了解 MotrixLab 基本用法
- 阅读 [训练示例](../demo/cartpole.md) 学习更多训练任务
- 探索 [基础框架](../tutorial/basic_frame.md) 深入理解框架架构

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

@@ -5,6 +5,7 @@
:maxdepth: 1 :maxdepth: 1
getting_started/installation getting_started/installation
getting_started/container_deployment
getting_started/hello_motrixlab getting_started/hello_motrixlab
``` ```
@@ -25,16 +26,22 @@ tutorial/training_and_result
:caption: 训练示例 :caption: 训练示例
:maxdepth: 1 :maxdepth: 1
demo/acrobot
demo/cartpole demo/cartpole
demo/pendulum
demo/bounce_ball demo/bounce_ball
demo/dm_walker demo/dm_walker
demo/dm_cheetah demo/dm_cheetah
demo/dm_hopper demo/dm_hopper
demo/dm_reacher demo/dm_reacher
demo/dm_finger
demo/dm_humanoid
demo/bring_ball
demo/locomotion_unitree_go1 demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c demo/anymal_c
demo/franka_lift_cube demo/franka_lift_cube
demo/franka_open_cabinet demo/franka_open_cabinet
demo/shadow_hand_repose
``` ```

4
motrix_envs/pyproject.toml
View File

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

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

@@ -13,4 +13,16 @@
# limitations under the License. # limitations under the License.
# ============================================================================== # ==============================================================================
from . import bounce_ball, cartpole, cheetah, hopper, reacher, walker # noqa: F401 import to register envs from . import ( # noqa: F401 import to register envs
acrobot,
bounce_ball,
cartpole,
cheetah,
finger,
hopper,
humanoid,
manipulator,
pendulum,
reacher,
walker,
)

16
motrix_envs/src/motrix_envs/basic/acrobot/__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 acrobot_np # noqa: F401

49
motrix_envs/src/motrix_envs/basic/acrobot/acrobot.xml Normal file
View File

@@ -0,0 +1,49 @@
<!--
Based on Coulomb's [1] rather than Spong's [2] model.
[1] Coulom, Rémi. Reinforcement learning using neural networks, with applications to motor control.
Diss. Institut National Polytechnique de Grenoble-INPG, 2002.
[2] Spong, Mark W. "The swing up control problem for the acrobot."
IEEE control systems 15, no. 1 (1995): 49-55.
-->
<mujoco model="acrobot">
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<default>
<joint damping="0.05"/>
<geom type="capsule" mass="1"/>
</default>
<option timestep="0.01" integrator="RK4" gravity="0 0 -9.81">
<flag contact="disable" energy="enable"/>
</option>
<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 -1"/>
<site name="target" type="sphere" pos="0 0 4" size="0.2" material="target" group="3"/>
<camera name="fixed" pos="0 -6 2" zaxis="0 -1 0"/>
<camera name="lookat" mode="targetbodycom" target="upper_arm" pos="0 -2 3"/>
<body name="upper_arm" pos="0 0 2">
<joint name="shoulder" type="hinge" axis="0 1 0"/>
<geom name="upper_arm_decoration" material="decoration" type="cylinder" fromto="0 -.06 0 0 .06 0" size="0.051" mass="0"/>
<geom name="upper_arm" fromto="0 0 0 0 0 1" size="0.05" material="self"/>
<body name="lower_arm" pos="0 0 1">
<joint name="elbow" type="hinge" axis="0 1 0"/>
<geom name="lower_arm" fromto="0 0 0 0 0 1" size="0.049" material="self"/>
<site name="tip" pos="0 0 1" size="0.01"/>
</body>
</body>
</worldbody>
<actuator>
<motor name="elbow" joint="elbow" gear="30" ctrllimited="true" ctrlrange="-1 1"/>
</actuator>
</mujoco>

152
motrix_envs/src/motrix_envs/basic/acrobot/acrobot_np.py Normal file
View File

@@ -0,0 +1,152 @@
# 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 import reward
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import AcrobotEnvCfg
@registry.env("acrobot", "np")
class AcrobotEnv(NpEnv):
_cfg: AcrobotEnvCfg
def __init__(self, cfg: AcrobotEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._action_space = gym.spaces.Box(-1.0, 1.0, (1,), dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (6,), dtype=np.float32)
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._tip = self._model.get_site("tip")
self._target = self._model.get_site("target")
self._upper_arm = self._model.get_link("upper_arm")
self._lower_arm = self._model.get_link("lower_arm")
self._target_radius = 0.2
self._step_count = np.zeros(self._num_envs, dtype=np.int32)
self._max_steps = int(cfg.max_episode_seconds / 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
shoulder_angle = dof_pos[:, 0]
elbow_angle = dof_pos[:, 1]
upper_arm_horizontal = np.cos(shoulder_angle)
upper_arm_vertical = np.sin(shoulder_angle)
total_angle = shoulder_angle + elbow_angle
lower_arm_horizontal = np.cos(total_angle)
lower_arm_vertical = np.sin(total_angle)
dof_vel = data.dof_vel
obs = np.concatenate(
[
upper_arm_horizontal.reshape(-1, 1),
lower_arm_horizontal.reshape(-1, 1),
upper_arm_vertical.reshape(-1, 1),
lower_arm_vertical.reshape(-1, 1),
dof_vel,
],
axis=-1,
)
return obs
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
tip_pos = self._tip.get_pose(data)
target_pos = self._target.get_pose(data)
dist_to_target = np.linalg.norm(tip_pos[:, :3] - target_pos[:, :3], axis=-1)
base_rwd = reward.tolerance(
dist_to_target,
bounds=(0, self._target_radius),
margin=0,
value_at_margin=0.0,
sigmoid="linear",
)
in_target = dist_to_target < self._target_radius
continuous_reward = 0.1 * in_target
distance_reward = 0.3 * (1.0 - np.clip(dist_to_target / 2.0, 0, 1.0))
dof_vel = data.dof_vel
vel_magnitude = np.mean(np.abs(dof_vel), axis=-1)
velocity_penalty = 0.01 * np.maximum(0, vel_magnitude - 2.0)
rwd = base_rwd + continuous_reward + distance_reward - velocity_penalty
self._step_count += 1
terminated = np.zeros((self._num_envs,), dtype=bool)
terminated = np.logical_or(self._step_count >= self._max_steps, 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]
shoulder_angle = np.random.uniform(-np.pi, np.pi, size=num_reset).astype(np.float32)
elbow_angle = np.random.uniform(-np.pi, np.pi, size=num_reset).astype(np.float32)
dof_pos = np.stack([shoulder_angle, elbow_angle], axis=-1)
dof_vel = np.zeros((*data.shape, self._num_dof_vel), dtype=np.float32)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
obs = self._get_obs(data)
return obs, {}
def _reset_done_envs(self):
"""
Reset the environments that are done
"""
super()._reset_done_envs()
done = self._state.done
if np.any(done):
self._step_count[done] = 0

33
motrix_envs/src/motrix_envs/basic/acrobot/cfg.py Normal file
View File

@@ -0,0 +1,33 @@
# 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__) + "/acrobot.xml"
@registry.envcfg("acrobot")
@dataclass
class AcrobotEnvCfg(EnvCfg):
model_file: str = model_file
reset_noise_scale: float = 0.1
max_episode_seconds: float = 10.0
render_spacing: float = 2.0
sim_dt: float = 0.01
ctrl_dt: float = 0.02

30
motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_ctrl.xml
View File

@@ -31,8 +31,34 @@
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/> <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom type="plane" size="0 0 .01" material="motphys-ground"/> <geom type="plane" size="0 0 .01" material="motphys-ground"/>
<!-- Target height visualization using mocap body (can be moved at runtime) -->
<body name="target_height_marker" mocap="true" pos="0.58856 0 0.5">
<geom name="target_height_visual"
type="cylinder"
size="0.1 0.002"
rgba="0 1 0 0.5"
contype="0"
conaffinity="0"/>
<geom name="target_center_dot"
type="cylinder"
size="0.01 0.002"
rgba="0 0.6 0 0.9"
contype="0"
conaffinity="0"/>
</body>
<!-- Paddle home position marker -->
<body name="paddle_home_marker" mocap="true" pos="0.5857 -0.0082 0.2">
<geom name="paddle_home_visual"
type="box"
size="0.1 0.1 0.003"
rgba="1 1 0.6 0.4"
contype="0"
conaffinity="0"/>
</body>
<geom type="mesh" rgba="0.25098 0.25098 0.25098 1" mesh="base_link" class="visual"/> <geom type="mesh" rgba="0.25098 0.25098 0.25098 1" mesh="base_link" class="visual"/>
<body name="Link1" pos="0 0 0.1"> <body name="Link1" pos="0 0 0.33">
<inertial pos="0.00022014 -7.0626e-06 -0.10379" quat="0.998051 -0.0622621 0.00360518 0.00215072" mass="0.98482" diaginertia="0.00658905 0.00502396 0.00428069"/> <inertial pos="0.00022014 -7.0626e-06 -0.10379" quat="0.998051 -0.0622621 0.00360518 0.00215072" mass="0.98482" diaginertia="0.00658905 0.00502396 0.00428069"/>
<joint name="Joint1" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-300 300"/> <joint name="Joint1" pos="0 0 0" axis="0 0 1" range="-2.96706 2.96706" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link1" class="visual"/> <geom type="mesh" rgba="1 0.69804 0 1" mesh="Link1" class="visual"/>
@@ -68,7 +94,7 @@
<body name="ball_link" pos="0 0 0.07"> <body name="ball_link" pos="0 0 0.07">
<freejoint/> <freejoint/>
<inertial pos="0 0 0" mass="0.0027" diaginertia="1 1 1"/> <inertial pos="0 0 0" mass="0.0027" diaginertia="1 1 1"/>
<geom size="0.019" contype="1" rgba="0 1 0 1" solref="1 0"/> <geom size="0.019" contype="1" rgba="1 0 0 1" solref="1 0"/>
</body> </body>
</worldbody> </worldbody>

534
motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py
View File

@@ -30,36 +30,71 @@ class BounceBallEnv(NpEnv):
def __init__(self, cfg: BounceBallEnvCfg, num_envs: int = 1): def __init__(self, cfg: BounceBallEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs) super().__init__(cfg, num_envs=num_envs)
# Action space: 6D normalized paddle velocity (dx, dy, dz, dr_x, dr_y, dr_z) # Action space: 6D joint position control
self._action_space = gym.spaces.Box(-1.0, 1.0, (6,), dtype=np.float32) self._action_space = gym.spaces.Box(-1.0, 1.0, (6,), dtype=np.float32)
# Observation space: simplified version using only DOF information # Observation space: joint states + paddle position + target height (29D)
# DOF pos (13) + DOF vel (12) = 25 (this includes ball state implicitly) self._observation_space = gym.spaces.Box(-np.inf, np.inf, (29,), dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (25,), dtype=np.float32)
self._num_dof_pos = self._model.num_dof_pos self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel self._num_dof_vel = self._model.num_dof_vel
# Initial arm joint positions (degrees converted to radians) # Initial arm joint positions
self._init_arm_qpos = np.array(self._cfg.arm_init_qpos, dtype=np.float32) * np.pi / 180.0 self._init_arm_qpos = np.array(self._cfg.arm_init_qpos, dtype=np.float32) * np.pi / 180.0
self._init_dof_vel = np.zeros(self._model.num_dof_vel, dtype=np.float32) self._init_dof_vel = np.zeros(self._model.num_dof_vel, dtype=np.float32)
# Initialize full DOF positions (6 arm joints + 7 for ball free joint) # Full DOF positions (6 arm joints + 7 ball free joint)
self._init_dof_pos = np.zeros(self._model.num_dof_pos, dtype=np.float32) self._init_dof_pos = np.zeros(self._model.num_dof_pos, dtype=np.float32)
self._init_dof_pos[:6] = self._init_arm_qpos self._init_dof_pos[:6] = self._init_arm_qpos
# Get body and geom IDs # Body and geom references
self._paddle_geom_id = self._model.geom_names.index("blocker") self._paddle_geom = self._model.get_geom("blocker")
self._ball_body_id = self._model.body_names.index("ball_link") self._ball_body_id = self._model.body_names.index("ball_link")
# Action scaling parameters # Mocap bodies for visual markers
self._target_marker_body = self._model.get_body("target_height_marker")
assert self._target_marker_body.is_mocap, "target_height_marker must be a mocap body"
self._paddle_home_marker_body = self._model.get_body("paddle_home_marker")
assert self._paddle_home_marker_body.is_mocap, "paddle_home_marker must be a mocap body"
# Action scaling
self._action_scale = np.array(self._cfg.action_scale, dtype=np.float32) self._action_scale = np.array(self._cfg.action_scale, dtype=np.float32)
self._action_bias = np.array(self._cfg.action_bias, dtype=np.float32) self._action_bias = np.array(self._cfg.action_bias, dtype=np.float32)
# Track ball initial position for reset # Ball initial conditions
self._ball_init_pos = np.array(self._cfg.ball_init_pos, dtype=np.float32) self._ball_init_pos = np.array(self._cfg.ball_init_pos, dtype=np.float32)
self._ball_init_vel = np.array(self._cfg.ball_init_vel, dtype=np.float32) self._ball_init_vel = np.array(self._cfg.ball_init_vel, dtype=np.float32)
# Constants for marker poses
self._ball_radius = 0.019 # Ball radius in meters
# Target marker base pose: [x, y, z_placeholder, qx, qy, qz, qw]
self._target_marker_base_pose = np.array(
[
self._cfg.target_ball_x,
self._cfg.target_ball_y,
0.0, # z will be set per environment
0.0,
0.0,
0.0,
1.0, # identity quaternion
],
dtype=np.float32,
)
# Paddle home marker pose: [x, y, z, qx, qy, qz, qw]
self._paddle_home_marker_pose = np.array(
[
self._cfg.target_ball_x,
self._cfg.target_ball_y,
self._cfg.paddle_home_position_z,
0.0,
0.0,
0.0,
1.0, # identity quaternion
],
dtype=np.float32,
)
@property @property
def observation_space(self): def observation_space(self):
return self._observation_space return self._observation_space
@@ -69,118 +104,361 @@ class BounceBallEnv(NpEnv):
return self._action_space return self._action_space
def _denormalize_action(self, action: np.ndarray) -> np.ndarray: def _denormalize_action(self, action: np.ndarray) -> np.ndarray:
"""Denormalize action to get actual paddle velocity changes""" """Denormalize action from [-1, 1] to joint position changes"""
return self._action_scale * action + self._action_bias return self._action_scale * action + self._action_bias
def _compute_observation(self, data: mtx.SceneData) -> np.ndarray: def _compute_observation(self, data: mtx.SceneData, target_heights: np.ndarray) -> np.ndarray:
"""Compute 25-dimensional observation vector from DOF states""" """Compute observation: joint states + paddle position + target height (29D)"""
# Use DOF positions and velocities directly
dof_pos = data.dof_pos dof_pos = data.dof_pos
dof_vel = data.dof_vel dof_vel = data.dof_vel
# Concatenate DOF positions (13) and velocities (12) # Get paddle position
obs = np.concatenate([dof_pos, dof_vel], axis=-1) paddle_pose = self._paddle_geom.get_pose(data)
paddle_xyz = paddle_pose[:, :3]
# Concatenate: DOF pos (13) + DOF vel (12) + paddle xyz (3) + target height (1)
obs = np.concatenate([dof_pos, dof_vel, paddle_xyz, target_heights[:, np.newaxis]], axis=-1)
return obs.astype(np.float32) return obs.astype(np.float32)
def _compute_reward( def _compute_reward(
self, obs: np.ndarray, data: mtx.SceneData = None, consecutive_bounces: np.ndarray = None self,
) -> np.ndarray: obs: np.ndarray,
"""Compute reward based on ball height, position, and controlled upward velocity""" data: mtx.SceneData = None,
# Extract ball position and velocity from DOF consecutive_bounces: np.ndarray = None,
ball_x = obs[:, 6] # Ball x position bounce_detected: np.ndarray = None,
ball_z = obs[:, 8] # Ball z position target_heights: np.ndarray = None,
current_actions: np.ndarray = None,
last_actions: np.ndarray = None,
) -> tuple:
"""
Compute reward based on ball position, velocity, and paddle alignment.
ball_vz = obs[:, 13 + 8] # Ball z velocity (13 pos + 8 vel) The reward function uses a composite design with multiple reward and penalty terms
to guide the robot to learn a stable ball bouncing strategy.
Returns:
tuple: (total_reward, reward_details) where reward_details contains
individual reward components for analysis.
"""
# Extract ball state
ball_x = obs[:, 6]
ball_y = obs[:, 7]
ball_z = obs[:, 8]
ball_vz = obs[:, 13 + 8]
# Extract paddle position
paddle_xy = obs[:, 25:27]
paddle_z = obs[:, 27]
# Target positions # Target positions
target_ball_x = 0.58856 # Target x position target_ball_x = self._cfg.target_ball_x
target_height = self._cfg.target_ball_height target_ball_y = self._cfg.target_ball_y
tolerance = self._cfg.height_tolerance target_height = target_heights
# 1. Position control reward - MOST IMPORTANT for keeping ball centered # Physics constant
# Strong reward for ball being at the right x position (paddle center) g = self._cfg.gravity
# ============================================================================
# 1. Horizontal position reward (weighted by vertical distance)
# Core reward ensuring ball stays directly above paddle
# ============================================================================
x_position_error = np.abs(ball_x - target_ball_x) x_position_error = np.abs(ball_x - target_ball_x)
x_position_reward = np.exp(-(x_position_error**2) / (2 * 0.05**2)) # Tight tolerance for x position y_position_error = np.abs(ball_y - target_ball_y)
xy_position_error = np.sqrt(x_position_error**2 + y_position_error**2)
# 2. Height-based reward - less important than position control vertical_dist = np.abs(ball_z - paddle_z)
vertical_weight = np.exp(-vertical_dist / self._cfg.vertical_weight_scale)
weighted_horizontal_scale = self._cfg.weighted_horizontal_base_scale * (
1.0 + self._cfg.weighted_horizontal_weight_factor * vertical_weight
)
weighted_position_reward = np.exp(-(xy_position_error**2) / (2 * weighted_horizontal_scale**2))
# ============================================================================
# 2. Out-of-position penalty
# Strong penalty for severe deviation to prevent ball flying out of control
# ============================================================================
out_of_position_penalty = -2.0 / (
1.0
+ np.exp(-(xy_position_error - self._cfg.out_of_position_threshold) / self._cfg.out_of_position_sharpness)
)
# ============================================================================
# 3. Velocity matching reward
# Based on projectile motion physics, encourages ball trajectory to have
# desired velocity at target height for stable control
# ============================================================================
height_diff = target_height - ball_z
desired_velocity_at_target = self._cfg.desired_velocity_at_target
# Calculate velocity at target height using physics
upward_below_condition = (ball_vz > 0) & (ball_z < target_height)
velocity_squared_at_target_upward = np.where(upward_below_condition, ball_vz**2 - 2 * g * height_diff, 0.0)
velocity_at_target_upward = np.sqrt(np.maximum(0, velocity_squared_at_target_upward))
downward_above_condition = (ball_vz < 0) & (ball_z > target_height)
velocity_squared_at_target_downward = np.where(
downward_above_condition, ball_vz**2 + 2 * g * np.abs(height_diff), 0.0
)
velocity_at_target_downward = -np.sqrt(np.maximum(0, velocity_squared_at_target_downward))
near_target_condition = np.abs(height_diff) < 0.05
velocity_at_target_near = np.where(near_target_condition, ball_vz, 0.0)
# Smooth combination
upward_motion = 1.0 / (1.0 + np.exp(-ball_vz / 0.2))
below_target = 1.0 / (1.0 + np.exp(-height_diff / 0.02))
downward_motion = 1.0 - upward_motion
above_target = 1.0 - below_target
at_target_weight = np.exp(-(height_diff**2) / (2 * 0.01**2))
velocity_at_target = (
velocity_at_target_upward * upward_motion * below_target
+ velocity_at_target_downward * downward_motion * above_target
+ velocity_at_target_near * at_target_weight
)
velocity_error = np.abs(velocity_at_target - desired_velocity_at_target)
velocity_matching_reward = np.exp(-(velocity_error**2) / (2 * self._cfg.velocity_error_sigma**2))
# ============================================================================
# 4. Height reward
# Directly encourages ball to approach target height, core task objective
# ============================================================================
height_error = np.abs(ball_z - target_height) height_error = np.abs(ball_z - target_height)
height_reward = np.exp(-(height_error**2) / (2 * tolerance**2)) height_reward = np.exp(-(height_error**2) / (2 * self._cfg.height_error_sigma**2))
# 3. Controlled upward velocity reward - only when ball is in good position # Height progress bonus
# Only reward upward velocity when ball is well-positioned horizontally height_progress_bonus = (
well_positioned = x_position_error < 0.02 # Ball must be very close to target x np.maximum(0, ball_z - self._cfg.height_progress_threshold) * self._cfg.height_progress_scale
controlled_upward_reward = np.where(
well_positioned & (ball_vz > 0.1) & (ball_vz < 1.5), # Reasonable upward velocity
np.clip(ball_vz * 1.5, 0.0, 1.5), # Reduced scale
0.0,
) )
# 4. Strong penalty for being out of position horizontally # ============================================================================
out_of_position_penalty = np.where( # 5. Controlled upward velocity reward
x_position_error > 0.1, # Only rewards upward velocity when ball position is good, avoiding random hitting
-2.0, # Heavy penalty for being far from center # Guides strategy to learn precise hitting force
0.0, # ============================================================================
positioning_quality = np.exp(-(xy_position_error**2) / (2 * self._cfg.positioning_quality_sigma**2))
ideal_launch_velocity = np.sqrt(2 * g * np.maximum(0, height_diff))
ideal_launch_velocity = np.clip(
ideal_launch_velocity, self._cfg.ideal_velocity_min, self._cfg.ideal_velocity_max
) )
# 5. Velocity penalties - discourage excessive speeds upward_velocity_quality = np.exp(
excessive_upward_penalty = np.where(ball_vz > 2.0, -1.0, 0.0) -((ball_vz - ideal_launch_velocity) ** 2) / (2 * self._cfg.upward_velocity_sigma**2)
downward_velocity_penalty = np.where(ball_vz < -0.5, -np.clip(-ball_vz * 0.3, 0.0, 0.5), 0.0)
# 6. Position-based penalties (reduced)
overshoot_penalty = np.where(ball_z > target_height + tolerance, -0.3, 0.0)
undershoot_penalty = np.where(ball_z < 0.1, -0.5, 0.0)
# 7. Consecutive bounces reward - only when position is good
good_position_for_bounce = x_position_error < 0.05 # Reasonable position for bouncing
consecutive_bounces_reward = np.where(
good_position_for_bounce & (consecutive_bounces > 0),
np.log(consecutive_bounces + 1) * 0.3, # Reduced scale
0.0,
) )
# Bonus for high bounce counts (only when well-positioned) upward_mask = 1.0 / (1.0 + np.exp(-ball_vz / self._cfg.upward_mask_scale))
high_bounce_bonus = np.where(
good_position_for_bounce & (consecutive_bounces >= 3), controlled_upward_reward = (
consecutive_bounces * 0.1, # Reduced bonus positioning_quality
0.0, * upward_velocity_quality
* upward_mask
* np.clip(ball_vz * 1.0, 0.0, self._cfg.controlled_upward_clip_max)
) )
# Combine all rewards with corrected priorities # ============================================================================
# 6. Velocity penalties
# Prevents ball velocity from being too fast or falling freely
# Ensures ball motion stays within controllable range
# ============================================================================
excessive_upward_penalty = -1.0 / (
1.0 + np.exp(-(ball_vz - self._cfg.excessive_upward_threshold) / self._cfg.excessive_upward_sharpness)
)
downward_penalty_magnitude = -ball_vz * np.clip(
-ball_vz * self._cfg.downward_velocity_scale, 0.0, self._cfg.downward_velocity_clip_max
)
downward_penalty_trigger = 1.0 / (1.0 + np.exp((ball_vz - self._cfg.downward_velocity_threshold) / 0.2))
downward_velocity_penalty = downward_penalty_magnitude * downward_penalty_trigger
# ============================================================================
# 7. Consecutive bounces reward
# Encourages multiple consecutive successful bounces for stable long-term control
# Uses logarithmic function to avoid infinite reward growth
# ============================================================================
bounce_positioning_quality = np.exp(-(xy_position_error**2) / (2 * self._cfg.bounce_positioning_sigma**2))
bounce_log_reward = np.log(consecutive_bounces.astype(np.float32) + 1.0) * self._cfg.bounce_log_scale
bounce_activation = (consecutive_bounces > 0).astype(np.float32) * bounce_positioning_quality
consecutive_bounces_reward = bounce_log_reward * bounce_activation
# High bounce count bonus
high_bounce_activation = 1.0 / (
1.0
+ np.exp(
-(consecutive_bounces.astype(np.float32) - self._cfg.high_bounce_threshold)
/ self._cfg.high_bounce_sharpness
)
)
high_bounce_bonus = (
consecutive_bounces.astype(np.float32)
* self._cfg.high_bounce_scale
* bounce_positioning_quality
* high_bounce_activation
)
# ============================================================================
# 8. Paddle-ball horizontal alignment
# Encourages paddle to actively move directly below ball
# Extra reward at bounce moment to reinforce correct hitting behavior
# ============================================================================
if bounce_detected is None:
bounce_detected = np.zeros(ball_x.shape[0], dtype=bool)
ball_xy = np.stack([ball_x, ball_y], axis=1)
paddle_ball_xy_error = np.linalg.norm(ball_xy - paddle_xy, axis=1)
vertical_proximity_weight = np.exp(-vertical_dist / self._cfg.vertical_proximity_scale)
paddle_alignment_quality = np.exp(-(paddle_ball_xy_error**2) / (2 * self._cfg.paddle_alignment_sigma**2)) * (
1.0 + self._cfg.paddle_alignment_weight_factor * vertical_proximity_weight
)
bounce_boost = bounce_detected.astype(np.float32) * self._cfg.bounce_boost_factor + 1.0
paddle_center_reward = paddle_alignment_quality * bounce_boost * self._cfg.paddle_center_scale
# ============================================================================
# 9. Paddle home position reward
# Encourages paddle to return to home position when ball is far away
# Makes paddle motion more energy-efficient and natural
# ============================================================================
paddle_home_z = self._cfg.paddle_home_position_z
paddle_height_deviation = np.abs(paddle_z - paddle_home_z)
# Distance-based dynamic factor
distance_factor = 1.0 + self._cfg.distance_factor_scale / (
1.0 + np.exp(-(vertical_dist - self._cfg.distance_factor_threshold) / self._cfg.distance_factor_sharpness)
)
home_position_reward = (
np.exp(-(paddle_height_deviation**2) / (2 * self._cfg.home_position_sigma**2)) * distance_factor
)
# Height violation penalty
max_deviation = self._cfg.max_paddle_height_deviation
height_violation = np.maximum(0, paddle_height_deviation - max_deviation)
height_violation_penalty = -height_violation * self._cfg.height_violation_scale
# ============================================================================
# 10. Action and velocity penalties
# Penalizes drastic action changes and excessive joint velocities
# Encourages smooth and energy-efficient control
# ============================================================================
num_envs = obs.shape[0]
if current_actions is None:
current_actions = np.zeros((num_envs, 6), dtype=np.float32)
if last_actions is None:
last_actions = np.zeros((num_envs, 6), dtype=np.float32)
action_diff = current_actions - last_actions
action_penalty = np.sum(np.square(action_diff), axis=-1)
joint_vel = data.dof_vel[:, :6]
joint_vel_penalty = np.sum(np.square(joint_vel), axis=-1)
# ============================================================================
# Total reward
# ============================================================================
action_penalty_rate = self._cfg.action_penalty_rate
joint_vel_penalty_rate = self._cfg.joint_vel_penalty_rate
total_reward = ( total_reward = (
x_position_reward * 2.0 # X position control (200%) - MOST IMPORTANT weighted_position_reward * self._cfg.weighted_position_weight
+ controlled_upward_reward * 1.0 # Controlled upward velocity (100%) + velocity_matching_reward * self._cfg.velocity_matching_weight
+ height_reward * 0.3 # Height accuracy (30%) - less important + height_reward * self._cfg.height_reward_weight
+ consecutive_bounces_reward * 1.0 # Consecutive bounces (100%) - reduced + height_progress_bonus * self._cfg.height_progress_weight
+ high_bounce_bonus * 0.3 # High bounce bonus (30%) - reduced + controlled_upward_reward * self._cfg.controlled_upward_weight
+ out_of_position_penalty * 1.0 # Out of position penalty (100%) + consecutive_bounces_reward * self._cfg.consecutive_bounces_weight
+ excessive_upward_penalty * 1.0 # Excessive upward penalty (100%) + high_bounce_bonus * self._cfg.high_bounce_weight
+ downward_velocity_penalty * 1.0 # Downward penalty (100%) + paddle_center_reward * self._cfg.paddle_center_weight
+ overshoot_penalty # Height overshoot penalty + home_position_reward * self._cfg.home_position_weight
+ undershoot_penalty # Height undershoot penalty + out_of_position_penalty * self._cfg.out_of_position_weight
+ excessive_upward_penalty * self._cfg.excessive_upward_weight
+ downward_velocity_penalty * self._cfg.downward_velocity_weight
+ height_violation_penalty * self._cfg.height_violation_weight
- action_penalty_rate * action_penalty
- joint_vel_penalty_rate * joint_vel_penalty
) )
return total_reward # ============================================================================
# Reward details for debugging
# ============================================================================
reward_details = {
"x_position_error": x_position_error,
"y_position_error": y_position_error,
"xy_position_error": xy_position_error,
"ball_z": ball_z,
"paddle_z": paddle_z,
"vertical_dist": vertical_dist,
"vertical_weight": vertical_weight,
"ball_vz": ball_vz,
"height_diff": height_diff,
"height_error": height_error,
"velocity_at_target": velocity_at_target,
"desired_velocity_at_target": np.full_like(ball_vz, desired_velocity_at_target),
"velocity_error": velocity_error,
"ideal_launch_velocity": ideal_launch_velocity,
"positioning_quality": positioning_quality,
"upward_velocity_quality": upward_velocity_quality,
"bounce_positioning_quality": bounce_positioning_quality,
"paddle_ball_xy_error": paddle_ball_xy_error,
"vertical_proximity_weight": vertical_proximity_weight,
"bounce_detected": bounce_detected.astype(np.float32),
"consecutive_bounces": consecutive_bounces.astype(np.float32),
"action_penalty": action_penalty,
"joint_vel_penalty": joint_vel_penalty,
"distance_factor": distance_factor,
"weighted_position_reward": weighted_position_reward * 2.0,
"velocity_matching_reward": velocity_matching_reward * 2.0,
"height_reward": height_reward * 4.5,
"height_progress_bonus": height_progress_bonus * 1.0,
"controlled_upward_reward": controlled_upward_reward * 1.5,
"consecutive_bounces_reward": consecutive_bounces_reward * 0.8,
"high_bounce_bonus": high_bounce_bonus * 0.3,
"paddle_center_reward": paddle_center_reward * 0.6,
"home_position_reward": home_position_reward * 1.5,
"out_of_position_penalty": out_of_position_penalty * 1.0,
"excessive_upward_penalty": excessive_upward_penalty * 1.0,
"downward_velocity_penalty": downward_velocity_penalty * 1.0,
"height_violation_penalty": height_violation_penalty * 1.0,
"action_penalty_weighted": -action_penalty_rate * action_penalty,
"joint_vel_penalty_weighted": -joint_vel_penalty_rate * joint_vel_penalty,
"paddle_height_deviation": paddle_height_deviation,
"total_reward": total_reward,
}
def _compute_terminated(self, obs: np.ndarray) -> np.ndarray: return total_reward, reward_details
def _compute_terminated(self, obs: np.ndarray, target_heights: np.ndarray) -> np.ndarray:
"""Check if episode should terminate based on DOF states""" """Check if episode should terminate based on DOF states"""
# Extract ball position from DOF (indices 6-8 for x,y,z) # Extract ball position from DOF (indices 6-8 for x,y,z)
ball_x = obs[:, 6] # Ball x position ball_x = obs[:, 6] # Ball x position
ball_y = obs[:, 7] # Ball y position
ball_z = obs[:, 8] # Ball z position ball_z = obs[:, 8] # Ball z position
# Target height from config
target_height = self._cfg.target_ball_height
# Terminate if ball falls below ground or goes significantly higher than target # Terminate if ball falls below ground or goes significantly higher than target
terminated = (ball_z < 0.05) | (ball_z > target_height + 1.0) terminated = (ball_z < 0.05) | (ball_z > target_heights + 1.0)
# Also terminate if ball goes too far horizontally # Also terminate if ball goes too far horizontally
terminated |= np.abs(ball_x) > 1.5 terminated |= (np.abs(ball_x) > 1.5) | (np.abs(ball_y) > 1.5)
# Terminate if joint velocity is too high
# Limit: 360 degrees/second = 2*pi rad/s ≈ 6.28 rad/s
joint_vel = obs[:, 13:19] # Joint velocities (indices 13-18 for 6 arm joints)
max_joint_vel = 2.0 * np.pi # 360 degrees/second in radians
terminated |= np.abs(joint_vel).max(axis=-1) > max_joint_vel
return terminated return terminated
def apply_action(self, actions: np.ndarray, state: NpEnvState) -> NpEnvState: def apply_action(self, actions: np.ndarray, state: NpEnvState) -> NpEnvState:
"""Apply action to control paddle position""" """Apply action to control paddle position"""
# Store last actions for penalty calculation
state.info["last_actions"] = state.info.get("current_actions", np.zeros_like(actions))
state.info["current_actions"] = actions
# Get current joint positions # Get current joint positions
current_joint_pos = state.data.dof_pos[:, :6] # First 6 DOFs are arm joints current_joint_pos = state.data.dof_pos[:, :6] # First 6 DOFs are arm joints
@@ -198,12 +476,15 @@ class BounceBallEnv(NpEnv):
"""Update state with new observations, rewards, and termination flags""" """Update state with new observations, rewards, and termination flags"""
data = state.data data = state.data
# Compute observation # Get bounce tracking and target heights from info
obs = self._compute_observation(data)
# Get bounce tracking from info
consecutive_bounces = state.info.get("consecutive_bounces", np.zeros(data.shape[0], dtype=np.int32)) consecutive_bounces = state.info.get("consecutive_bounces", np.zeros(data.shape[0], dtype=np.int32))
ball_was_upward = state.info.get("ball_was_upward", np.zeros(data.shape[0], dtype=bool)) ball_was_upward = state.info.get("ball_was_upward", np.zeros(data.shape[0], dtype=bool))
# Use mean of target_height_range as fallback
default_height = np.mean(self._cfg.target_height_range)
target_heights = state.info.get("target_heights", np.full(data.shape[0], default_height, dtype=np.float32))
# Compute observation with target heights
obs = self._compute_observation(data, target_heights)
# Detect bounces and update consecutive bounce count # Detect bounces and update consecutive bounce count
current_ball_z = obs[:, 8] # Ball z position current_ball_z = obs[:, 8] # Ball z position
@@ -239,19 +520,42 @@ class BounceBallEnv(NpEnv):
normalized_obs = obs normalized_obs = obs
# Compute reward and termination # Compute reward and termination
reward = self._compute_reward(obs, data, consecutive_bounces) reward, reward_details = self._compute_reward(
terminated = self._compute_terminated(obs) obs,
data,
consecutive_bounces,
bounce_detected=bounce_detected,
target_heights=target_heights,
current_actions=state.info.get("current_actions"),
last_actions=state.info.get("last_actions"),
)
terminated = self._compute_terminated(obs, target_heights=target_heights)
state.obs = normalized_obs state.obs = normalized_obs
state.reward = reward state.reward = reward
state.terminated = terminated state.terminated = terminated
# Store reward details for debugging
if self._cfg.store_reward_details:
state.info["Reward"] = reward_details
state.info["target_heights"] = target_heights # Ensure target_heights persists across steps
return state return state
def reset(self, data: mtx.SceneData) -> tuple: def reset(self, data: mtx.SceneData) -> tuple:
"""Reset environment to initial state""" """Reset environment to initial state with randomized target heights"""
cfg: BounceBallEnvCfg = self._cfg cfg: BounceBallEnvCfg = self._cfg
num_reset = data.shape[0] num_reset = data.shape[0]
# Randomize target heights for the environments being reset
if cfg.randomize_target_height:
min_height, max_height = cfg.target_height_range
new_target_heights = np.random.uniform(min_height, max_height, num_reset).astype(np.float32)
else:
# Use mean of target_height_range when not randomizing
default_height = np.mean(cfg.target_height_range)
new_target_heights = np.full(num_reset, default_height, dtype=np.float32)
# Add noise to initial arm joint positions only (not ball) # Add noise to initial arm joint positions only (not ball)
arm_noise_pos = np.random.uniform( arm_noise_pos = np.random.uniform(
-cfg.reset_noise_scale, -cfg.reset_noise_scale,
@@ -267,42 +571,56 @@ class BounceBallEnv(NpEnv):
# Reset simulation first to get proper DOF structure # Reset simulation first to get proper DOF structure
data.reset(self._model) data.reset(self._model)
# Get current DOF positions and modify only the arm joints # Get current DOF positions
current_dof_pos = data.dof_pos current_dof_pos = data.dof_pos
current_dof_vel = data.dof_vel
# === Modify all DOF positions ===
# Set arm joint positions (first 6 DOFs) # Set arm joint positions (first 6 DOFs)
current_dof_pos[:, :6] = np.tile(self._init_arm_qpos, (num_reset, 1)) + arm_noise_pos current_dof_pos[:, :6] = np.tile(self._init_arm_qpos, (num_reset, 1)) + arm_noise_pos
current_dof_vel[:, :6] = noise_vel[:, :6]
# Set the quaternion part properly (DOFs 9-12 are quaternion w,x,y,z for freejoint)
# The ball has a freejoint which uses quaternion representation
for i in range(num_reset):
# Set quaternion for ball (indices 9-12: w, x, y, z)
current_dof_pos[i, 9:13] = [1.0, 0.0, 0.0, 0.0] # Identity quaternion
data.set_dof_pos(current_dof_pos, self._model)
data.set_dof_vel(current_dof_vel)
# Set ball position in DOF (indices 6-8 for x, y, z positions) # Set ball position in DOF (indices 6-8 for x, y, z positions)
for i in range(num_reset): ball_noise_pos = np.random.uniform(-0.01, 0.01, (num_reset, 3))
ball_noise_pos = np.random.uniform(-0.01, 0.01, 3) ball_pos = self._ball_init_pos + ball_noise_pos
ball_pos = self._ball_init_pos + ball_noise_pos current_dof_pos[:, 6:9] = ball_pos
# Set ball position in DOF coordinates (indices 6-8)
current_dof_pos[i, 6:9] = ball_pos
# Final update to set both ball position and quaternion # Apply all DOF position changes
data.set_dof_pos(current_dof_pos, self._model) data.set_dof_pos(current_dof_pos, self._model)
# Get current DOF velocities
current_dof_vel = data.dof_vel
# === Modify all DOF velocities ===
# Set arm joint velocities (first 6 DOFs)
current_dof_vel[:, :6] = noise_vel[:, :6]
# Apply ball linear velocity in DOF
data.set_dof_vel(current_dof_vel)
# Update target height marker position (thin cylinder disc)
# Marker center aligns with ball top: marker_z = target_height + ball_radius
target_marker_poses = np.tile(self._target_marker_base_pose, (num_reset, 1))
target_marker_poses[:, 2] = new_target_heights + self._ball_radius # Set z position
self._target_marker_body.mocap.set_pose(data, target_marker_poses)
# Update paddle home marker position
paddle_home_marker_poses = np.tile(self._paddle_home_marker_pose, (num_reset, 1))
# Set paddle home marker mocap body pose
self._paddle_home_marker_body.mocap.set_pose(data, paddle_home_marker_poses)
# Initialize info dict with bounce tracking variables # Initialize info dict with bounce tracking variables
info = { info = {
"consecutive_bounces": np.zeros(num_reset, dtype=np.int32), "consecutive_bounces": np.zeros(num_reset, dtype=np.int32),
"ball_was_upward": np.zeros(num_reset, dtype=bool), "ball_was_upward": np.zeros(num_reset, dtype=bool),
"max_consecutive_bounces": 0, "max_consecutive_bounces": 0,
"target_heights": new_target_heights.copy(), # Return target heights for this reset batch
"current_actions": np.zeros((num_reset, 6), dtype=np.float32),
"last_actions": np.zeros((num_reset, 6), dtype=np.float32),
} }
# Compute initial observation # Compute initial observation with target heights
obs = self._compute_observation(data) obs = self._compute_observation(data, new_target_heights)
normalized_obs = obs # No normalization for now normalized_obs = obs # No normalization for now
return normalized_obs, info return normalized_obs, info

97
motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py
View File

@@ -42,19 +42,110 @@ class BounceBallEnvCfg(EnvCfg):
ball_init_vel: list = None ball_init_vel: list = None
arm_init_qpos: list = None arm_init_qpos: list = None
# Target positions
target_ball_x: float = 0.58856 # Target x position (m)
target_ball_y: float = 0.0 # Target y position (m)
# Target height for bouncing (configurable parameter) # Target height for bouncing (configurable parameter)
target_ball_height: float = 0.8 # Default target height in meters target_height_range: tuple = (0.53, 0.83) # Range for random target height in meters
randomize_target_height: bool = True # Whether to randomize target height on reset
height_tolerance: float = 0.1 # Tolerance for reward calculation height_tolerance: float = 0.1 # Tolerance for reward calculation
# Paddle behavior constraints
paddle_home_position_z: float = 0.28 # Home position height for paddle (m)
max_paddle_height_deviation: float = 0.1 # Max deviation from home position (m)
encourage_return_home: bool = True # Encourage paddle to return to home position
encourage_impact_velocity: bool = True # Encourage high upward velocity at impact
# Physics constants
gravity: float = 9.81 # Gravity acceleration (m/s^2)
# Reward function parameters
# Position reward
vertical_weight_scale: float = 0.15 # Scale for vertical distance weight
weighted_horizontal_base_scale: float = 0.1 # Base scale for horizontal position
weighted_horizontal_weight_factor: float = 3.0 # Weight factor for vertical proximity
# Out of position penalty
out_of_position_threshold: float = 0.05 # Threshold distance (m)
out_of_position_sharpness: float = 0.03 # Sigmoid sharpness
# Velocity matching
desired_velocity_at_target: float = 0.5 # Target velocity at target height (m/s)
velocity_error_sigma: float = 0.8 # Sigma for velocity error Gaussian
# Height reward
height_error_sigma: float = 0.15 # Sigma for height error Gaussian
height_progress_scale: float = 2.0 # Scale for height progress bonus
height_progress_threshold: float = 0.2 # Minimum height for progress bonus (m)
# Controlled upward velocity
positioning_quality_sigma: float = 0.02 # Sigma for positioning quality
ideal_velocity_min: float = 0.5 # Minimum ideal launch velocity (m/s)
ideal_velocity_max: float = 3.0 # Maximum ideal launch velocity (m/s)
upward_velocity_sigma: float = 0.5 # Sigma for upward velocity quality
upward_mask_scale: float = 0.1 # Scale for upward mask sigmoid
controlled_upward_clip_max: float = 1.5 # Max clip value for controlled upward
# Velocity penalties
excessive_upward_threshold: float = 3.5 # Threshold for excessive upward velocity (m/s)
excessive_upward_sharpness: float = 0.3 # Sigmoid sharpness
downward_velocity_threshold: float = -0.2 # Threshold for downward penalty (m/s)
downward_velocity_scale: float = 0.3 # Scale for downward penalty magnitude
downward_velocity_clip_max: float = 0.5 # Max clip for downward penalty
# Bounce rewards
bounce_positioning_sigma: float = 0.05 # Sigma for bounce positioning quality
bounce_log_scale: float = 0.5 # Scale for logarithmic bounce reward
high_bounce_threshold: float = 2.0 # Threshold for high bounce bonus
high_bounce_sharpness: float = 0.5 # Sigmoid sharpness for high bounce
high_bounce_scale: float = 0.15 # Scale for high bounce bonus
# Paddle alignment
paddle_alignment_sigma: float = 0.03 # Sigma for paddle-ball alignment
vertical_proximity_scale: float = 0.1 # Scale for vertical proximity weight
paddle_alignment_weight_factor: float = 2.0 # Weight factor for vertical proximity
bounce_boost_factor: float = 2.0 # Boost factor when bounce detected
paddle_center_scale: float = 0.3 # Overall scale for paddle center reward
# Home position reward
home_position_sigma: float = 0.05 # Sigma for home position deviation
distance_factor_threshold: float = 0.15 # Threshold for distance factor (m)
distance_factor_sharpness: float = 0.03 # Sigmoid sharpness
distance_factor_scale: float = 0.5 # Scale for distance factor
height_violation_scale: float = 20.0 # Scale for height violation penalty
# Action and velocity penalties
action_penalty_rate: float = 1e-4 # Penalty rate for action changes
joint_vel_penalty_rate: float = 1e-4 # Penalty rate for joint velocities
# Reward weights
weighted_position_weight: float = 2.0
velocity_matching_weight: float = 2.0
height_reward_weight: float = 4.5
height_progress_weight: float = 1.0
controlled_upward_weight: float = 1.5
consecutive_bounces_weight: float = 0.8
high_bounce_weight: float = 0.3
paddle_center_weight: float = 0.6
home_position_weight: float = 1.5
out_of_position_weight: float = 1.0
excessive_upward_weight: float = 1.0
downward_velocity_weight: float = 1.0
height_violation_weight: float = 1.0
# Action scaling parameters # Action scaling parameters
action_scale: list = None action_scale: list = None
action_bias: list = None action_bias: list = None
# Debug options
store_reward_details: bool = False # Whether to store detailed reward breakdown in state.info, it's very slow
def __post_init__(self): def __post_init__(self):
if self.ball_init_pos is None: if self.ball_init_pos is None:
self.ball_init_pos = [0.58856, 0, 1.27796] # Slightly above paddle (paddle z=0.2803) self.ball_init_pos = [0.58856, 0, 0.68]
if self.ball_init_vel is None: if self.ball_init_vel is None:
self.ball_init_vel = [0.0, 0.0, 0.0] self.ball_init_vel = [0.0, 0.0, -0.2]
if self.arm_init_qpos is None: if self.arm_init_qpos is None:
self.arm_init_qpos = [0, 40, 110, 0, -60, 0] self.arm_init_qpos = [0, 40, 110, 0, -60, 0]

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

@@ -30,4 +30,4 @@ class CheetahEnvCfg(EnvCfg):
render_spacing: float = 2.0 render_spacing: float = 2.0
sim_dt: float = 0.01 sim_dt: float = 0.01
ctrl_dt: float = 0.025 ctrl_dt: float = 0.025
run_speed: float = 30.0 run_speed: float = 10.0

16
motrix_envs/src/motrix_envs/basic/finger/__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 finger_np # noqa: F401

102
motrix_envs/src/motrix_envs/basic/finger/cfg.py Normal file
View File

@@ -0,0 +1,102 @@
# 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__) + "/finger.xml"
spin_model_file = os.path.dirname(__file__) + "/finger_spin.xml"
turn_easy_model_file = os.path.dirname(__file__) + "/finger_turn_easy.xml"
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
sim_dt: float = 0.01
ctrl_dt: float = 0.02
# Task setup
task: str = "spin" # "spin" | "turn"
target_radius: float = 0.07
# Reward thresholds (match dm_control defaults)
spin_velocity_threshold: float = 15.0
# Reward mode
# - "sparse": match dm_control (1 if hinge_velocity <= -threshold else 0)
# - "shaped": dense reward to make training easier
reward_mode: str = "sparse"
shaped_reward_beta: float = 1.0
# Extra shaping for Spin tasks (helps reduce "no contact" failures)
spin_touch_bonus_scale: float = 0.0
spin_touch_bonus_tanh_scale: float = 50.0
spin_approach_reward_scale: float = 0.0
spin_approach_sigma: float = 0.15
# Turn shaping: reward falls linearly to 0 at margin = scale * target_radius
turn_reward_margin_scale: float = 4.0
turn_reward_min_margin: float = 0.0
turn_shaped_reward_beta: float = 1.0
# Turn shaping mode:
# - "linear": clip(1 - max(dist,0)/margin, 0..1)
# - "exp": exp(-max(dist,0)/sigma)
turn_reward_shape: str = "linear"
turn_reward_sigma_scale: float = 1.0
turn_reward_sigma_min: float = 0.05
# Extra shaping for Turn tasks (to reduce jitter and help contact)
turn_touch_bonus_scale: float = 0.05
turn_touch_bonus_tanh_scale: float = 50.0
# Encourage approaching the spinner (helps avoid "no contact" deadlock)
turn_approach_reward_scale: float = 0.3
turn_approach_sigma: float = 0.15
turn_action_l2_penalty_scale: float = 0.002
turn_action_delta_l2_penalty_scale: float = 0.01
# Reset sampling
reset_collision_free_attempts: int = 200
@registry.envcfg("dm-finger-spin")
@dataclass
class FingerSpinCfg(FingerBaseCfg):
model_file: str = spin_model_file
task: str = "spin"
reward_mode: str = "shaped"
spin_approach_reward_scale: float = 0.15
spin_touch_bonus_scale: float = 0.03
@registry.envcfg("dm-finger-turn-easy")
@dataclass
class FingerTurnEasyCfg(FingerBaseCfg):
model_file: str = turn_easy_model_file
task: str = "turn"
target_radius: float = 0.07
reward_mode: str = "shaped"
turn_reward_shape: str = "exp"
@registry.envcfg("dm-finger-turn-hard")
@dataclass
class FingerTurnHardCfg(FingerTurnEasyCfg):
model_file: str = turn_hard_model_file
target_radius: float = 0.03
reward_mode: str = "shaped"
turn_reward_shape: str = "exp"

76
motrix_envs/src/motrix_envs/basic/finger/finger.xml Normal file
View File

@@ -0,0 +1,76 @@
<mujoco model="finger">
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<!-- Match cartpole's skybox + floor look -->
<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>
<option timestep="0.01" cone="elliptic" iterations="200">
<flag gravity="disable"/>
</option>
<default>
<geom solimp="0 0.9 0.01" solref=".02 1"/>
<joint type="hinge" axis="0 -1 0"/>
<motor ctrllimited="true" ctrlrange="-1 1"/>
<default class="finger">
<joint damping="2.5" limited="true"/>
<site type="sphere" size=".03" material="site" group="3"/>
</default>
</default>
<worldbody>
<light name="light" directional="true" diffuse=".6 .6 .6" pos="0 0 2" specular=".3 .3 .3"/>
<geom name="ground" type="plane" pos="0 0 0" size="0 0 0.01" material="motphys-ground"/>
<camera name="cam0" pos="0 -1 .8" xyaxes="1 0 0 0 1 2"/>
<camera name="cam1" pos="0 -1 .4" xyaxes="1 0 0 0 0 1"/>
<body name="proximal" pos="-.2 0 .4" childclass="finger">
<geom name="proximal_decoration" type="cylinder" fromto="0 -.033 0 0 .033 0" size=".034" material="decoration"/>
<joint name="proximal" range="-110 110" ref="-90"/>
<geom name="proximal" type="capsule" material="self" size=".03" fromto="0 0 0 0 0 -.17"/>
<body name="distal" pos="0 0 -.18" childclass="finger">
<joint name="distal" range="-110 110"/>
<geom name="distal" type="capsule" size=".028" material="self" fromto="0 0 0 0 0 -.16" contype="0" conaffinity="0"/>
<geom name="fingertip" type="capsule" size=".03" material="effector" fromto="0 0 -.13 0 0 -.161"/>
<site name="touchtop" pos=".01 0 -.17"/>
<site name="touchbottom" pos="-.01 0 -.17"/>
</body>
</body>
<body name="spinner" pos=".2 0 .4">
<joint name="hinge" frictionloss=".1" damping=".5"/>
<geom name="cap1" type="capsule" size=".04 .09" material="self" pos=".02 0 0"/>
<geom name="cap2" type="capsule" size=".04 .09" material="self" pos="-.02 0 0"/>
<site name="tip" type="sphere" size=".02" pos="0 0 .13" material="target"/>
<geom name="spinner_decoration" type="cylinder" fromto="0 -.045 0 0 .045 0" size=".02" material="decoration"/>
</body>
<site name="target" type="sphere" size=".03" pos="0 0 .4" material="target"/>
</worldbody>
<actuator>
<motor name="proximal" joint="proximal" gear="30"/>
<motor name="distal" joint="distal" gear="15"/>
</actuator>
<!-- All finger observations are functions of sensors. This is useful for finite-differencing. -->
<sensor>
<jointpos name="proximal" joint="proximal"/>
<jointpos name="distal" joint="distal"/>
<jointvel name="proximal_velocity" joint="proximal"/>
<jointvel name="distal_velocity" joint="distal"/>
<jointvel name="hinge_velocity" joint="hinge"/>
<framepos name="tip" objtype="site" objname="tip"/>
<framepos name="target" objtype="site" objname="target"/>
<framepos name="spinner" objtype="xbody" objname="spinner"/>
<touch name="touchtop" site="touchtop"/>
<touch name="touchbottom" site="touchbottom"/>
<framepos name="touchtop_pos" objtype="site" objname="touchtop"/>
<framepos name="touchbottom_pos" objtype="site" objname="touchbottom"/>
</sensor>
</mujoco>

431
motrix_envs/src/motrix_envs/basic/finger/finger_np.py Normal file
View File

@@ -0,0 +1,431 @@
# 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.finger.cfg import FingerBaseCfg
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
class FingerEnv(NpEnv):
_cfg: FingerBaseCfg
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: FingerBaseCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._cfg = cfg
self._spinner = self._model.get_link("spinner")
self._tip_site = self._model.get_site("tip")
self._target_site = self._model.get_site("target")
self._cap1 = self._model.get_geom("cap1")
self._touchtop_site = self._model.get_site("touchtop")
self._touchbottom_site = self._model.get_site("touchbottom")
self._joint_limit_low, self._joint_limit_high = _sanitize_joint_limits(*self._model.joint_limits)
# Cache joint dof indices
self._prox_qpos_i = self._joint_pos_index("proximal")
self._dist_qpos_i = self._joint_pos_index("distal")
self._hinge_qpos_i = self._joint_pos_index("hinge")
self._hinge_qvel_i = self._joint_vel_index("hinge")
self._prox_qvel_i = self._joint_vel_index("proximal")
self._dist_qvel_i = self._joint_vel_index("distal")
self._target_xyz = np.zeros((num_envs, 3), dtype=np.float32)
self._target_radius = float(cfg.target_radius)
self._spin_vel_threshold = float(cfg.spin_velocity_threshold)
self._init_obs_space()
self._init_action_space()
def _joint_pos_index(self, joint_name: str) -> int:
joint_index = self._model.get_joint_index(joint_name)
return int(self._model.joint_dof_pos_indices[joint_index])
def _joint_vel_index(self, joint_name: str) -> int:
joint_index = self._model.get_joint_index(joint_name)
return int(self._model.joint_dof_vel_indices[joint_index])
def _init_obs_space(self):
raise NotImplementedError
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:
# Keep track of actions for reward shaping (e.g., smoothness penalties)
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 _touch(self, data: mtx.SceneData) -> np.ndarray:
top = np.asarray(self._model.get_sensor_value("touchtop", data)).reshape(data.shape[0], -1)[:, 0]
bottom = np.asarray(self._model.get_sensor_value("touchbottom", data)).reshape(data.shape[0], -1)[:, 0]
return np.log1p(np.stack([top, bottom], axis=-1))
def _tip_position_xz(self, data: mtx.SceneData) -> np.ndarray:
tip_xyz = self._tip_site.get_position(data)
spinner_xyz = self._spinner.get_position(data)
return (tip_xyz - spinner_xyz)[:, [0, 2]]
def _target_position_xz(self, data: mtx.SceneData) -> np.ndarray:
spinner_xyz = self._spinner.get_position(data)
return (self._target_xyz - spinner_xyz)[:, [0, 2]]
def _dist_to_target(self, data: mtx.SceneData) -> np.ndarray:
# Signed distance to the target surface. Negative means inside.
tip_xyz = self._tip_site.get_position(data)
dist = np.linalg.norm((self._target_xyz - tip_xyz)[:, [0, 2]], axis=-1)
return dist - self._target_radius
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
raise NotImplementedError
def update_state(self, state: NpEnvState) -> NpEnvState:
raise NotImplementedError
def _maybe_init_target_freejoint(self, dof_pos: np.ndarray) -> slice | None:
# Optional freejoint-backed target visualization body (7 qpos: xyz + quat).
try:
self._model.get_geom("target_geom")
target_free_pos = slice(self._model.num_dof_pos - 7, self._model.num_dof_pos)
dof_pos[:, target_free_pos] = np.array([0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 1.0], dtype=np.float32)
return target_free_pos
except Exception:
return None
def _reset_collision_free_joint_angles(
self, data: mtx.SceneData, dof_pos: np.ndarray, target_free_pos: slice | None
):
# Randomize joint angles with a collision-free rejection sampler (dm_control-style).
# The MotrixSim joint_limits are per-joint (not per-DOF), so we explicitly fill each DOF.
num = int(data.shape[0])
max_attempts = int(getattr(self._cfg, "reset_collision_free_attempts", 200))
pending = np.ones((num,), dtype=bool)
for _ in range(max_attempts):
if not pending.any():
break
num_pending = int(pending.sum())
for joint_name in ("proximal", "distal"):
j = self._model.get_joint_index(joint_name)
dof_i = self._joint_pos_index(joint_name)
low = float(self._joint_limit_low[j])
high = float(self._joint_limit_high[j])
dof_pos[pending, dof_i] = np.random.uniform(low=low, high=high, size=(num_pending,)).astype(np.float32)
# Sample hinge position (unlimited in model)
dof_pos[pending, self._hinge_qpos_i] = np.random.uniform(
low=-np.pi, high=np.pi, size=(num_pending,)
).astype(np.float32)
if target_free_pos is not None:
dof_pos[:, target_free_pos] = np.array([0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 1.0], dtype=np.float32)
data.set_dof_pos(dof_pos, self._model)
data.set_dof_vel(np.zeros((num, self._model.num_dof_vel), dtype=np.float32))
self._model.forward_kinematic(data)
pending = self._model.get_contact_query(data).num_contacts > 0
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
raise NotImplementedError
@registry.env("dm-finger-spin", "np")
class FingerSpinEnv(FingerEnv):
def _init_obs_space(self):
# Match dm_control's observation dict, but flatten into a vector.
# Spin: position(4) + velocity(3) + touch(2) = 9
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (9,), dtype=np.float32)
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
qvel = data.dof_vel
position = np.concatenate(
[
qpos[:, [self._prox_qpos_i, self._dist_qpos_i]],
self._tip_position_xz(data),
],
axis=-1,
)
velocity = qvel[:, [self._prox_qvel_i, self._dist_qvel_i, self._hinge_qvel_i]]
touch = self._touch(data)
return np.concatenate([position, velocity, touch], axis=-1).astype(np.float32)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
terminated = np.isnan(obs).any(axis=-1)
hinge_velocity = data.dof_vel[:, self._hinge_qvel_i]
spin_sparse = (hinge_velocity <= -self._spin_vel_threshold).astype(np.float32)
if self._cfg.reward_mode == "shaped":
# Dense reward to help PPO learn: encourage fast negative hinge velocity.
# Range: [0, 1] roughly, with 1 around reaching the threshold.
spin = np.clip((-hinge_velocity) / self._spin_vel_threshold, 0.0, 1.0).astype(np.float32)
if self._cfg.shaped_reward_beta != 1.0:
spin = np.power(spin, self._cfg.shaped_reward_beta, dtype=np.float32)
else:
spin = spin_sparse
touch_raw = np.zeros((data.shape[0],), dtype=np.float32)
touch_bonus = np.zeros((data.shape[0],), dtype=np.float32)
approach_dist = np.zeros((data.shape[0],), dtype=np.float32)
approach_reward = np.zeros((data.shape[0],), dtype=np.float32)
if self._cfg.reward_mode == "shaped":
if float(getattr(self._cfg, "spin_touch_bonus_scale", 0.0)) > 0.0:
top = np.asarray(self._model.get_sensor_value("touchtop", data)).reshape(data.shape[0], -1)[:, 0]
bottom = np.asarray(self._model.get_sensor_value("touchbottom", data)).reshape(data.shape[0], -1)[:, 0]
touch_raw = (top + bottom).astype(np.float32)
touch_bonus = (
float(self._cfg.spin_touch_bonus_scale)
* np.tanh(touch_raw / float(max(self._cfg.spin_touch_bonus_tanh_scale, 1e-6)))
).astype(np.float32)
if float(getattr(self._cfg, "spin_approach_reward_scale", 0.0)) > 0.0:
spinner_xyz = self._spinner.get_position(data)
top_xyz = self._touchtop_site.get_position(data)
bottom_xyz = self._touchbottom_site.get_position(data)
top_dist = np.linalg.norm((top_xyz - spinner_xyz)[:, [0, 2]], axis=-1)
bottom_dist = np.linalg.norm((bottom_xyz - spinner_xyz)[:, [0, 2]], axis=-1)
approach_dist = np.minimum(top_dist, bottom_dist).astype(np.float32)
sigma = float(max(self._cfg.spin_approach_sigma, 1e-6))
approach_reward = (float(self._cfg.spin_approach_reward_scale) * np.exp(-approach_dist / sigma)).astype(
np.float32
)
spin = np.clip(spin + touch_bonus + approach_reward, 0.0, 1.0).astype(np.float32)
rwd = spin
state.info["Reward"] = {
"hinge_velocity": hinge_velocity.copy(),
"spin": spin.copy(),
"spin_sparse": spin_sparse.copy(),
"touch_raw": touch_raw.copy(),
"touch_bonus": touch_bonus.copy(),
"approach_dist": approach_dist.copy(),
"approach_reward": approach_reward.copy(),
}
rwd[terminated] = 0.0
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.zeros((num, self._model.num_dof_pos), dtype=np.float32)
target_free_pos = self._maybe_init_target_freejoint(dof_pos)
self._reset_collision_free_joint_angles(data, dof_pos, target_free_pos)
info: dict = {"Reward": {}}
info["actions"] = np.zeros((num, self._model.num_actuators), dtype=np.float32)
info["last_actions"] = np.zeros((num, self._model.num_actuators), dtype=np.float32)
info["Reward"] = {
"hinge_velocity": np.zeros((num,), dtype=np.float32),
"spin": np.zeros((num,), dtype=np.float32),
"spin_sparse": np.zeros((num,), dtype=np.float32),
"touch_raw": np.zeros((num,), dtype=np.float32),
"touch_bonus": np.zeros((num,), dtype=np.float32),
"approach_dist": np.zeros((num,), dtype=np.float32),
"approach_reward": np.zeros((num,), dtype=np.float32),
}
obs = self._get_obs(data)
return obs, info
@registry.env("dm-finger-turn-easy", "np")
@registry.env("dm-finger-turn-hard", "np")
class FingerTurnEnv(FingerEnv):
def _init_obs_space(self):
# Match dm_control's observation dict, but flatten into a vector.
# Turn: position(4) + velocity(3) + touch(2) + target_position(2) + dist_to_target(1) = 12
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (12,), dtype=np.float32)
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
qvel = data.dof_vel
position = np.concatenate(
[
qpos[:, [self._prox_qpos_i, self._dist_qpos_i]],
self._tip_position_xz(data),
],
axis=-1,
)
velocity = qvel[:, [self._prox_qvel_i, self._dist_qvel_i, self._hinge_qvel_i]]
touch = self._touch(data)
target_position = self._target_position_xz(data)
dist_to_target = self._dist_to_target(data).reshape(data.shape[0], 1)
return np.concatenate([position, velocity, touch, target_position, dist_to_target], axis=-1).astype(np.float32)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
terminated = np.isnan(obs).any(axis=-1)
dist_to_target = self._dist_to_target(data)
turn_sparse = (dist_to_target <= 0.0).astype(np.float32)
touch_raw = np.zeros((data.shape[0],), dtype=np.float32)
touch_bonus = np.zeros((data.shape[0],), dtype=np.float32)
approach_dist = np.zeros((data.shape[0],), dtype=np.float32)
approach_reward = np.zeros((data.shape[0],), dtype=np.float32)
action_l2 = np.zeros((data.shape[0],), dtype=np.float32)
action_delta_l2 = np.zeros((data.shape[0],), dtype=np.float32)
if self._cfg.reward_mode == "shaped":
# Encourage approaching the spinner so the agent actually makes contact and can rotate it.
spinner_xyz = self._spinner.get_position(data)
top_xyz = self._touchtop_site.get_position(data)
bottom_xyz = self._touchbottom_site.get_position(data)
top_dist = np.linalg.norm((top_xyz - spinner_xyz)[:, [0, 2]], axis=-1)
bottom_dist = np.linalg.norm((bottom_xyz - spinner_xyz)[:, [0, 2]], axis=-1)
approach_dist = np.minimum(top_dist, bottom_dist).astype(np.float32)
sigma = max(float(self._cfg.turn_approach_sigma), 1e-6)
approach_reward = (self._cfg.turn_approach_reward_scale * np.exp(-approach_dist / sigma)).astype(np.float32)
dist_pos = np.maximum(dist_to_target, 0.0).astype(np.float32)
if getattr(self._cfg, "turn_reward_shape", "linear") == "exp":
sigma = float(
max(self._cfg.turn_reward_sigma_scale * self._target_radius, self._cfg.turn_reward_sigma_min)
)
sigma = max(sigma, 1e-6)
turn = np.exp(-dist_pos / sigma).astype(np.float32)
else:
margin = float(
max(self._cfg.turn_reward_margin_scale * self._target_radius, self._cfg.turn_reward_min_margin)
)
margin = max(margin, 1e-6)
# Dense reward: 1 inside target sphere, decays to 0 at `margin` outside.
turn = np.clip(1.0 - dist_pos / margin, 0.0, 1.0).astype(np.float32)
if self._cfg.turn_shaped_reward_beta != 1.0:
turn = np.power(turn, self._cfg.turn_shaped_reward_beta, dtype=np.float32)
# Encourage making contact (to actually be able to rotate the spinner)
top = np.asarray(self._model.get_sensor_value("touchtop", data)).reshape(data.shape[0], -1)[:, 0]
bottom = np.asarray(self._model.get_sensor_value("touchbottom", data)).reshape(data.shape[0], -1)[:, 0]
touch_raw = (top + bottom).astype(np.float32)
touch_bonus = self._cfg.turn_touch_bonus_scale * np.tanh(touch_raw / self._cfg.turn_touch_bonus_tanh_scale)
# Reduce jitter: penalize large actions and action changes
actions = state.info.get("actions", data.actuator_ctrls).astype(np.float32)
last_actions = state.info.get("last_actions", actions).astype(np.float32)
action_l2 = np.mean(np.square(actions), axis=-1).astype(np.float32)
action_delta_l2 = np.mean(np.square(actions - last_actions), axis=-1).astype(np.float32)
turn = (
turn
+ approach_reward
+ touch_bonus
- self._cfg.turn_action_l2_penalty_scale * action_l2
- self._cfg.turn_action_delta_l2_penalty_scale * action_delta_l2
).astype(np.float32)
turn = np.clip(turn, 0.0, 1.0).astype(np.float32)
else:
turn = turn_sparse
rwd = turn
state.info["Reward"] = {
"dist_to_target": dist_to_target.copy(),
"turn": turn.copy(),
"turn_sparse": turn_sparse.copy(),
"touch_raw": touch_raw.copy(),
"touch_bonus": touch_bonus.copy(),
"approach_dist": approach_dist.copy(),
"approach_reward": approach_reward.copy(),
"action_l2": action_l2.copy(),
"action_delta_l2": action_delta_l2.copy(),
}
state.info["target_info"] = {"positions": self._target_xyz.copy(), "radius": self._target_radius}
rwd[terminated] = 0.0
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.zeros((num, self._model.num_dof_pos), dtype=np.float32)
target_free_pos = self._maybe_init_target_freejoint(dof_pos)
self._reset_collision_free_joint_angles(data, dof_pos, target_free_pos)
hinge_xyz = self._spinner.get_position(data)
# Match dm_control: radius = cap1.geom_size.sum() for capsule (radius + half-length).
radius = float(np.sum(self._cap1.size[:2]))
target_angle = np.random.uniform(-np.pi, np.pi, size=(num,))
target_x = hinge_xyz[:, 0] + radius * np.sin(target_angle)
target_z = hinge_xyz[:, 2] + radius * np.cos(target_angle)
self._target_xyz = np.stack([target_x, hinge_xyz[:, 1], target_z], axis=-1).astype(np.float32)
# Best-effort visualization when num_envs == 1 (site position is model-shared).
if self._num_envs == 1:
try:
self._target_site.local_pos = self._target_xyz[0]
self._target_site.size = np.asarray([self._target_radius], dtype=np.float32)
except Exception:
pass
# If we have a freejoint-backed visual target (geom), set its pose in the state.
if target_free_pos is not None:
dof_pos[:, target_free_pos] = np.concatenate(
[
self._target_xyz.astype(np.float32),
np.tile(np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32), (num, 1)),
],
axis=-1,
)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
info: dict = {"Reward": {}}
info["actions"] = np.zeros((num, self._model.num_actuators), dtype=np.float32)
info["last_actions"] = np.zeros((num, self._model.num_actuators), dtype=np.float32)
info["target_info"] = {"positions": self._target_xyz.copy(), "radius": self._target_radius}
info["Reward"] = {
"dist_to_target": np.zeros((num,), dtype=np.float32),
"turn": np.zeros((num,), dtype=np.float32),
"turn_sparse": np.zeros((num,), dtype=np.float32),
}
obs = self._get_obs(data)
return obs, info

77
motrix_envs/src/motrix_envs/basic/finger/finger_spin.xml Normal file
View File

@@ -0,0 +1,77 @@
<mujoco model="finger">
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<!-- Match cartpole's skybox + floor look -->
<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>
<option timestep="0.01" cone="elliptic" iterations="200">
<flag gravity="disable"/>
</option>
<default>
<geom solimp="0 0.9 0.01" solref=".02 1"/>
<joint type="hinge" axis="0 -1 0"/>
<motor ctrllimited="true" ctrlrange="-1 1"/>
<default class="finger">
<joint damping="2.5" limited="true"/>
<site type="sphere" size=".03" material="site" group="3"/>
</default>
</default>
<worldbody>
<light name="light" directional="true" diffuse=".6 .6 .6" pos="0 0 2" specular=".3 .3 .3"/>
<geom name="ground" type="plane" pos="0 0 0" size="0 0 0.01" material="motphys-ground"/>
<camera name="cam0" pos="0 -1 .8" xyaxes="1 0 0 0 1 2"/>
<camera name="cam1" pos="0 -1 .4" xyaxes="1 0 0 0 0 1"/>
<body name="proximal" pos="-.2 0 .4" childclass="finger">
<geom name="proximal_decoration" type="cylinder" fromto="0 -.033 0 0 .033 0" size=".034" material="decoration"/>
<joint name="proximal" range="-110 110" ref="-90"/>
<geom name="proximal" type="capsule" material="self" size=".03" fromto="0 0 0 0 0 -.17"/>
<body name="distal" pos="0 0 -.18" childclass="finger">
<joint name="distal" range="-110 110"/>
<geom name="distal" type="capsule" size=".028" material="self" fromto="0 0 0 0 0 -.16" contype="0" conaffinity="0"/>
<geom name="fingertip" type="capsule" size=".03" material="effector" fromto="0 0 -.13 0 0 -.161"/>
<site name="touchtop" pos=".01 0 -.17"/>
<site name="touchbottom" pos="-.01 0 -.17"/>
</body>
</body>
<body name="spinner" pos=".2 0 .4">
<!-- dm_control's Spin task reduces hinge damping for easier spinning -->
<joint name="hinge" frictionloss=".1" damping=".03"/>
<geom name="cap1" type="capsule" size=".04 .09" material="self" pos=".02 0 0"/>
<geom name="cap2" type="capsule" size=".04 .09" material="self" pos="-.02 0 0"/>
<site name="tip" type="sphere" size=".02" pos="0 0 .13" material="target"/>
<geom name="spinner_decoration" type="cylinder" fromto="0 -.045 0 0 .045 0" size=".02" material="decoration"/>
</body>
<site name="target" type="sphere" size=".03" pos="0 0 .4" material="target"/>
</worldbody>
<actuator>
<motor name="proximal" joint="proximal" gear="30"/>
<motor name="distal" joint="distal" gear="15"/>
</actuator>
<sensor>
<jointpos name="proximal" joint="proximal"/>
<jointpos name="distal" joint="distal"/>
<jointvel name="proximal_velocity" joint="proximal"/>
<jointvel name="distal_velocity" joint="distal"/>
<jointvel name="hinge_velocity" joint="hinge"/>
<framepos name="tip" objtype="site" objname="tip"/>
<framepos name="target" objtype="site" objname="target"/>
<framepos name="spinner" objtype="xbody" objname="spinner"/>
<touch name="touchtop" site="touchtop"/>
<touch name="touchbottom" site="touchbottom"/>
<framepos name="touchtop_pos" objtype="site" objname="touchtop"/>
<framepos name="touchbottom_pos" objtype="site" objname="touchbottom"/>
</sensor>
</mujoco>

83
motrix_envs/src/motrix_envs/basic/finger/finger_turn_easy.xml Normal file
View File

@@ -0,0 +1,83 @@
<mujoco model="finger">
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<!-- Match cartpole's skybox + floor look -->
<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>
<option timestep="0.01" cone="elliptic" iterations="200">
<flag gravity="disable"/>
</option>
<default>
<geom solimp="0 0.9 0.01" solref=".02 1"/>
<joint type="hinge" axis="0 -1 0"/>
<motor ctrllimited="true" ctrlrange="-1 1"/>
<default class="finger">
<joint damping="2.5" limited="true"/>
<site type="sphere" size=".03" material="site" group="3"/>
</default>
</default>
<worldbody>
<light name="light" directional="true" diffuse=".6 .6 .6" pos="0 0 2" specular=".3 .3 .3"/>
<geom name="ground" type="plane" pos="0 0 0" size="0 0 0.01" material="motphys-ground"/>
<camera name="cam0" pos="0 -1 .8" xyaxes="1 0 0 0 1 2"/>
<camera name="cam1" pos="0 -1 .4" xyaxes="1 0 0 0 0 1"/>
<body name="proximal" pos="-.2 0 .4" childclass="finger">
<geom name="proximal_decoration" type="cylinder" fromto="0 -.033 0 0 .033 0" size=".034" material="decoration"/>
<joint name="proximal" range="-110 110" ref="-90"/>
<geom name="proximal" type="capsule" material="self" size=".03" fromto="0 0 0 0 0 -.17"/>
<body name="distal" pos="0 0 -.18" childclass="finger">
<joint name="distal" range="-110 110"/>
<geom name="distal" type="capsule" size=".028" material="self" fromto="0 0 0 0 0 -.16" contype="0" conaffinity="0"/>
<geom name="fingertip" type="capsule" size=".03" material="effector" fromto="0 0 -.13 0 0 -.161"/>
<site name="touchtop" pos=".01 0 -.17"/>
<site name="touchbottom" pos="-.01 0 -.17"/>
</body>
</body>
<body name="spinner" pos=".2 0 .4">
<joint name="hinge" frictionloss=".1" damping=".5"/>
<geom name="cap1" type="capsule" size=".04 .09" material="self" pos=".02 0 0"/>
<geom name="cap2" type="capsule" size=".04 .09" material="self" pos="-.02 0 0"/>
<site name="tip" type="sphere" size=".02" pos="0 0 .13" material="target"/>
<geom name="spinner_decoration" type="cylinder" fromto="0 -.045 0 0 .045 0" size=".02" material="decoration"/>
</body>
<!-- Target as a Site (used for sensors) -->
<site name="target" type="sphere" size=".03" pos="0 0 .4" material="target"/>
<!-- Visible target geometry (MotrixSim renderer may not draw sites) -->
<body name="target_vis" pos="0 0 .4">
<freejoint name="target_free"/>
<geom name="target_geom" type="sphere" size=".07" material="target" contype="0" conaffinity="0"/>
</body>
</worldbody>
<actuator>
<motor name="proximal" joint="proximal" gear="30"/>
<motor name="distal" joint="distal" gear="15"/>
</actuator>
<!-- All finger observations are functions of sensors. This is useful for finite-differencing. -->
<sensor>
<jointpos name="proximal" joint="proximal"/>
<jointpos name="distal" joint="distal"/>
<jointvel name="proximal_velocity" joint="proximal"/>
<jointvel name="distal_velocity" joint="distal"/>
<jointvel name="hinge_velocity" joint="hinge"/>
<framepos name="tip" objtype="site" objname="tip"/>
<framepos name="target" objtype="site" objname="target"/>
<framepos name="spinner" objtype="xbody" objname="spinner"/>
<touch name="touchtop" site="touchtop"/>
<touch name="touchbottom" site="touchbottom"/>
<framepos name="touchtop_pos" objtype="site" objname="touchtop"/>
<framepos name="touchbottom_pos" objtype="site" objname="touchbottom"/>
</sensor>
</mujoco>

82
motrix_envs/src/motrix_envs/basic/finger/finger_turn_hard.xml Normal file
View File

@@ -0,0 +1,82 @@
<mujoco model="finger">
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<!-- Match cartpole's skybox + floor look -->
<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>
<option timestep="0.01" cone="elliptic" iterations="200">
<flag gravity="disable"/>
</option>
<default>
<geom solimp="0 0.9 0.01" solref=".02 1"/>
<joint type="hinge" axis="0 -1 0"/>
<motor ctrllimited="true" ctrlrange="-1 1"/>
<default class="finger">
<joint damping="2.5" limited="true"/>
<site type="sphere" size=".03" material="site" group="3"/>
</default>
</default>
<worldbody>
<light name="light" directional="true" diffuse=".6 .6 .6" pos="0 0 2" specular=".3 .3 .3"/>
<geom name="ground" type="plane" pos="0 0 0" size="0 0 0.01" material="motphys-ground"/>
<camera name="cam0" pos="0 -1 .8" xyaxes="1 0 0 0 1 2"/>
<camera name="cam1" pos="0 -1 .4" xyaxes="1 0 0 0 0 1"/>
<body name="proximal" pos="-.2 0 .4" childclass="finger">
<geom name="proximal_decoration" type="cylinder" fromto="0 -.033 0 0 .033 0" size=".034" material="decoration"/>
<joint name="proximal" range="-110 110" ref="-90"/>
<geom name="proximal" type="capsule" material="self" size=".03" fromto="0 0 0 0 0 -.17"/>
<body name="distal" pos="0 0 -.18" childclass="finger">
<joint name="distal" range="-110 110"/>
<geom name="distal" type="capsule" size=".028" material="self" fromto="0 0 0 0 0 -.16" contype="0" conaffinity="0"/>
<geom name="fingertip" type="capsule" size=".03" material="effector" fromto="0 0 -.13 0 0 -.161"/>
<site name="touchtop" pos=".01 0 -.17"/>
<site name="touchbottom" pos="-.01 0 -.17"/>
</body>
</body>
<body name="spinner" pos=".2 0 .4">
<joint name="hinge" frictionloss=".1" damping=".5"/>
<geom name="cap1" type="capsule" size=".04 .09" material="self" pos=".02 0 0"/>
<geom name="cap2" type="capsule" size=".04 .09" material="self" pos="-.02 0 0"/>
<site name="tip" type="sphere" size=".02" pos="0 0 .13" material="target"/>
<geom name="spinner_decoration" type="cylinder" fromto="0 -.045 0 0 .045 0" size=".02" material="decoration"/>
</body>
<!-- Target as a Site (used for sensors) -->
<site name="target" type="sphere" size=".03" pos="0 0 .4" material="target"/>
<!-- Visible target geometry (MotrixSim renderer may not draw sites) -->
<body name="target_vis" pos="0 0 .4">
<freejoint name="target_free"/>
<geom name="target_geom" type="sphere" size=".03" material="target" contype="0" conaffinity="0"/>
</body>
</worldbody>
<actuator>
<motor name="proximal" joint="proximal" gear="30"/>
<motor name="distal" joint="distal" gear="15"/>
</actuator>
<sensor>
<jointpos name="proximal" joint="proximal"/>
<jointpos name="distal" joint="distal"/>
<jointvel name="proximal_velocity" joint="proximal"/>
<jointvel name="distal_velocity" joint="distal"/>
<jointvel name="hinge_velocity" joint="hinge"/>
<framepos name="tip" objtype="site" objname="tip"/>
<framepos name="target" objtype="site" objname="target"/>
<framepos name="spinner" objtype="xbody" objname="spinner"/>
<touch name="touchtop" site="touchtop"/>
<touch name="touchbottom" site="touchbottom"/>
<framepos name="touchtop_pos" objtype="site" objname="touchtop"/>
<framepos name="touchbottom_pos" objtype="site" objname="touchbottom"/>
</sensor>
</mujoco>

16
motrix_envs/src/motrix_envs/basic/humanoid/__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 humanoid_np # noqa: F401

84
motrix_envs/src/motrix_envs/basic/humanoid/cfg.py Normal file
View File

@@ -0,0 +1,84 @@
# 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__) + "/humanoid.xml"
@dataclass
class InitStateConfig:
reset_height_factor: float = 0.95
reset_qvel_range: float = 0.01
reset_actuator_range: float = 0.02
hip_yaw_range: tuple[float, float] = (-15.0, 15.0)
hip_roll_range: tuple[float, float] = (-12.0, 12.0)
hip_pitch_range: tuple[float, float] = (-12.0, 12.0)
symmetric_leg_pairs: list[tuple[int, int, tuple[float, float]]] = field(
default_factory=lambda: [
(10, 16, (-18.0, 2.0)),
(11, 17, (-25.0, 20.0)),
(12, 18, (-70.0, 5.0)),
(13, 19, (-45.0, -25.0)),
(14, 20, (-40.0, 0.0)),
(15, 21, (-25.0, 25.0)),
]
)
symmetric_arm_pairs: list[tuple[int, int]] = field(
default_factory=lambda: [
(22, 25),
(23, 26),
(24, 27),
]
)
arm_margin_factor: float = 0.1
@dataclass
class TerminationConfig:
head_height_factor: float = 0.5
torso_upright_threshold: float = 0.2
extreme_vel_threshold: float = 200.0
@registry.envcfg("dm-humanoid-walk")
@dataclass
class HumanoidWalkCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 25.0
sim_dt: float = 0.01
ctrl_dt: float = 0.01
move_speed: float = 1.0
stand_height: float = 1.4
init_state: InitStateConfig = field(default_factory=InitStateConfig)
termination_config: TerminationConfig = field(default_factory=TerminationConfig)
@registry.envcfg("dm-humanoid-stand")
@dataclass
class HumanoidStandCfg(HumanoidWalkCfg):
move_speed: float = 0.0
@registry.envcfg("dm-humanoid-run")
@dataclass
class HumanoidRunCfg(HumanoidWalkCfg):
move_speed: float = 10.0

207
motrix_envs/src/motrix_envs/basic/humanoid/humanoid.xml Normal file
View File

@@ -0,0 +1,207 @@
<mujoco model="humanoid">
<include file="../../common/skybox.xml"/>
<include file="../../common/visual.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<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>
<statistic extent="2" center="0 0 1"/>
<option timestep=".005"/>
<default>
<motor ctrlrange="-1 1" ctrllimited="true"/>
<default class="body">
<geom type="capsule" condim="1" friction=".7" solimp=".9 .99 .003" solref=".015 1" material="self"/>
<joint type="hinge" damping=".2" stiffness="1" armature=".01" limited="true" solimplimit="0 .99 .01"/>
<default class="big_joint">
<joint damping="5" stiffness="10"/>
<default class="big_stiff_joint">
<joint stiffness="20"/>
</default>
</default>
<site size=".04" group="3"/>
<default class="force-torque">
<site type="box" size=".01 .01 .02" rgba="1 0 0 1" />
</default>
<default class="touch">
<site type="capsule" rgba="0 0 1 .3"/>
</default>
</default>
</default>
<worldbody>
<geom name="floor" size="0 0 0.01" type="plane" material="motphys-ground" contype="1"
conaffinity="1" priority="1" friction="0.6" condim="3" />
<body name="torso" pos="0 0 1.5" childclass="body">
<light name="sun" pos="0 0 8" dir="0 0 -1" directional="true" ambient="0.4 0.4 0.4" diffuse="0.8 0.8 0.8" specular="0.1 0.1 0.1"/>
<camera name="back" pos="-3 0 1" xyaxes="0 -1 0 1 0 2" mode="trackcom"/>
<camera name="side" pos="0 -3 1" xyaxes="1 0 0 0 1 2" mode="trackcom"/>
<freejoint name="root"/>
<site name="root" class="force-torque"/>
<geom name="torso" fromto="0 -.07 0 0 .07 0" size=".07"/>
<geom name="upper_waist" fromto="-.01 -.06 -.12 -.01 .06 -.12" size=".06"/>
<site name="torso" class="touch" type="box" pos="0 0 -.05" size=".075 .14 .13"/>
<body name="head" pos="0 0 .19">
<geom name="head" type="sphere" size=".09"/>
<site name="head" class="touch" type="sphere" size=".091"/>
<camera name="egocentric" pos=".09 0 0" xyaxes="0 -1 0 .1 0 1" fovy="80"/>
</body>
<body name="lower_waist" pos="-.01 0 -.260" quat="1.000 0 -.002 0">
<geom name="lower_waist" fromto="0 -.06 0 0 .06 0" size=".06"/>
<site name="lower_waist" class="touch" size=".061 .06" zaxis="0 1 0"/>
<joint name="abdomen_z" pos="0 0 .065" axis="0 0 1" range="-45 45" class="big_stiff_joint"/>
<joint name="abdomen_y" pos="0 0 .065" axis="0 1 0" range="-75 30" class="big_joint"/>
<body name="pelvis" pos="0 0 -.165" quat="1.000 0 -.002 0">
<joint name="abdomen_x" pos="0 0 .1" axis="1 0 0" range="-35 35" class="big_joint"/>
<geom name="butt" fromto="-.02 -.07 0 -.02 .07 0" size=".09"/>
<site name="butt" class="touch" size=".091 .07" pos="-.02 0 0" zaxis="0 1 0"/>
<body name="right_thigh" pos="0 -.1 -.04">
<site name="right_hip" class="force-torque"/>
<joint name="right_hip_x" axis="1 0 0" range="-25 5" class="big_joint"/>
<joint name="right_hip_z" axis="0 0 1" range="-60 35" class="big_joint"/>
<joint name="right_hip_y" axis="0 1 0" range="-120 20" class="big_stiff_joint"/>
<geom name="right_thigh" fromto="0 0 0 0 .01 -.34" size=".06"/>
<site name="right_thigh" class="touch" pos="0 .005 -.17" size=".061 .17" zaxis="0 -1 34"/>
<body name="right_shin" pos="0 .01 -.403">
<site name="right_knee" class="force-torque" pos="0 0 .02"/>
<joint name="right_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="right_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<site name="right_shin" class="touch" pos="0 0 -.15" size=".05 .15"/>
<body name="right_foot" pos="0 0 -.39">
<site name="right_ankle" class="force-torque"/>
<joint name="right_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="right_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="right_right_foot" fromto="-.07 -.02 0 .14 -.04 0" size=".027"/>
<geom name="left_right_foot" fromto="-.07 0 0 .14 .02 0" size=".027"/>
<site name="right_right_foot" class="touch" pos=".035 -.03 0" size=".03 .11" zaxis="21 -2 0"/>
<site name="left_right_foot" class="touch" pos=".035 .01 0" size=".03 .11" zaxis="21 2 0"/>
</body>
</body>
</body>
<body name="left_thigh" pos="0 .1 -.04">
<site name="left_hip" class="force-torque"/>
<joint name="left_hip_x" axis="-1 0 0" range="-25 5" class="big_joint"/>
<joint name="left_hip_z" axis="0 0 -1" range="-60 35" class="big_joint"/>
<joint name="left_hip_y" axis="0 1 0" range="-120 20" class="big_stiff_joint"/>
<geom name="left_thigh" fromto="0 0 0 0 -.01 -.34" size=".06"/>
<site name="left_thigh" class="touch" pos="0 -.005 -.17" size=".061 .17" zaxis="0 1 34"/>
<body name="left_shin" pos="0 -.01 -.403">
<site name="left_knee" class="force-torque" pos="0 0 .02"/>
<joint name="left_knee" pos="0 0 .02" axis="0 -1 0" range="-160 2"/>
<geom name="left_shin" fromto="0 0 0 0 0 -.3" size=".049"/>
<site name="left_shin" class="touch" pos="0 0 -.15" size=".05 .15"/>
<body name="left_foot" pos="0 0 -.39">
<site name="left_ankle" class="force-torque"/>
<joint name="left_ankle_y" pos="0 0 .08" axis="0 1 0" range="-50 50" stiffness="6"/>
<joint name="left_ankle_x" pos="0 0 .04" axis="1 0 .5" range="-50 50" stiffness="3"/>
<geom name="left_left_foot" fromto="-.07 .02 0 .14 .04 0" size=".027"/>
<geom name="right_left_foot" fromto="-.07 0 0 .14 -.02 0" size=".027"/>
<site name="right_left_foot" class="touch" pos=".035 -.01 0" size=".03 .11" zaxis="21 -2 0"/>
<site name="left_left_foot" class="touch" pos=".035 .03 0" size=".03 .11" zaxis="21 2 0"/>
</body>
</body>
</body>
</body>
</body>
<body name="right_upper_arm" pos="0 -.17 .06">
<joint name="right_shoulder1" axis="2 1 1" range="-85 60"/>
<joint name="right_shoulder2" axis="0 -1 1" range="-85 60"/>
<geom name="right_upper_arm" fromto="0 0 0 .16 -.16 -.16" size=".04 .16"/>
<site name="right_upper_arm" class="touch" pos=".08 -.08 -.08" size=".041 .14" zaxis="1 -1 -1"/>
<body name="right_lower_arm" pos=".18 -.18 -.18">
<joint name="right_elbow" axis="0 -1 1" range="-90 50" stiffness="0"/>
<geom name="right_lower_arm" fromto=".01 .01 .01 .17 .17 .17" size=".031"/>
<site name="right_lower_arm" class="touch" pos=".09 .09 .09" size=".032 .14" zaxis="1 1 1"/>
<body name="right_hand" pos=".18 .18 .18">
<geom name="right_hand" type="sphere" size=".04"/>
<site name="right_hand" class="touch" type="sphere" size=".041"/>
</body>
</body>
</body>
<body name="left_upper_arm" pos="0 .17 .06">
<joint name="left_shoulder1" axis="2 -1 1" range="-60 85"/>
<joint name="left_shoulder2" axis="0 1 1" range="-60 85"/>
<geom name="left_upper_arm" fromto="0 0 0 .16 .16 -.16" size=".04 .16"/>
<site name="left_upper_arm" class="touch" pos=".08 .08 -.08" size=".041 .14" zaxis="1 1 -1"/>
<body name="left_lower_arm" pos=".18 .18 -.18">
<joint name="left_elbow" axis="0 -1 -1" range="-90 50" stiffness="0"/>
<geom name="left_lower_arm" fromto=".01 -.01 .01 .17 -.17 .17" size=".031"/>
<site name="left_lower_arm" class="touch" pos=".09 -.09 .09" size=".032 .14" zaxis="1 -1 1"/>
<body name="left_hand" pos=".18 -.18 .18">
<geom name="left_hand" type="sphere" size=".04"/>
<site name="left_hand" class="touch" type="sphere" size=".041"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor name="abdomen_y" gear="40" joint="abdomen_y"/>
<motor name="abdomen_z" gear="40" joint="abdomen_z"/>
<motor name="abdomen_x" gear="40" joint="abdomen_x"/>
<motor name="right_hip_x" gear="40" joint="right_hip_x"/>
<motor name="right_hip_z" gear="40" joint="right_hip_z"/>
<motor name="right_hip_y" gear="120" joint="right_hip_y"/>
<motor name="right_knee" gear="80" joint="right_knee"/>
<motor name="right_ankle_x" gear="20" joint="right_ankle_x"/>
<motor name="right_ankle_y" gear="20" joint="right_ankle_y"/>
<motor name="left_hip_x" gear="40" joint="left_hip_x"/>
<motor name="left_hip_z" gear="40" joint="left_hip_z"/>
<motor name="left_hip_y" gear="120" joint="left_hip_y"/>
<motor name="left_knee" gear="80" joint="left_knee"/>
<motor name="left_ankle_x" gear="20" joint="left_ankle_x"/>
<motor name="left_ankle_y" gear="20" joint="left_ankle_y"/>
<motor name="right_shoulder1" gear="20" joint="right_shoulder1"/>
<motor name="right_shoulder2" gear="20" joint="right_shoulder2"/>
<motor name="right_elbow" gear="40" joint="right_elbow"/>
<motor name="left_shoulder1" gear="20" joint="left_shoulder1"/>
<motor name="left_shoulder2" gear="20" joint="left_shoulder2"/>
<motor name="left_elbow" gear="40" joint="left_elbow"/>
</actuator>
<sensor>
<subtreelinvel name="torso_subtreelinvel" body="torso"/>
<accelerometer name="torso_accel" site="root"/>
<velocimeter name="torso_vel" site="root"/>
<gyro name="torso_gyro" site="root"/>
<force name="left_ankle_force" site="left_ankle"/>
<force name="right_ankle_force" site="right_ankle"/>
<force name="left_knee_force" site="left_knee"/>
<force name="right_knee_force" site="right_knee"/>
<force name="left_hip_force" site="left_hip"/>
<force name="right_hip_force" site="right_hip"/>
<torque name="left_ankle_torque" site="left_ankle"/>
<torque name="right_ankle_torque" site="right_ankle"/>
<torque name="left_knee_torque" site="left_knee"/>
<torque name="right_knee_torque" site="right_knee"/>
<torque name="left_hip_torque" site="left_hip"/>
<torque name="right_hip_torque" site="right_hip"/>
<touch name="torso_touch" site="torso"/>
<touch name="head_touch" site="head"/>
<touch name="lower_waist_touch" site="lower_waist"/>
<touch name="butt_touch" site="butt"/>
<touch name="right_thigh_touch" site="right_thigh"/>
<touch name="right_shin_touch" site="right_shin"/>
<touch name="right_right_foot_touch" site="right_right_foot"/>
<touch name="left_right_foot_touch" site="left_right_foot"/>
<touch name="left_thigh_touch" site="left_thigh"/>
<touch name="left_shin_touch" site="left_shin"/>
<touch name="right_left_foot_touch" site="right_left_foot"/>
<touch name="left_left_foot_touch" site="left_left_foot"/>
<touch name="right_upper_arm_touch" site="right_upper_arm"/>
<touch name="right_lower_arm_touch" site="right_lower_arm"/>
<touch name="right_hand_touch" site="right_hand"/>
<touch name="left_upper_arm_touch" site="left_upper_arm"/>
<touch name="left_lower_arm_touch" site="left_lower_arm"/>
<touch name="left_hand_touch" site="left_hand"/>
</sensor>
</mujoco>

550
motrix_envs/src/motrix_envs/basic/humanoid/humanoid_np.py Normal file
View File

@@ -0,0 +1,550 @@
# 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.humanoid.cfg import HumanoidWalkCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-humanoid-stand", "np")
@registry.env("dm-humanoid-walk", "np")
@registry.env("dm-humanoid-run", "np")
class Humanoid3DEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: HumanoidWalkCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_obs_space()
self._init_action_space()
self._torso = self._model.get_link("torso")
self._head = self._model.get_link("head")
self._pelvis = self._model.get_link("pelvis")
self._left_hand = self._model.get_link("left_hand")
self._right_hand = self._model.get_link("right_hand")
self._left_foot = self._model.get_link("left_foot")
self._right_foot = self._model.get_link("right_foot")
self._move_speed = float(cfg.move_speed)
self._stand_height = float(cfg.stand_height)
self._target_direction = np.array([1.0, 0.0, 0.0], dtype=np.float32)
self._target_direction_xy = self._target_direction[:2].copy()
self._qpos_low, self._qpos_high = self._build_qpos_limits(self._model)
self._cache_derived_constants(cfg)
self._init_joint_randomization_config(cfg)
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)
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)
return low, high
k = int(jl.shape[1])
low = np.full((num_dof_pos,), -np.inf, dtype=np.float32)
high = np.full((num_dof_pos,), np.inf, dtype=np.float32)
m = min(k, num_dof_pos)
low[:m] = jl[0, :m]
high[:m] = jl[1, :m]
return low, high
def _cache_derived_constants(self, cfg: HumanoidWalkCfg) -> None:
t_cfg = cfg.termination_config
self._head_height_min = self._stand_height * 0.95
self._pelvis_height_min = 0.6 * self._stand_height
self._pelvis_height_margin = 0.6 * self._stand_height
self._term_head_height_min = float(t_cfg.head_height_factor) * self._stand_height
self._term_torso_upright_threshold = float(t_cfg.torso_upright_threshold)
self._term_extreme_vel_threshold = float(t_cfg.extreme_vel_threshold)
def _init_obs_space(self):
model = self._model
num_joint_angles = model.num_dof_pos - 7
num_head_height = 1
num_extremities = 12
num_torso_vertical = 3
num_com_vel = 3
num_qvel = model.num_dof_vel
num_target_local = 3
num_obs = (
num_joint_angles
+ num_head_height
+ num_extremities
+ num_torso_vertical
+ num_com_vel
+ num_qvel
+ num_target_local
)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
model = self._model
self._action_space = gym.spaces.Box(
model.actuator_ctrl_limits[0],
model.actuator_ctrl_limits[1],
(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:
state.data.actuator_ctrls = actions
return state
def update_state(self, state: NpEnvState) -> NpEnvState:
state = self.update_observation(state)
state = self.update_terminated(state)
state = self.update_reward(state)
return state
def update_observation(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
return state.replace(obs=obs)
def update_terminated(self, state: NpEnvState) -> NpEnvState:
data = state.data
head_height = self._get_head_height(data)
torso_upright = self._get_torso_upright(data)
terminated = self._compute_terminated(data, head_height, torso_upright)
return state.replace(
terminated=terminated,
)
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data
terminated = state.terminated
head_height = self._get_head_height(data)
pelvis_height = self._get_pelvis_height(data)
torso_upright = self._get_torso_upright(data)
rwd, reward_components = self._compute_reward(data, head_height, torso_upright, pelvis_height)
rwd, reward_components = self._apply_termination_mask(terminated, rwd, reward_components)
state.info["Reward"] = reward_components
return state.replace(reward=rwd)
def _apply_termination_mask(
self,
terminated: np.ndarray,
rwd: np.ndarray,
reward_components: dict,
) -> tuple[np.ndarray, dict]:
rwd = np.where(terminated, 0.0, rwd).astype(np.float32)
for k, v in reward_components.items():
reward_components[k] = np.where(terminated, 0.0, v).astype(np.float32)
return rwd, reward_components
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
self._randomize_joints_inplace(data)
obs = self._get_obs(data)
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)
torso_rot = self._torso.get_rotation_mat(data)
torso_vertical = np.asarray(torso_rot[:, 2, :], dtype=np.float32)
com_vel = np.asarray(self._model.get_sensor_value("torso_subtreelinvel", data), dtype=np.float32)
qvel = np.asarray(data.dof_vel, dtype=np.float32)
target_direction_local = self._get_target_direction_local(data).astype(np.float32)
obs = np.concatenate(
[joint_angles, head_height, extremities, torso_vertical, com_vel, qvel, target_direction_local], axis=-1
)
return obs
def _get_head_height(self, data: mtx.SceneData) -> np.ndarray:
return np.asarray(self._head.get_position(data)[:, 2], dtype=np.float32)
def _get_pelvis_height(self, data: mtx.SceneData) -> np.ndarray:
return np.asarray(self._pelvis.get_position(data)[:, 2], dtype=np.float32)
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)
def _get_extremities(self, data: mtx.SceneData) -> np.ndarray:
torso_rot = self._torso.get_rotation_mat(data)
torso_pos = self._torso.get_position(data)
parts = [
self._left_hand.get_position(data),
self._left_foot.get_position(data),
self._right_hand.get_position(data),
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)
out.append(v_body)
return np.concatenate(out, axis=-1)
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)
return target_local
def _compute_reward(
self,
data: mtx.SceneData,
head_height: np.ndarray,
torso_upright: np.ndarray,
pelvis_height: np.ndarray,
) -> tuple[np.ndarray, dict]:
posture_reward = self._compute_posture_reward(head_height, torso_upright, pelvis_height)
speed_reward, energy_reward = self._compute_speed_and_energy_reward(data)
gait_reward = self._compute_gait_reward(data)
rwd = (posture_reward * speed_reward * energy_reward * gait_reward).astype(np.float32)
comps = {
"energy": energy_reward.astype(np.float32),
"speed": speed_reward.astype(np.float32),
"posture": posture_reward.astype(np.float32),
"gait": gait_reward.astype(np.float32),
}
return rwd, comps
def _compute_posture_reward(
self,
head_height: np.ndarray,
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()
)
upright_reward = (
reward.tolerance(
torso_upright,
bounds=(0.9, float("inf")),
sigmoid="linear",
margin=0.9,
)
.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,
)
.astype(np.float32)
.flatten()
)
return (stand_reward * upright_reward * pelvis_height_reward).astype(np.float32)
def _compute_speed_and_energy_reward(
self,
data: mtx.SceneData,
) -> 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)
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()
)
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()
)
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()
)
return speed_reward, energy_reward
def _compute_heading_reward(
self,
forward_vec: np.ndarray,
target_dir: np.ndarray,
bounds,
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()
)
def _compute_gait_reward(self, data: mtx.SceneData) -> np.ndarray:
target_dir = self._target_direction
torso_rot = self._torso.get_rotation_mat(data)
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_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_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_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()
)
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()
)
return (
torso_heading_reward * head_heading_reward * pelvis_yaw_reward * pelvis_level_reward * feet_height_reward
).astype(np.float32)
def _compute_terminated(
self,
data: mtx.SceneData,
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)
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)
def _init_joint_randomization_config(self, cfg: HumanoidWalkCfg) -> None:
init_cfg = cfg.init_state
self._reset_height = self._stand_height * init_cfg.reset_height_factor
self._reset_qvel_range = init_cfg.reset_qvel_range
self._reset_actuator_range = init_cfg.reset_actuator_range
self._hip_yaw_range = tuple(np.deg2rad(x) for x in init_cfg.hip_yaw_range)
self._hip_roll_range = tuple(np.deg2rad(x) for x in init_cfg.hip_roll_range)
self._hip_pitch_range = tuple(np.deg2rad(x) for x in init_cfg.hip_pitch_range)
self._symmetric_leg_pairs_rad = [
(left_idx, right_idx, tuple(np.deg2rad(x) for x in deg_range))
for left_idx, right_idx, deg_range in init_cfg.symmetric_leg_pairs
]
self._symmetric_arm_pairs = init_cfg.symmetric_arm_pairs
self._arm_margin_factor = init_cfg.arm_margin_factor
self._symmetric_arm_used_indices = set()
for left_idx, right_idx in self._symmetric_arm_pairs:
self._symmetric_arm_used_indices.add(left_idx)
self._symmetric_arm_used_indices.add(right_idx)
def _randomize_joints_inplace(self, data: mtx.SceneData) -> None:
# qpos layout (humanoid.xml): 0-6 free (x,y,z,qw,qx,qy,qz), 7=abdomen_z, 8=abdomen_y, 9=abdomen_x,
# 10-15 right leg (hip_x,z,y, knee, ankle_y,x), 16-21 left leg, 22-24 right arm, 25-27 left arm (num_dof_pos=28)
model = self._model
n = int(data.shape[0])
num_dof_pos = int(model.num_dof_pos)
num_dof_vel = int(model.num_dof_vel)
num_actuators = int(model.num_actuators)
low, high = self._qpos_low, self._qpos_high
qpos = np.zeros((n, num_dof_pos), dtype=np.float32)
qpos[:, 2] = self._reset_height
qpos[:, 3] = 1.0
# qpos 7=abdomen_z (yaw), 8=abdomen_y (pitch), 9=abdomen_x (roll) per humanoid.xml
qpos[:, 7] = np.random.uniform(self._hip_yaw_range[0], self._hip_yaw_range[1], size=(n,))
qpos[:, 8] = np.random.uniform(self._hip_pitch_range[0], self._hip_pitch_range[1], size=(n,))
qpos[:, 9] = np.random.uniform(self._hip_roll_range[0], self._hip_roll_range[1], size=(n,))
self._randomize_symmetric_legs(qpos, n, num_dof_pos, low, high)
self._randomize_symmetric_arms(qpos, n, num_dof_pos, low, high)
self._randomize_remaining_joints(qpos, n, num_dof_pos, low, high)
qvel = np.random.uniform(-self._reset_qvel_range, self._reset_qvel_range, size=(n, num_dof_vel)).astype(
np.float32
)
actuator_ctrls = np.random.uniform(
-self._reset_actuator_range, self._reset_actuator_range, size=(n, num_actuators)
).astype(np.float32)
qpos_set = qpos.copy()
qpos_set[:, 3:7] = np.concatenate([qpos[:, 4:7], qpos[:, 3:4]], axis=1)
data.set_dof_pos(qpos_set, self._model)
data.set_dof_vel(qvel)
data.actuator_ctrls[:] = actuator_ctrls
self._model.forward_kinematic(data)
def _randomize_symmetric_legs(
self, qpos: np.ndarray, n: int, num_dof_pos: int, low: np.ndarray, high: np.ndarray
) -> None:
for left_idx, right_idx, (min_rad, max_rad) in self._symmetric_leg_pairs_rad:
if left_idx < num_dof_pos:
qpos[:, left_idx] = np.random.uniform(
np.clip(min_rad, low[left_idx], high[left_idx]),
np.clip(max_rad, low[left_idx], high[left_idx]),
size=(n,),
)
if right_idx < num_dof_pos:
right_min_rad = -max_rad
right_max_rad = -min_rad
qpos[:, right_idx] = np.random.uniform(
np.clip(right_min_rad, low[right_idx], high[right_idx]),
np.clip(right_max_rad, low[right_idx], high[right_idx]),
size=(n,),
)
def _randomize_symmetric_arms(
self, qpos: np.ndarray, n: int, num_dof_pos: int, low: np.ndarray, high: np.ndarray
) -> None:
# Default range when model joint_limits are missing (low/high are ±inf);
# np.random.uniform requires finite bounds.
default_lo, default_hi = -np.pi, np.pi
for left_idx, right_idx in self._symmetric_arm_pairs:
if left_idx < num_dof_pos and right_idx < num_dof_pos:
lo_l = low[left_idx] if np.isfinite(low[left_idx]) else default_lo
hi_l = high[left_idx] if np.isfinite(high[left_idx]) else default_hi
lo_r = low[right_idx] if np.isfinite(low[right_idx]) else default_lo
hi_r = high[right_idx] if np.isfinite(high[right_idx]) else default_hi
left_range = hi_l - lo_l
left_margin = left_range * self._arm_margin_factor
left_min = lo_l + left_margin
left_max = hi_l - left_margin
right_min = -left_max
right_max = -left_min
right_min_clipped = max(right_min, lo_r)
right_max_clipped = min(right_max, hi_r)
if left_min < left_max:
qpos[:, left_idx] = np.random.uniform(left_min, left_max, size=(n,))
else:
qpos[:, left_idx] = np.random.uniform(lo_l, hi_l, size=(n,))
if right_min_clipped < right_max_clipped:
qpos[:, right_idx] = np.random.uniform(right_min_clipped, right_max_clipped, size=(n,))
else:
qpos[:, right_idx] = np.random.uniform(lo_r, hi_r, size=(n,))
def _randomize_remaining_joints(
self, qpos: np.ndarray, n: int, num_dof_pos: int, low: np.ndarray, high: np.ndarray
) -> None:
used_indices = self._symmetric_arm_used_indices
default_lo, default_hi = -np.pi, np.pi
# 22 = first arm joint (right_shoulder1) in humanoid.xml qpos order;
# arms 22-27 are covered by symmetric_arm_pairs
for i in range(22, num_dof_pos):
if i not in used_indices:
lo = low[i] if np.isfinite(low[i]) else default_lo
hi = high[i] if np.isfinite(high[i]) else default_hi
qpos[:, i] = np.random.uniform(lo, hi, size=(n,))

16
motrix_envs/src/motrix_envs/basic/manipulator/__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 manipulator_np # noqa: F401

95
motrix_envs/src/motrix_envs/basic/manipulator/cfg.py Normal file
View File

@@ -0,0 +1,95 @@
# 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
bring_ball_model_file = os.path.join(os.path.dirname(__file__), "manipulator_bring_ball.xml")
@registry.envcfg("dm-manipulator-bring-ball")
@dataclass
class BringBallCfg(EnvCfg):
# Simulation
model_file: str = bring_ball_model_file
max_episode_seconds: float = 10.0
sim_dt: float = 0.001
ctrl_dt: float = 0.01
render_spacing: float = 2.5
# Reset sampling (match dm_control defaults).
p_in_hand: float = 0.1
p_in_target: float = 0.1
randomize_arm: bool = True
# Target sampling.
target_x_range: tuple[float, float] = (-0.4, 0.4)
target_z_range: tuple[float, float] = (0.1, 0.4)
target_y: float = 0.001
target_angle_range: tuple[float, float] = (-3.14159265, 3.14159265)
# Object sampling.
object_x_range: tuple[float, float] = (-0.4, 0.4)
object_z_range: tuple[float, float] = (0.0, 0.7)
object_angle_range: tuple[float, float] = (0.0, 6.28318531)
object_x_vel_range: tuple[float, float] = (-5.0, 5.0)
min_object_hand_dist: float = 0.08
# Physics settling at episode start (in control steps, i.e. ctrl_dt units).
# Internally this will be converted to `settle_steps * sim_substeps` physics steps.
settle_steps: int = 80
settle_zero_vel: bool = True
# BringBall reward shaping.
lift_height_threshold: float = 0.04
touch_threshold: float = 0.01
side_penalty_scale: float = 0.05
side_penalty_tanh_scale: float = 10.0
hover_penalty_scale: float = 0.02
hover_close_threshold: float = 0.1
post_grasp_discount: float = 0.7
lift_height_weight: float = 0.3
transport_weight: float = 0.7
transport_progress_scale: float = 0.0
transport_progress_clip: float = 0.02
precision_weight: float = 0.0
precision_margin: float = 0.02
precision_value_at_margin: float = 0.1
# BringBall-specific overrides.
settle_steps: int = 300
p_in_hand: float = 0.0
p_in_target: float = 0.0
randomize_arm: bool = False
object_z_range: tuple[float, float] = (0.2, 0.7)
object_x_vel_range: tuple[float, float] = (0.0, 0.0)
hover_penalty_scale: float = 0.03
post_grasp_discount: float = 0.0
lift_height_weight: float = 0.1
transport_weight: float = 2.0
transport_progress_scale: float = 2.0
transport_progress_clip: float = 0.02
precision_weight: float = 1.0
precision_margin: float = 0.01
# Reward component weights (total reward mixing).
reach_weight: float = 1.0
orient_weight: float = 1.5
pause_weight: float = 0.5
close_weight: float = 2.0
lift_reward_weight: float = 6.0

171
motrix_envs/src/motrix_envs/basic/manipulator/manipulator_bring_ball.xml Normal file
View File

@@ -0,0 +1,171 @@
<mujoco model="planar manipulator - bring ball">
<include file="../../common/visual.xml"/>
<include file="../../common/skybox.xml"/>
<include file="../../common/materials.xml"/>
<asset>
<texture name="background" type="2d" file="../../common/motphys-ground.png"/>
<material name="background" texture="background" texrepeat="1 1" texuniform="true"/>
</asset>
<visual>
<map shadowclip=".5"/>
<quality shadowsize="2048"/>
</visual>
<option timestep="0.001" cone="elliptic"/>
<default>
<geom friction=".7" solimp="0.9 0.97 0.001" solref=".005 1"/>
<joint solimplimit="0 0.99 0.01" solreflimit=".005 1"/>
<general ctrllimited="true"/>
<tendon width="0.01"/>
<site size=".003 .003 .003" material="site" group="3"/>
<default class="arm">
<geom type="capsule" material="self" density="500"/>
<joint type="hinge" pos="0 0 0" axis="0 -1 0" limited="true"/>
<default class="hand">
<joint damping=".5" range="-10 60"/>
<geom size=".008"/>
<site type="box" size=".018 .005 .005" pos=".022 0 -.002" euler="0 15 0" group="4"/>
<default class="fingertip">
<geom type="sphere" size=".008" material="effector"/>
<joint damping=".01" stiffness=".01" range="-40 20"/>
<site size=".012 .005 .008" pos=".003 0 .003" group="4" euler="0 0 0"/>
</default>
</default>
</default>
<default class="object">
<geom material="self"/>
</default>
<default class="task">
<site rgba="0 0 0 0"/>
</default>
<default class="obstacle">
<geom material="decoration" friction="0"/>
</default>
<default class="ghost">
<geom material="target" contype="0" conaffinity="0"/>
</default>
</default>
<worldbody>
<!-- Arena -->
<light name="light" directional="true" diffuse=".6 .6 .6" pos="0 0 1" dir="0 0.1 -1" specular=".3 .3 .3"/>
<geom name="floor" type="plane" pos="0 0 0" size=".4 .2 10" material="background"/>
<geom name="wall1" type="plane" pos="-.682843 0 .282843" size=".4 .2 10" material="background" zaxis="1 0 1" contype="0" conaffinity="0"/>
<geom name="wall2" type="plane" pos=".682843 0 .282843" size=".4 .2 10" material="background" zaxis="-1 0 1" contype="0" conaffinity="0"/>
<geom name="wall1_collider" type="box" pos="-.7394115 0 .2262745" size="5 .2 .08" quat="0.9238795 0 0.3826834 0" contype="1" conaffinity="1" rgba="0 0 0 0.001"/>
<geom name="wall2_collider" type="box" pos=".7394115 0 .2262745" size="5 .2 .08" quat="0.9238795 0 -0.3826834 0" contype="1" conaffinity="1" rgba="0 0 0 0.001"/>
<geom name="background" type="plane" pos="0 .2 .5" size="1 .5 10" material="background" zaxis="0 -1 0" contype="0" conaffinity="0"/>
<camera name="fixed" pos="0 -16 .4" xyaxes="1 0 0 0 0 1" fovy="4"/>
<!-- Arm -->
<geom name="arm_root" type="cylinder" fromto="0 -.022 .4 0 .022 .4" size=".024"
material="decoration" contype="0" conaffinity="0"/>
<body name="upper_arm" pos="0 0 .4" childclass="arm">
<joint name="arm_root" damping="2" limited="false"/>
<geom name="upper_arm" size=".02" fromto="0 0 0 0 0 .18"/>
<body name="middle_arm" pos="0 0 .18" childclass="arm">
<joint name="arm_shoulder" damping="1.5" range="-160 160"/>
<geom name="middle_arm" size=".017" fromto="0 0 0 0 0 .15"/>
<body name="lower_arm" pos="0 0 .15">
<joint name="arm_elbow" damping="1" range="-160 160"/>
<geom name="lower_arm" size=".014" fromto="0 0 0 0 0 .12"/>
<body name="hand" pos="0 0 .12">
<joint name="arm_wrist" damping=".5" range="-140 140" />
<geom name="hand" size=".011" fromto="0 0 0 0 0 .03"/>
<geom name="palm1" fromto="0 0 .03 .03 0 .045" class="hand"/>
<geom name="palm2" fromto="0 0 .03 -.03 0 .045" class="hand"/>
<site name="grasp" pos="0 0 .065"/>
<body name="pinch site" pos="0 0 .090">
<site name="pinch"/>
<inertial pos="0 0 0" mass="1e-6" diaginertia="1e-12 1e-12 1e-12"/>
<camera name="hand" pos="0 -.3 0" xyaxes="1 0 0 0 0 1" mode="track"/>
</body>
<site name="palm_touch" type="box" group="4" size=".025 .005 .008" pos="0 0 .043"/>
<body name="thumb" pos=".03 0 .045" euler="0 -90 0" childclass="hand">
<joint name="thumb"/>
<geom name="thumb1" fromto="0 0 0 .02 0 -.01" size=".007"/>
<geom name="thumb2" fromto=".02 0 -.01 .04 0 -.01" size=".007"/>
<site name="thumb_touch" group="4"/>
<body name="thumbtip" pos=".05 0 -.01" childclass="fingertip">
<joint name="thumbtip"/>
<geom name="thumbtip1" pos="-.003 0 0" />
<geom name="thumbtip2" pos=".003 0 0" />
<site name="thumbtip_touch" group="4"/>
</body>
</body>
<body name="finger" pos="-.03 0 .045" euler="0 90 180" childclass="hand">
<joint name="finger"/>
<geom name="finger1" fromto="0 0 0 .02 0 -.01" size=".007" />
<geom name="finger2" fromto=".02 0 -.01 .04 0 -.01" size=".007"/>
<site name="finger_touch"/>
<body name="fingertip" pos=".05 0 -.01" childclass="fingertip">
<joint name="fingertip"/>
<geom name="fingertip1" pos="-.003 0 0" />
<geom name="fingertip2" pos=".003 0 0" />
<site name="fingertip_touch"/>
</body>
</body>
</body>
</body>
</body>
</body>
<!-- prop: ball -->
<body name="ball" pos=".4 0 .4" childclass="object">
<joint name="ball_x" type="slide" axis="1 0 0" ref=".4"/>
<joint name="ball_z" type="slide" axis="0 0 1" ref=".4"/>
<joint name="ball_y" type="hinge" axis="0 1 0"/>
<geom name="ball" type="sphere" size=".022" />
<site name="ball" type="sphere"/>
</body>
<!-- target -->
<body name="target_ball" pos=".4 .001 .4" mocap="true" childclass="ghost">
<geom name="target_ball" type="sphere" size=".02" />
<site name="target_ball" type="sphere"/>
</body>
</worldbody>
<tendon>
<fixed name="grasp">
<joint joint="thumb" coef=".5"/>
<joint joint="finger" coef=".5"/>
</fixed>
<fixed name="coupling">
<joint joint="thumb" coef="-.5"/>
<joint joint="finger" coef=".5"/>
</fixed>
</tendon>
<equality>
<tendon name="coupling" tendon1="coupling" solimp="0.95 0.99 0.001" solref=".005 .5"/>
</equality>
<sensor>
<touch name="palm_touch" site="palm_touch"/>
<touch name="finger_touch" site="finger_touch"/>
<touch name="thumb_touch" site="thumb_touch"/>
<touch name="fingertip_touch" site="fingertip_touch"/>
<touch name="thumbtip_touch" site="thumbtip_touch"/>
</sensor>
<actuator>
<motor name="root" joint="arm_root" ctrlrange="-1 1" gear="12"/>
<motor name="shoulder" joint="arm_shoulder" ctrlrange="-1 1" gear="8"/>
<motor name="elbow" joint="arm_elbow" ctrlrange="-1 1" gear="4"/>
<motor name="wrist" joint="arm_wrist" ctrlrange="-1 1" gear="2"/>
<motor name="grasp" tendon="grasp" ctrlrange="-1 1" gear="2"/>
</actuator>
</mujoco>

579
motrix_envs/src/motrix_envs/basic/manipulator/manipulator_np.py Normal file
View File

@@ -0,0 +1,579 @@
# 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.manipulator.cfg import BringBallCfg
from motrix_envs.math import quaternion
from motrix_envs.np import reward as reward_utils
from motrix_envs.np.env import NpEnv, NpEnvState
_ARM_JOINTS = (
"arm_root",
"arm_shoulder",
"arm_elbow",
"arm_wrist",
"finger",
"fingertip",
"thumb",
"thumbtip",
)
_TOUCH_SENSORS = ("palm_touch", "finger_touch", "thumb_touch", "fingertip_touch", "thumbtip_touch")
_HAND_GEOMS = (
"hand",
"palm1",
"palm2",
"thumb1",
"thumb2",
"thumbtip1",
"thumbtip2",
"finger1",
"finger2",
"fingertip1",
"fingertip2",
)
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.astype(np.float32), high.astype(np.float32)
def _quat_from_y_angle(angle: np.ndarray) -> np.ndarray:
zeros = np.zeros_like(angle)
return quaternion.from_euler(zeros, angle, zeros)
def _quat_to_z_axis(quat: np.ndarray) -> np.ndarray:
quat = np.asarray(quat, dtype=np.float32)
return quaternion.rotate_vector(quat, np.array([0.0, 0.0, 1.0], dtype=np.float32)).astype(np.float32)
def _tolerance(
x: np.ndarray,
*,
bounds: tuple[float, float] = (0.0, 0.0),
margin: float = 0.0,
sigmoid: str = "gaussian",
value_at_margin: float = 0.1,
) -> np.ndarray:
"""Vectorized tolerance reward (ported from dm_control-style reward_utils)."""
return reward_utils.tolerance(x, bounds=bounds, margin=margin, sigmoid=sigmoid, value_at_margin=value_at_margin)
class ManipulatorBase(NpEnv):
_cfg: BringBallCfg
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: BringBallCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._cfg = cfg
self._joint_limit_low, self._joint_limit_high = _sanitize_joint_limits(*self._model.joint_limits)
self._arm_joint_pos_indices = np.array([self._joint_pos_index(n) for n in _ARM_JOINTS], dtype=np.int32)
self._arm_joint_vel_indices = np.array([self._joint_vel_index(n) for n in _ARM_JOINTS], dtype=np.int32)
self._thumb_qpos_i = self._joint_pos_index("thumb")
self._finger_qpos_i = self._joint_pos_index("finger")
self._thumbtip_qpos_i = self._joint_pos_index("thumbtip")
self._fingertip_qpos_i = self._joint_pos_index("fingertip")
self._grasp_site = self._model.get_site("grasp")
# Ensure correct actuator index is retrieved from base model
self._grasp_act_i = int(self._model.get_actuator_index("grasp"))
self._object_site = self._model.get_site("ball")
self._target_site = self._model.get_site("target_ball")
target_body = self._model.get_body("target_ball")
if target_body is None:
raise ValueError("Target body 'target_ball' not found in model")
self._target_mocap = target_body.mocap
object_qpos_joints = ("ball_x", "ball_z", "ball_y")
object_geom_names = ("ball",)
self._object_qpos_indices = np.array([self._joint_pos_index(n) for n in object_qpos_joints], dtype=np.int32)
self._object_qvel_indices = np.array([self._joint_vel_index(n) for n in object_qpos_joints], dtype=np.int32)
self._object_x_qvel_i = int(self._object_qvel_indices[0])
self._hand_geom_indices = np.array([self._model.get_geom_index(name) for name in _HAND_GEOMS], dtype=np.uint32)
object_geom_indices = np.array(
[self._model.get_geom_index(name) for name in object_geom_names], dtype=np.uint32
)
self._hand_object_pairs = np.stack(
[
np.repeat(self._hand_geom_indices, object_geom_indices.shape[0]),
np.tile(object_geom_indices, self._hand_geom_indices.shape[0]),
],
axis=-1,
).astype(np.uint32)
self._num_hand_object_pairs = int(self._hand_object_pairs.shape[0])
self._init_dof_pos = self._model.compute_init_dof_pos().astype(np.float32)
self._init_dof_vel = np.zeros((self._model.num_dof_vel,), dtype=np.float32)
self._init_action_space()
self._init_obs_space()
def _joint_pos_index(self, joint_name: str) -> int:
joint_index = self._model.get_joint_index(joint_name)
return int(self._model.joint_dof_pos_indices[joint_index])
def _joint_vel_index(self, joint_name: str) -> int:
joint_index = self._model.get_joint_index(joint_name)
return int(self._model.joint_dof_vel_indices[joint_index])
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)
def _init_obs_space(self):
# arm_pos(sin,cos)=16 + arm_vel=8 + touch=5 + hand=3 + object=3 + target=3 + rel=3
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (41,), 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:
actions = np.asarray(actions, dtype=np.float32)
# Enforce actuator control limits to avoid out-of-range impulses.
actions = np.clip(actions, self._action_space.low, self._action_space.high).astype(np.float32)
state.info["last_actions"] = state.info["actions"]
state.info["actions"] = actions
state.data.actuator_ctrls = actions
return state
def _touch_raw(self, data: mtx.SceneData) -> np.ndarray:
values = []
for name in _TOUCH_SENSORS:
v = np.asarray(self._model.get_sensor_value(name, data)).reshape(data.shape[0], -1)[:, 0]
values.append(v)
return np.stack(values, axis=-1).astype(np.float32)
def _touch_log(self, data: mtx.SceneData) -> np.ndarray:
return np.log1p(self._touch_raw(data))
def _hand_pos(self, data: mtx.SceneData) -> np.ndarray:
return self._grasp_site.get_position(data).astype(np.float32)
def _object_pos(self, data: mtx.SceneData) -> np.ndarray:
return self._object_site.get_position(data).astype(np.float32)
def _target_pos(self, data: mtx.SceneData) -> np.ndarray:
return self._target_site.get_position(data).astype(np.float32)
def _contact_with_object(self, data: mtx.SceneData) -> np.ndarray:
cquery = self._model.get_contact_query(data)
colliding = cquery.is_colliding(self._hand_object_pairs)
colliding = np.asarray(colliding).reshape((data.shape[0], self._num_hand_object_pairs))
return colliding.any(axis=-1)
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos[:, self._arm_joint_pos_indices]
arm_pos = np.stack([np.sin(qpos), np.cos(qpos)], axis=-1).reshape(data.shape[0], -1)
arm_vel = data.dof_vel[:, self._arm_joint_vel_indices]
touch = self._touch_log(data)
hand_pos = self._hand_pos(data)
object_pos = self._object_pos(data)
target_pos = self._target_pos(data)
rel = object_pos - target_pos
obs = np.concatenate([arm_pos, arm_vel, touch, hand_pos, object_pos, target_pos, rel], axis=-1)
assert obs.shape == (data.shape[0], self._observation_space.shape[0])
return obs.astype(np.float32)
def _sample_arm_joint_angles(self, num: int) -> np.ndarray:
joint_indices = np.array([self._model.get_joint_index(n) for n in _ARM_JOINTS], dtype=np.int32)
low = self._joint_limit_low[joint_indices]
high = self._joint_limit_high[joint_indices]
return np.random.uniform(low=low, high=high, size=(num, joint_indices.shape[0])).astype(np.float32)
def _sample_target_pose(self, num: int) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
cfg = self._cfg
target_x = np.random.uniform(cfg.target_x_range[0], cfg.target_x_range[1], size=(num,)).astype(np.float32)
target_z = np.random.uniform(cfg.target_z_range[0], cfg.target_z_range[1], size=(num,)).astype(np.float32)
target_angle = np.random.uniform(cfg.target_angle_range[0], cfg.target_angle_range[1], size=(num,)).astype(
np.float32
)
return target_x, target_z, target_angle
def _set_target_mocap(
self,
data: mtx.SceneData,
target_x: np.ndarray,
target_z: np.ndarray,
target_angle: np.ndarray,
):
pose = np.zeros((data.shape[0], 7), dtype=np.float32)
pose[:, 0] = target_x
pose[:, 1] = float(self._cfg.target_y)
pose[:, 2] = target_z
pose[:, 3:7] = _quat_from_y_angle(target_angle)
self._target_mocap.set_pose(data, pose)
def _set_object_state(
self,
dof_pos: np.ndarray,
dof_vel: np.ndarray,
target_x: np.ndarray,
target_z: np.ndarray,
target_angle: np.ndarray,
grasp_pos: np.ndarray,
):
cfg = self._cfg
num = dof_pos.shape[0]
# Default: uniform in workspace.
object_x = np.random.uniform(cfg.object_x_range[0], cfg.object_x_range[1], size=(num,)).astype(np.float32)
object_z = np.random.uniform(cfg.object_z_range[0], cfg.object_z_range[1], size=(num,)).astype(np.float32)
object_angle = np.random.uniform(cfg.object_angle_range[0], cfg.object_angle_range[1], size=(num,)).astype(
np.float32
)
# dm_control-style object init distribution.
r = np.random.uniform(0.0, 1.0, size=(num,)).astype(np.float32)
in_hand = r < float(cfg.p_in_hand)
in_target = (r >= float(cfg.p_in_hand)) & (r < float(cfg.p_in_hand + cfg.p_in_target))
uniform = ~(in_hand | in_target)
# Avoid initializing the object too close to the hand to prevent interpenetration / impulse explosions.
min_dist = float(getattr(cfg, "min_object_hand_dist", 0.0))
if min_dist > 0.0 and uniform.any():
min_dist_sq = np.float32(min_dist * min_dist)
max_attempts = 50
pending = uniform.copy()
for _ in range(max_attempts):
if not pending.any():
break
dx = object_x - grasp_pos[:, 0]
dz = object_z - grasp_pos[:, 2]
too_close = (dx * dx + dz * dz) < min_dist_sq
pending = pending & too_close
if not pending.any():
break
n = int(pending.sum())
object_x[pending] = np.random.uniform(cfg.object_x_range[0], cfg.object_x_range[1], size=(n,)).astype(
np.float32
)
object_z[pending] = np.random.uniform(cfg.object_z_range[0], cfg.object_z_range[1], size=(n,)).astype(
np.float32
)
object_x[in_target] = target_x[in_target]
object_z[in_target] = target_z[in_target]
object_angle[in_target] = target_angle[in_target]
object_x[in_hand] = grasp_pos[in_hand, 0]
object_z[in_hand] = grasp_pos[in_hand, 2]
object_angle[in_hand] = 0.0
dof_pos[:, self._object_qpos_indices] = np.stack([object_x, object_z, object_angle], axis=-1)
dof_vel[:, self._object_qvel_indices] = 0.0
if uniform.any():
dof_vel[uniform, self._object_x_qvel_i] = np.random.uniform(
cfg.object_x_vel_range[0], cfg.object_x_vel_range[1], size=(int(uniform.sum()),)
).astype(np.float32)
def _settle(self, data: mtx.SceneData):
control_steps = int(self._cfg.settle_steps)
if control_steps <= 0:
return
substeps = int(self._cfg.sim_substeps)
physics_steps = control_steps * max(substeps, 1)
data.actuator_ctrls = np.zeros((data.shape[0], self._model.num_actuators), dtype=np.float32)
for _ in range(physics_steps):
self._model.step(data)
if self._cfg.settle_zero_vel:
data.set_dof_vel(np.zeros((data.shape[0], self._model.num_dof_vel), dtype=np.float32))
self._model.forward_kinematic(data)
def initialize_episode(self, data: mtx.SceneData) -> None:
"""Episode initialization with optional physics settling (dm_control-style)."""
num = int(data.shape[0])
dof_pos = np.tile(self._init_dof_pos, (num, 1))
dof_vel = np.tile(self._init_dof_vel, (num, 1))
# Optionally randomize arm joint angles and symmetrize the hand.
if getattr(self._cfg, "randomize_arm", True):
arm_angles = self._sample_arm_joint_angles(num)
dof_pos[:, self._arm_joint_pos_indices] = arm_angles
dof_pos[:, self._finger_qpos_i] = dof_pos[:, self._thumb_qpos_i]
dof_pos[:, self._fingertip_qpos_i] = dof_pos[:, self._thumbtip_qpos_i]
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
target_x, target_z, target_angle = self._sample_target_pose(num)
self._set_target_mocap(data, target_x, target_z, target_angle)
self._model.forward_kinematic(data)
grasp_pos = self._grasp_site.get_position(data)
self._set_object_state(dof_pos, dof_vel, target_x, target_z, target_angle, grasp_pos)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
arm_qpos = data.dof_pos[:, self._arm_joint_pos_indices].copy()
self._settle(data)
if not getattr(self._cfg, "randomize_arm", True):
dof_pos_after = data.dof_pos.copy()
dof_vel_after = data.dof_vel.copy()
dof_pos_after[:, self._arm_joint_pos_indices] = arm_qpos
dof_vel_after[:, self._arm_joint_vel_indices] = 0.0
data.set_dof_pos(dof_pos_after, self._model)
data.set_dof_vel(dof_vel_after)
self._model.forward_kinematic(data)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
num = int(data.shape[0])
self.initialize_episode(data)
obs = self._get_obs(data)
info = {
"actions": np.zeros((num, self._model.num_actuators), dtype=np.float32),
"last_actions": np.zeros((num, self._model.num_actuators), dtype=np.float32),
}
return obs, info
@registry.env("dm-manipulator-bring-ball", "np")
class BringBall(ManipulatorBase):
_cfg: BringBallCfg
def __init__(self, cfg: BringBallCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
# 1. Sensors setup
self._fingertip_site = self._model.get_site("fingertip_touch")
self._thumbtip_site = self._model.get_site("thumbtip_touch")
self._touch_idx_palm = _TOUCH_SENSORS.index("palm_touch")
self._touch_idx_fingertip = _TOUCH_SENSORS.index("fingertip_touch")
self._touch_idx_thumbtip = _TOUCH_SENSORS.index("thumbtip_touch")
def _compute_hand_direction(self, data: mtx.SceneData) -> np.ndarray:
"""Calculates the Z-axis vector of the hand (grasp site)."""
grasp_pose = self._grasp_site.get_pose(data)
return _quat_to_z_axis(grasp_pose[:, 3:])
def _get_tip_positions(self, data: mtx.SceneData) -> tuple[np.ndarray, np.ndarray]:
fingertip_pos = self._fingertip_site.get_position(data).astype(np.float32)
thumbtip_pos = self._thumbtip_site.get_position(data).astype(np.float32)
return fingertip_pos, thumbtip_pos
def _compute_aim_direction(self, object_pos: np.ndarray, grasp_pos: np.ndarray) -> np.ndarray:
vec_to_aim = object_pos - grasp_pos
dist_to_aim = np.linalg.norm(vec_to_aim, axis=-1, keepdims=True)
return vec_to_aim / (dist_to_aim + 1e-6)
def _strict_grasp_condition(self, data: mtx.SceneData, object_pos: np.ndarray) -> np.ndarray:
cfg = self._cfg
height_ok = object_pos[:, 2] > float(cfg.lift_height_threshold)
all_touch = self._touch_raw(data)
touch_threshold = float(cfg.touch_threshold)
touch_ok = (
(all_touch[..., self._touch_idx_palm] > touch_threshold)
| (all_touch[..., self._touch_idx_fingertip] > touch_threshold)
| (all_touch[..., self._touch_idx_thumbtip] > touch_threshold)
)
object_contact_ok = self._contact_with_object(data)
return height_ok & touch_ok & object_contact_ok
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
cfg = self._cfg
# 1. Observation
obs = self._get_obs(data)
terminated = np.isnan(obs).any(axis=-1)
# 2. Positions
object_pos = self._object_pos(data)
target_pos = self._target_pos(data)
grasp_pos = self._hand_pos(data)
# 3. Kinematics
fingertip_pos, thumbtip_pos = self._get_tip_positions(data)
dist_finger = np.linalg.norm(fingertip_pos - object_pos, axis=-1)
dist_thumb = np.linalg.norm(thumbtip_pos - object_pos, axis=-1)
avg_tip_dist = ((dist_finger + dist_thumb) / 2.0).astype(np.float32)
move_dist = np.linalg.norm(object_pos - target_pos, axis=-1).astype(np.float32)
# 4. Dynamics
arm_vel = data.dof_vel[:, self._arm_joint_vel_indices[:4]].astype(np.float32)
arm_speed = np.linalg.norm(arm_vel, axis=-1).astype(np.float32)
arm_speed_step = (arm_speed * float(cfg.ctrl_dt)).astype(np.float32)
# 5. Logic Checks
is_grasped = self._strict_grasp_condition(data, object_pos)
contact_with_obj = self._contact_with_object(data)
hover_threshold = float(cfg.hover_close_threshold)
is_close_to_ball = (avg_tip_dist < hover_threshold).astype(np.float32)
grasp_mask = is_grasped.astype(np.float32)
post_grasp_scale = 1.0 - grasp_mask * float(cfg.post_grasp_discount)
# --- Rewards ---
# R1: Reach
r_reach = _tolerance(avg_tip_dist, bounds=(0.0, 0.02), margin=0.25, sigmoid="linear").astype(np.float32)
r_reach = (r_reach * post_grasp_scale).astype(np.float32)
# R2: Orient
hand_dir = self._compute_hand_direction(data)
unit_vec_to_aim = self._compute_aim_direction(object_pos, grasp_pos)
pointing_dot = np.sum(hand_dir * unit_vec_to_aim, axis=-1)
# Dynamic tolerance
dist_from_base = np.linalg.norm(object_pos[:, :2], axis=-1)
orient_bound_lower = 0.95 * np.clip(dist_from_base / 0.5, 0.0, 1.0)
r_orient_raw = 1.0 - orient_bound_lower + pointing_dot
r_orient = np.clip(r_orient_raw, 0.0, 1.0).astype(np.float32)
r_orient = (r_orient * post_grasp_scale).astype(np.float32)
# R3: Pause
r_pause = (
_tolerance(arm_speed_step, bounds=(0.0, 0.05), margin=0.3, sigmoid="linear").astype(np.float32)
* is_close_to_ball
)
r_pause = (r_pause * post_grasp_scale).astype(np.float32)
# R4: Close
default_actions = np.zeros((data.shape[0], self._model.num_actuators), dtype=np.float32)
grasp_action = state.info.get("actions", default_actions)[:, self._grasp_act_i].astype(np.float32)
r_close_intent = _tolerance(
grasp_action, bounds=(0.8, 1.0), margin=1.0, sigmoid="linear", value_at_margin=0.01
).astype(np.float32)
r_approach_grasp = r_close_intent * is_close_to_ball * r_orient * r_pause * contact_with_obj.astype(np.float32)
r_sustain_grasp = r_close_intent * grasp_mask
r_close = (r_approach_grasp * (1.0 - grasp_mask) + r_sustain_grasp).astype(np.float32)
# R5: Lift & Transport
lift_h = float(cfg.lift_height_threshold)
ball_z = object_pos[:, 2].astype(np.float32)
r_lift_height = (
_tolerance(ball_z, bounds=(lift_h, lift_h + 0.15), margin=0.02, sigmoid="linear", value_at_margin=0.01)
* grasp_mask
).astype(np.float32)
r_transport = (_tolerance(move_dist, bounds=(0.0, 0.01), margin=0.3, sigmoid="linear") * grasp_mask).astype(
np.float32
)
r_precision = (
_tolerance(
move_dist,
bounds=(0.0, 0.0),
margin=float(cfg.precision_margin),
sigmoid="gaussian",
value_at_margin=float(cfg.precision_value_at_margin),
)
* grasp_mask
).astype(np.float32)
lift_height_weight = float(cfg.lift_height_weight)
transport_weight = float(cfg.transport_weight)
lift_norm = max(lift_height_weight + transport_weight, 1e-6)
r_lift = ((lift_height_weight * r_lift_height + transport_weight * r_transport) / lift_norm).astype(np.float32)
prev_move_dist = state.info.get("prev_move_dist")
if prev_move_dist is None:
prev_move_dist = move_dist
else:
prev_move_dist = np.asarray(prev_move_dist, dtype=np.float32)
if "steps" in state.info:
first_step = state.info["steps"] == 0
prev_move_dist = np.where(first_step, move_dist, prev_move_dist)
progress_clip = float(cfg.transport_progress_clip)
progress = (prev_move_dist - move_dist) / max(progress_clip, 1e-6)
progress = np.clip(progress, -1.0, 1.0).astype(np.float32)
r_progress = (progress * float(cfg.transport_progress_scale) * grasp_mask).astype(np.float32)
state.info["prev_move_dist"] = move_dist.astype(np.float32)
# --- Penalties ---
all_touch = self._touch_raw(data)
side_touch_sum = (all_touch[..., 1] + all_touch[..., 2]).astype(np.float32)
penalty_side = (
-float(cfg.side_penalty_scale) * np.tanh(side_touch_sum * float(cfg.side_penalty_tanh_scale))
).astype(np.float32)
hover_phase = (is_close_to_ball > 0.5) & (~contact_with_obj)
penalty_hover = (-float(cfg.hover_penalty_scale) * hover_phase.astype(np.float32)).astype(np.float32)
# --- Total ---
reach_w = float(cfg.reach_weight)
orient_w = float(cfg.orient_weight)
pause_w = float(cfg.pause_weight)
close_w = float(cfg.close_weight)
lift_w = float(cfg.lift_reward_weight)
precision_w = float(cfg.precision_weight)
weight_sum = max(reach_w + orient_w + pause_w + close_w + lift_w + precision_w, 1e-6)
reward = (
(
reach_w * r_reach
+ orient_w * r_orient
+ pause_w * r_pause
+ close_w * r_close
+ lift_w * r_lift
+ precision_w * r_precision
)
/ weight_sum
+ penalty_side
+ penalty_hover
+ r_progress
).astype(np.float32)
reward = np.where(terminated, 0.0, reward)
state.info["Reward"] = {
"reach": r_reach,
"orient": r_orient,
"close": r_close,
"lift": r_lift,
"transport": r_transport,
"precision": r_precision,
"progress": r_progress,
"total": reward,
}
state.info["metrics"] = {
"pointing_dot": pointing_dot,
"is_grasped": is_grasped.astype(np.float32),
"avg_tip_dist": avg_tip_dist,
"move_dist": move_dist,
"transport_reward": r_transport,
"precision_reward": r_precision,
"progress_reward": r_progress,
}
return state.replace(obs=obs, reward=reward, terminated=terminated)

16
motrix_envs/src/motrix_envs/basic/pendulum/__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 pendulum_np # noqa: F401

39
motrix_envs/src/motrix_envs/basic/pendulum/cfg.py Normal file
View File

@@ -0,0 +1,39 @@
# 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
import numpy as np
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/pendulum.xml"
@registry.envcfg("pendulum")
@dataclass
class PendulumEnvCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 10.0
sim_dt: float = 0.0125
ctrl_dt: float = 0.025
angle_bound: float = 8.0
cosing_bound: float = 0.0
# reset_noise_scale: float = 0.01
def __post_init__(self):
self.cosing_bound = float(np.cos(np.deg2rad(self.angle_bound)))

35
motrix_envs/src/motrix_envs/basic/pendulum/pendulum.xml Normal file
View File

@@ -0,0 +1,35 @@
<mujoco model="pendulum">
<include file="../../common/visual.xml" />
<include file="../../common/skybox.xml" />
<include file="../../common/materials.xml" />
<asset>
<texture name="motphys_ground_tex" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys_ground_mat" texture="motphys_ground_tex" texrepeat="1 1" texuniform="true" />
</asset>
<option timestep="0.02">
<flag contact="disable" energy="enable"/>
</option>
<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.2" type="plane" material="motphys_ground_mat"/>
<camera name="fixed" pos="0 -1.5 2" xyaxes='1 0 0 0 1 1'/>
<camera name="lookat" mode="targetbodycom" target="pole" pos="0 -2 1"/>
<body name="pole" pos="0 0 .6">
<joint name="hinge" type="hinge" axis="0 1 0" damping="0.1"/>
<geom name="base" material="decoration" type="cylinder" fromto="0 -.03 0 0 .03 0" size="0.021" mass="0"/>
<geom name="pole" material="self" type="capsule" fromto="0 0 0 0 0 0.5" size="0.02" mass="0"/>
<geom name="mass" material="effector" type="sphere" pos="0 0 0.5" size="0.05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor name="torque" joint="hinge" gear="5" ctrlrange="-1 1" ctrllimited="true"/>
</actuator>
</mujoco>

117
motrix_envs/src/motrix_envs/basic/pendulum/pendulum_np.py Normal file
View File

@@ -0,0 +1,117 @@
# 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 import reward as reward_utils
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import PendulumEnvCfg
@registry.env("pendulum", "np")
class PendulumEnv(NpEnv):
_cfg: PendulumEnvCfg
def __init__(self, cfg: PendulumEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
ctrl_limits = self._model.actuator_ctrl_limits
self._action_low = float(ctrl_limits[0, 0])
self._action_high = float(ctrl_limits[1, 0])
self._action_space = gym.spaces.Box(-1.0, 1.0, (1,), dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (3,), dtype=np.float32)
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._init_dof_pos = self._model.compute_init_dof_pos()
self._init_dof_vel = np.zeros(
(self._model.num_dof_vel,),
dtype=np.float32,
)
@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)
scaled = self._action_low + (actions + 1.0) * 0.5 * (self._action_high - self._action_low)
state.data.actuator_ctrls = scaled
return state
def update_state(self, state: NpEnvState):
# compute observation
data = state.data
dof_pos = data.dof_pos
dof_vel = data.dof_vel
angle = dof_pos[:, 0]
ang_vel = dof_vel[:, 0]
obs = np.stack([np.cos(angle), np.sin(angle), ang_vel], axis=-1)
assert obs.shape == (self._num_envs, 3)
# compute reward
angle_wrapped = (angle + np.pi) % (2 * np.pi) - np.pi
ctrl = data.actuator_ctrls[:, 0]
# In this model, zero angle corresponds to the hanging-down position.
# Shift the target by pi to encourage the upright (inverted) posture.
upright = (1.0 + np.cos(angle_wrapped)) * 0.5
prev_ctrl = state.info.get("prev_ctrl", np.zeros_like(ctrl))
ctrl_delta = ctrl - prev_ctrl
vel_penalty = 0.2 * (ang_vel**2)
energy = 0.5 * ang_vel**2 + (1.0 - np.cos(angle_wrapped))
energy_target = 2.0
energy_reward = reward_utils.tolerance(
energy,
bounds=(energy_target, energy_target),
margin=2.0,
value_at_margin=0.1,
sigmoid="gaussian",
)
reward = (3.0 * upright + energy_reward - vel_penalty - 0.001 * ctrl**2 - 0.001 * ctrl_delta**2).astype(
np.float32
)
# compute terminated
terminated = np.isnan(obs).any(axis=-1)
state.obs = obs
state.reward = reward
state.terminated = terminated
state.info["prev_ctrl"] = ctrl
return state
def reset(self, data: mtx.SceneData):
cfg: PendulumEnvCfg = self._cfg
reset_noise_scale = getattr(cfg, "reset_noise_scale", 0.0)
num_reset = data.shape[0]
dof_pos = np.zeros((num_reset, self._num_dof_pos), dtype=np.float32)
dof_vel = np.zeros((num_reset, self._num_dof_vel), dtype=np.float32)
dof_pos[:, 0] = np.random.uniform(-np.pi, np.pi, size=(num_reset,))
if reset_noise_scale > 0.0:
dof_vel[:, 0] = np.random.uniform(-reset_noise_scale, reset_noise_scale, size=(num_reset,))
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
angle = dof_pos[:, 0]
ang_vel = dof_vel[:, 0]
obs = np.stack([np.cos(angle), np.sin(angle), ang_vel], axis=-1)
return obs, {"prev_ctrl": np.zeros((num_reset,), dtype=np.float32)}

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

@@ -19,7 +19,7 @@ import motrixsim as mtx
import numpy as np import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.math.quaternion import Quaternion from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import AnymalCEnvCfg from .cfg import AnymalCEnvCfg
@@ -185,7 +185,7 @@ class AnymalCEnv(NpEnv):
# Get commands - convert to relative velocity commands # Get commands - convert to relative velocity commands
pose_commands = state.info["pose_commands"] pose_commands = state.info["pose_commands"]
robot_position = root_pos[:, :2] robot_position = root_pos[:, :2]
robot_heading = Quaternion.get_yaw(root_quat) robot_heading = quaternion.get_yaw(root_quat)
target_position = pose_commands[:, :2] target_position = pose_commands[:, :2]
target_heading = pose_commands[:, 2] target_heading = pose_commands[:, 2]
@@ -295,14 +295,14 @@ class AnymalCEnv(NpEnv):
) )
robot_arrow_pos = robot_pos.copy() robot_arrow_pos = robot_pos.copy()
robot_arrow_pos[:, 2] = arrow_height robot_arrow_pos[:, 2] = arrow_height
robot_arrow_quat = Quaternion.from_euler(0, 0, cur_yaw) robot_arrow_quat = quaternion.from_euler(0, 0, cur_yaw)
mocap = self._model.get_body("robot_heading_arrow").mocap mocap = self._model.get_body("robot_heading_arrow").mocap
mocap.set_pose(data, np.concatenate([robot_arrow_pos, robot_arrow_quat], axis=1)) mocap.set_pose(data, np.concatenate([robot_arrow_pos, robot_arrow_quat], axis=1))
des_yaw = np.where( des_yaw = np.where(
np.linalg.norm(desired_vel_xy, axis=1) > 1e-6, np.arctan2(desired_vel_xy[:, 1], desired_vel_xy[:, 0]), 0.0 np.linalg.norm(desired_vel_xy, axis=1) > 1e-6, np.arctan2(desired_vel_xy[:, 1], desired_vel_xy[:, 0]), 0.0
) )
desired_arrow_quat = Quaternion.from_euler(0, 0, des_yaw) desired_arrow_quat = quaternion.from_euler(0, 0, des_yaw)
mocap = self._model.get_body("desired_heading_arrow").mocap mocap = self._model.get_body("desired_heading_arrow").mocap
mocap.set_pose(data, np.concatenate([robot_arrow_pos, desired_arrow_quat], axis=1)) mocap.set_pose(data, np.concatenate([robot_arrow_pos, desired_arrow_quat], axis=1))
@@ -350,7 +350,7 @@ class AnymalCEnv(NpEnv):
# Get robot position and heading for arrival determination # Get robot position and heading for arrival determination
robot_position = pose[:, :2] robot_position = pose[:, :2]
robot_heading = Quaternion.get_yaw(root_quat) robot_heading = quaternion.get_yaw(root_quat)
target_position = info["pose_commands"][:, :2] target_position = info["pose_commands"][:, :2]
target_heading = info["pose_commands"][:, 2] target_heading = info["pose_commands"][:, 2]
position_error = target_position - robot_position position_error = target_position - robot_position
@@ -454,7 +454,7 @@ class AnymalCEnv(NpEnv):
arrow_pos = pose_commands.copy() arrow_pos = pose_commands.copy()
arrow_pos[:, 2] = 0.05 arrow_pos[:, 2] = 0.05
arrow_pos = np.column_stack([pose_commands[:, 0], pose_commands[:, 1], np.full((num_envs, 1), 0.5)]) arrow_pos = np.column_stack([pose_commands[:, 0], pose_commands[:, 1], np.full((num_envs, 1), 0.5)])
arrow_quat = Quaternion.from_euler(0, 0, pose_commands[:, 2]) arrow_quat = quaternion.from_euler(0, 0, pose_commands[:, 2])
mocap = self._model.get_body("target_marker").mocap mocap = self._model.get_body("target_marker").mocap
mocap.set_pose(data, np.concatenate([arrow_pos, arrow_quat], axis=1)) mocap.set_pose(data, np.concatenate([arrow_pos, arrow_quat], axis=1))
@@ -490,7 +490,7 @@ class AnymalCEnv(NpEnv):
return state.replace(terminated=terminated) return state.replace(terminated=terminated)
def reset(self, data: mtx.SceneData, done: np.ndarray = None) -> tuple[np.ndarray, dict]: def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
cfg: AnymalCEnvCfg = self._cfg cfg: AnymalCEnvCfg = self._cfg
num_envs = data.shape[0] num_envs = data.shape[0]
@@ -563,7 +563,7 @@ class AnymalCEnv(NpEnv):
# Calculate velocity commands (consistent with update_state) # Calculate velocity commands (consistent with update_state)
robot_position = root_pos[:, :2] robot_position = root_pos[:, :2]
robot_heading = Quaternion.get_yaw(root_quat) robot_heading = quaternion.get_yaw(root_quat)
target_position = pose_commands[:, :2] target_position = pose_commands[:, :2]
target_heading = pose_commands[:, 2] target_heading = pose_commands[:, 2]
@@ -651,4 +651,4 @@ class AnymalCEnv(NpEnv):
def _compute_projected_gravity(self, quat: np.ndarray) -> np.ndarray: def _compute_projected_gravity(self, quat: np.ndarray) -> np.ndarray:
gravity = np.array([0.0, 0.0, -1.0], dtype=np.float32) gravity = np.array([0.0, 0.0, -1.0], dtype=np.float32)
return Quaternion.rotate_vector(quat, gravity) return quaternion.rotate_vector(quat, gravity)

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

@@ -19,7 +19,7 @@ import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpEnvCfg from motrix_envs.locomotion.go1.cfg import Go1WalkNpEnvCfg
from motrix_envs.math.quaternion import Quaternion from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
@@ -190,7 +190,7 @@ class Go1WalkTask(NpEnv):
gyro = self.get_gyro(data) gyro = self.get_gyro(data)
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) local_gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel noisy_gyro = gyro * self.cfg.normalization.ang_vel
@@ -320,7 +320,7 @@ class Go1WalkTask(NpEnv):
# Penalize non flat base orientation # Penalize non flat base orientation
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1) return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData): def _reward_torques(self, data: mtx.SceneData):

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

@@ -19,7 +19,7 @@ import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpRoughEnvCfg from motrix_envs.locomotion.go1.cfg import Go1WalkNpRoughEnvCfg
from motrix_envs.math.quaternion import Quaternion from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
from .common import generate_repeating_array from .common import generate_repeating_array
@@ -44,18 +44,15 @@ class Go1WalkRoughTask(NpEnv):
(self._num_dof_vel,), (self._num_dof_vel,),
dtype=np.float32, dtype=np.float32,
) )
self.height_list = np.array([-2.5, 0.5, 2.0])
offset_h = [[2, 2, 1, 1, 1], [2, 2, 1, 1, 2], [2, 1, 1, 1, 1], [2, 1, 1, 1, 1], [2, 1, 1, 1, 1]] go1_init_height = 0.3
offset = [] self.reset_offset = self._generate_init_offsets(go1_init_height)
for i in range(5):
for j in range(5):
h_index = offset_h[j][i]
offset.append([(i - 2) * 8.0, (j - 2) * 8.0, self.height_list[h_index]])
self.offset_list = np.array(offset)
self._init_dof_pos = self._model.compute_init_dof_pos() self._init_dof_pos = self._model.compute_init_dof_pos()
self._init_dof_pos[2] = self.height_list[0] geom_floor_pos = self._model.get_geom("floor").local_pose[:3]
geom_floor_pos[2] += go1_init_height
self._init_dof_pos[:3] = geom_floor_pos
self._init_buffer() self._init_buffer()
self.height_counter = 0 self.reset_counter = 0
def _init_obs_space(self): def _init_obs_space(self):
model = self.model model = self.model
@@ -216,7 +213,7 @@ class Go1WalkRoughTask(NpEnv):
gyro = self.get_gyro(data) gyro = self.get_gyro(data)
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) local_gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel noisy_gyro = gyro * self.cfg.normalization.ang_vel
@@ -309,9 +306,9 @@ class Go1WalkRoughTask(NpEnv):
if self.training_level == 1: if self.training_level == 1:
num_period = 25 num_period = 25
idx = generate_repeating_array(num_period, num_reset, self.height_counter) idx = generate_repeating_array(num_period, num_reset, self.reset_counter)
self.height_counter = (self.height_counter + num_reset) % num_period self.reset_counter = (self.reset_counter + num_reset) % num_period
dof_pos[:, :3] = self.offset_list[idx] dof_pos[:, :3] = self.reset_offset[idx]
data.reset(self._model) data.reset(self._model)
data.set_dof_vel(dof_vel) data.set_dof_vel(dof_vel)
@@ -364,7 +361,7 @@ class Go1WalkRoughTask(NpEnv):
# Penalize non flat base orientation # Penalize non flat base orientation
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1) return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData): def _reward_torques(self, data: mtx.SceneData):
@@ -433,5 +430,48 @@ class Go1WalkRoughTask(NpEnv):
# check whether the robot reaching into the terrain border and change the move direction # check whether the robot reaching into the terrain border and change the move direction
border_size = 19.0 border_size = 19.0
position = self._body.get_position(data) position = self._body.get_position(data)
is_out = (np.square(position[:, :2]) > border_size**2).any(axis=1) geom_floor = self._model.get_geom("floor")
geom_floor_rough = self._model.get_geom("floor_rough")
in_rough_aera = self._is_in_area(position, border_size, border_size, geom_floor_rough.local_pose[:2])
in_flat_aera = self._is_in_area(position, border_size, border_size, geom_floor.local_pose[:2])
is_out = ~(in_rough_aera | in_flat_aera)
info["commands"][is_out] = [0, 0, 0] info["commands"][is_out] = [0, 0, 0]
def _is_in_area(self, pos, length, width, offset):
x = pos[:, 0]
y = pos[:, 1]
return (
((offset[0] - length) < x)
& ((offset[0] + length) > x)
& ((offset[1] - width) < y)
& ((offset[1] + width) > y)
)
def _generate_init_offsets(self, init_height: float, grid_size: tuple[int, int] = (5, 5)) -> np.ndarray:
"""Generate initialization offsets for rough terrain training."""
hfield = self._model.get_geom("floor_rough").hfield
nx, ny = grid_size
# Vectorized grid generation
idx_x, idx_y = np.meshgrid(np.arange(nx), np.arange(ny, dtype=np.float32), indexing="ij")
idx_x, idx_y = idx_x.flatten(), idx_y.flatten()
# Map to heightfield indices and get heights
hfield_idx_x = (hfield.ncol * idx_x // nx).astype(int)
hfield_idx_y = (hfield.nrow * idx_y // ny).astype(int)
heights = np.array([hfield.get(iy, ix) for ix, iy in zip(hfield_idx_x, hfield_idx_y)])
# Calculate positions
grid_len_x = (hfield.bound[3] - hfield.bound[0]) / nx
grid_len_y = (hfield.bound[4] - hfield.bound[1]) / ny
center_offset = np.array([grid_len_x / 2, grid_len_y / 2])
offsets = np.column_stack(
[
hfield.bound[0] + grid_len_x * idx_x + center_offset[0],
hfield.bound[4] - grid_len_y * idx_y - center_offset[1],
heights + init_height,
]
).astype(np.float32)
return offsets

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

@@ -19,7 +19,7 @@ import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpStairsEnvCfg from motrix_envs.locomotion.go1.cfg import Go1WalkNpStairsEnvCfg
from motrix_envs.math.quaternion import Quaternion from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
from .common import generate_repeating_array from .common import generate_repeating_array
@@ -215,7 +215,7 @@ class Go1WalkStairsTask(NpEnv):
gyro = self.get_gyro(data) gyro = self.get_gyro(data)
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) local_gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel noisy_gyro = gyro * self.cfg.normalization.ang_vel
@@ -277,7 +277,7 @@ class Go1WalkStairsTask(NpEnv):
force = [] force = []
for foot in self.cfg.sensor.feet: for foot in self.cfg.sensor.feet:
contact_force = self._model.get_sensor_value(foot + "_foot_contact", data) contact_force = self._model.get_sensor_value(foot + "_foot_contact", data)
contact_force = Quaternion.rotate_inverse(base_quat, contact_force) contact_force = quaternion.rotate_inverse(base_quat, contact_force)
force.append(contact_force) force.append(contact_force)
return np.concatenate(force, axis=1) return np.concatenate(force, axis=1)
@@ -370,7 +370,7 @@ class Go1WalkStairsTask(NpEnv):
# Penalize non flat base orientation # Penalize non flat base orientation
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec) gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1) return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData): def _reward_torques(self, data: mtx.SceneData):

7
motrix_envs/src/motrix_envs/locomotion/go1/xmls/scene_rough_terrain.xml
View File

@@ -14,13 +14,14 @@
<asset> <asset>
<hfield name="hfield" file="assets/heightmap.png" size="20 20 2.5 0.1" /> <hfield name="hfield" file="assets/heightmap.png" size="20 20 2.5 0.1" />
<hfield name="hfield-plane" nrow="2" ncol="2" elevation="0 0 0 0" size="20 20 1.0 0.1" />
</asset> </asset>
<worldbody> <worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" /> <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" pos="0 0 -3" size="0 0 0.01" type="plane" material="motphys-ground" <geom name="floor_rough" pos="0 0 0" type="hfield" hfield="hfield" material="motphys-ground"
contype="1" conaffinity="0" priority="1" friction="0.6" condim="3" /> contype="1" conaffinity="0" priority="1" friction="0.6" />
<geom name="floor_rough" pos="0 0 -1" type="hfield" hfield="hfield" material="motphys-ground" <geom name="floor" pos="0 40 0" type="hfield" hfield="hfield-plane" material="motphys-ground"
contype="1" conaffinity="0" priority="1" friction="0.6" /> contype="1" conaffinity="0" priority="1" friction="0.6" />
</worldbody> </worldbody>
</mujoco> </mujoco>

37
motrix_envs/src/motrix_envs/locomotion/go2/cfg.py
View File

@@ -41,22 +41,22 @@ class ControlConfig:
@dataclass @dataclass
class InitState: class InitState:
# the initial position of the robot in the world frame # the initial position of the robot in the world frame
pos = [0.0, 0.0, 0.42] #0.278 pos = [0.0, 0.0, 0.42] # 0.278
# the default angles for all joints. key = joint name, value = target angle [rad] # the default angles for all joints. key = joint name, value = target angle [rad]
default_joint_angles = { default_joint_angles = {
"FL_hip" : 0.1, # [rad] "FL_hip": 0.1, # [rad]
"FL_thigh" : 0.9, # [rad] "FL_thigh": 0.9, # [rad]
"FL_calf" : -1.8, # [rad] "FL_calf": -1.8, # [rad]
"FR_hip" : -0.1, # [rad] "FR_hip": -0.1, # [rad]
"FR_thigh" : 0.9, # [rad] "FR_thigh": 0.9, # [rad]
"FR_calf" : -1.8, # [rad] "FR_calf": -1.8, # [rad]
"RL_hip" : 0.1, # [rad] "RL_hip": 0.1, # [rad]
"RL_thigh" : 0.9, # [rad] "RL_thigh": 0.9, # [rad]
"RL_calf" : -1.8, # [rad] "RL_calf": -1.8, # [rad]
"RR_hip" : -0.1, # [rad] "RR_hip": -0.1, # [rad]
"RR_thigh" : 0.9, # [rad] "RR_thigh": 0.9, # [rad]
"RR_calf" : -1.8, # [rad] "RR_calf": -1.8, # [rad]
} }
@@ -64,7 +64,7 @@ class InitState:
class Commands: class Commands:
vel_limit = [ vel_limit = [
[-2.0, -1.0, -3.1416], # min: vel_x [m/s], vel_y [m/s], ang_vel [rad/s] [-2.0, -1.0, -3.1416], # min: vel_x [m/s], vel_y [m/s], ang_vel [rad/s]
[ 2.0, 1.0, 3.1416], # max [2.0, 1.0, 3.1416], # max
] ]
@@ -82,8 +82,13 @@ class Asset:
foot_name = "foot" foot_name = "foot"
penalize_contacts_on = ["thigh", "calf"] penalize_contacts_on = ["thigh", "calf"]
terminate_after_contacts_on = [ terminate_after_contacts_on = [
"base_collision_0", "base_collision_1", "base_collision_2", "base_collision_0",
"fl_hip_0", "fr_hip_0", "rl_hip_0", "rr_hip_0", "base_collision_1",
"base_collision_2",
"fl_hip_0",
"fr_hip_0",
"rl_hip_0",
"rr_hip_0",
] ]
ground = "floor" ground = "floor"

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

@@ -19,41 +19,10 @@ import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.locomotion.go2.cfg import Go2WalkNpEnvCfg from motrix_envs.locomotion.go2.cfg import Go2WalkNpEnvCfg
from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
## provide quat math utility from motrixsim.
def quat_rotate_inverse(quats, v):
"""
Rotate a fixed vector v by a list of quaternions using a vectorized approach.
Parameters:
quats (np.ndarray): Array of quaternions of shape (N, 4). Each quaternion is in [w, x, y, z] format.
v (np.ndarray): Fixed vector of shape (3,) to be rotated.
Returns:
np.ndarray: Array of rotated vectors of shape (N, 3).
"""
# Normalize the quaternions to ensure they are unit quaternions
# Extract the scalar (w) and vector (x, y, z) parts of the quaternions
w = quats[:, -1] # Shape (N,)
im = quats[:, :3] # Shape (N, 3)
# Compute the cross product between the imaginary part of each quaternion and the fixed vector v.
# np.cross broadcasts v to match each row in im, resulting in an array of shape (N, 3)
cross_im_v = np.cross(im, v)
# Compute the intermediate terms for the rotation formula:
term1 = w[:, np.newaxis] * cross_im_v # w * cross(im, v)
term2 = np.cross(im, cross_im_v) # cross(im, cross(im, v))
# Apply the rotation formula: v_rot = v + 2 * (term1 + term2)
v_rotated = v + 2 * (term1 + term2)
return v_rotated
@registry.env("go2-flat-terrain-walk", sim_backend="np") @registry.env("go2-flat-terrain-walk", sim_backend="np")
class Go2WalkTask(NpEnv): class Go2WalkTask(NpEnv):
_init_dof_pos: np.ndarray _init_dof_pos: np.ndarray
@@ -141,9 +110,7 @@ class Go2WalkTask(NpEnv):
self.hip_indices.append(i) self.hip_indices.append(i)
if "calf" in self._model.actuator_names[i]: if "calf" in self._model.actuator_names[i]:
self.calf_indices.append(i) self.calf_indices.append(i)
print("Default joint angles:", self.default_angles)
print("Actuator names:", self._model.actuator_names)
self._init_dof_pos[-self._num_action :] = self.default_angles self._init_dof_pos[-self._num_action :] = self.default_angles
self.ground = self._model.get_geom_index(cfg.asset.ground) self.ground = self._model.get_geom_index(cfg.asset.ground)
@@ -213,7 +180,7 @@ class Go2WalkTask(NpEnv):
gyro = self.get_gyro(data) gyro = self.get_gyro(data)
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
local_gravity = quat_rotate_inverse(base_quat, self.gravity_vec) local_gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel noisy_gyro = gyro * self.cfg.normalization.ang_vel
@@ -343,7 +310,7 @@ class Go2WalkTask(NpEnv):
# Penalize non flat base orientation # Penalize non flat base orientation
pose = self._body.get_pose(data) pose = self._body.get_pose(data)
base_quat = pose[:, 3:7] base_quat = pose[:, 3:7]
gravity = quat_rotate_inverse(base_quat, self.gravity_vec) gravity = quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1) return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData): def _reward_torques(self, data: mtx.SceneData):

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

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

8
motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py
View File

@@ -240,11 +240,11 @@ class FrankaLiftCubeEnv(NpEnv):
## action penalty rate ## action penalty rate
reach_weight = 1.5 # Cannot be too small reach_weight = 1.5 # Cannot be too small
cmd_tracking_weight = 10 cmd_tracking_weight = 10.0
cmd_tracking_fine_graind_weight = 20 # Should be larger, need strong pull to target area cmd_tracking_fine_graind_weight = 20.0 # Should be larger, need strong pull to target area
object_command_tracking_close_reward_weight = 10 object_command_tracking_close_reward_weight = 10.0
if self.count < 10000: if self.count < 20000:
action_penalty_rate = 1e-4 action_penalty_rate = 1e-4
joint_vel_penalty_rate = 1e-4 joint_vel_penalty_rate = 1e-4
else: else:

5
motrix_envs/src/motrix_envs/manipulation/franka_open_cabinet/franka_open_cabinet_np.py
View File

@@ -18,7 +18,7 @@ import motrixsim as mtx
import numpy as np import numpy as np
from motrix_envs import registry from motrix_envs import registry
from motrix_envs.math.quaternion import Quaternion from motrix_envs.math import quaternion
from motrix_envs.np.env import NpEnv, NpEnvState from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import FrankaOpenCabinetEnvCfg from .cfg import FrankaOpenCabinetEnvCfg
@@ -206,7 +206,7 @@ class FrankaOpenCabinetEnv(NpEnv):
dist_reward *= 10 dist_reward *= 10
## matching orientation reward ## matching orientation reward
quat_reward = Quaternion.similarity(robot_grasp_pose[:, -4:], drawer_grasp_pose[:, -4:]) quat_reward = quaternion.similarity(robot_grasp_pose[:, -4:], drawer_grasp_pose[:, -4:])
## close gripper reward ## close gripper reward
# When gripper distance < 0.025, closing gripper gets reward # When gripper distance < 0.025, closing gripper gets reward
@@ -227,6 +227,7 @@ class FrankaOpenCabinetEnv(NpEnv):
open_reward = ( open_reward = (
np.bitwise_not(wrong_open) * open_reward np.bitwise_not(wrong_open) * open_reward
) # No reward for forced opening (can't force open after increasing MJCF resistance) ) # No reward for forced opening (can't force open after increasing MJCF resistance)
quat_reward = np.where(open_reward > 0, 1.0, quat_reward)
##################### Penalty Terms #####################" ##################### Penalty Terms #####################"
## Action penalty ## Action penalty

21
motrix_envs/src/motrix_envs/manipulation/shadow_hand/__init__.py Normal file
View File

@@ -0,0 +1,21 @@
# 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 .cfg import ShadowHandReposeEnvCfg
# Use the full implementation from env.py (157-dim obs with fingertips)
from .shadow_hand_np import ShadowHandReposeEnv
__all__ = ["ShadowHandReposeEnvCfg", "ShadowHandReposeEnv"]

142
motrix_envs/src/motrix_envs/manipulation/shadow_hand/cfg.py Normal file
View File

@@ -0,0 +1,142 @@
# 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.
# ==============================================================================
"""Configuration for Shadow Hand Cube Reorientation Environment"""
import os
from dataclasses import dataclass
from typing import List, Tuple
from motrix_envs import registry
from motrix_envs.base import EnvCfg
# Path to the repose_cube.xml model
model_file = os.path.join(os.path.dirname(__file__), "xmls", "repose_cube.xml")
@registry.envcfg("shadow-hand-repose")
@dataclass
class ShadowHandReposeEnvCfg(EnvCfg):
"""
Configuration for Shadow Hand Cube Reorientation Environment
This environment trains the Shadow Hand to reorient a cube to match
randomly sampled target orientations.
"""
# ====================
# Model Configuration
# ====================
model_file: str = model_file
# ====================
# Simulation Parameters
# ====================
sim_dt: float = 0.01
sim_substeps: int = 1 # Number of simulation steps per control step
ctrl_dt: float = sim_dt * sim_substeps
max_episode_seconds: float = 10.0
max_episode_steps: int = int(max_episode_seconds / ctrl_dt)
# ====================
# Robot Configuration
# ====================
num_hand_dofs: int = 24 # Total DOFs in Shadow Hand
num_actuators: int = 20 # Actuated joints
# Fingertip link names for forward kinematics
fingertip_link_names: List[str] = (
"rh_ffdistal", # First finger (index) distal
"rh_mfdistal", # Middle finger distal
"rh_rfdistal", # Ring finger distal
"rh_lfdistal", # Little finger distal
"rh_thdistal", # Thumb distal
)
# ====================
# Object Configuration
# ====================
cube_initial_pos: Tuple[float, float, float] = (0.33, 0.00, 0.295) # Initial cube position
# ====================
# Reward Parameters
# ====================
# Core reward components
dist_reward_scale: float = -10.0 # Balanced for MotrixSim (推荐)
rot_reward_scale: float = 1.0 # Moderate rotation reward
rot_eps: float = 0.1 # Stable denominator
action_penalty_scale: float = -0.0002
# Success and failure criteria
success_tolerance: float = 0.1 # ~8.6° (moderate challenge)
reach_goal_bonus: float = 2.0 # Balanced incentive
fall_dist: float = 0.24 # Reasonable manipulation space # Distance threshold for dropping cube (meters)
fall_penalty: float = 0.0 # Penalty for dropping the cube
# In-hand distance threshold (only used for success check, not reward)
in_hand_dist_threshold: float = 0.05 # Distance threshold for "in-hand" (5cm)
# Success hold mechanism (uses max_consecutive_successes)
max_consecutive_successes: int = 50 # Reset after holding success for this many steps
# Averaging factor for consecutive successes tracking
av_factor: float = 0.1
# ====================
# Reset Noise Parameters
# ====================
reset_position_noise: float = 0.01 # Increased robustness
reset_dof_pos_noise: float = 0.2 # Higher generalization
reset_dof_vel_noise: float = 0.0 # DOF velocity noise at reset
# ====================
# Observation Scaling
# ====================
vel_obs_scale: float = 0.2 # Scale factor for velocity observations
# ====================
# Action Processing
# ====================
act_moving_average: float = 1.0 # Action smoothing (1.0 = no smoothing)
# ====================
# Visualization
# ====================
# Offset for target visualization (relative to hand position)
# Recommended: offset to upper-left to avoid occluding the real hand and cube
viz_target_offset: Tuple[float, float, float] = (
0.0, # Left (negative X)
0.0, # Forward/Up (negative Y)
0.2, # Up (positive Z)
)
# ====================
# Domain Randomization (Optional)
# ====================
# Enable domain randomization (recommended to start with False)
enable_domain_randomization: bool = False
# Randomization parameters (only used if enable_domain_randomization=True)
randomize_friction: bool = False
friction_range: Tuple[float, float] = (0.8, 12)
randomize_mass: bool = False
mass_range: Tuple[float, float] = (0.8, 1.2)
randomize_com: bool = False
com_displacement_range: float = 0.01

391
motrix_envs/src/motrix_envs/manipulation/shadow_hand/shadow_hand_np.py Normal file
View File

@@ -0,0 +1,391 @@
# 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.
# ==============================================================================
"""
Shadow Hand Cube Reorientation Environment for MotrixSim
This environment implements the classic in-hand cube manipulation task where the
Shadow Hand must reorient a cube to match random target orientations.
"""
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.math import quaternion, utils
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import ShadowHandReposeEnvCfg
@registry.env("shadow-hand-repose", sim_backend="np")
class ShadowHandReposeEnv(NpEnv):
"""
Shadow Hand Cube Reorientation Environment
Observation space: 157 dimensions
- 24: hand dof positions (unscaled)
- 24: hand dof velocities (scaled by 0.2)
- 7: object pose (pos + quat)
- 3: object linear velocity
- 3: object angular velocity (scaled by 0.2)
- 7: goal pose (pos + quat)
- 4: relative quaternion (object to goal)
- 65: fingertip states (5 fingertips * 13: pos + quat + vel)
- 20: previous actions
Action space: 20 dimensions (normalized [-1, 1] position targets for actuators)
"""
_cfg: ShadowHandReposeEnvCfg
def __init__(self, cfg: ShadowHandReposeEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
# Get model info
self._num_hand_dofs = cfg.num_hand_dofs # 24 total DOFs
self._num_actuators = cfg.num_actuators # 20 actuated joints
# Initialize spaces
self._action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(self._num_actuators,), dtype=np.float32)
self._observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(157,), dtype=np.float32)
# Get actuator control ranges from model
self._actuator_ctrl_lower = self._model.actuator_ctrl_limits[0, :]
self._actuator_ctrl_upper = self._model.actuator_ctrl_limits[1, :]
# Get joint limits for all DOFs (use model's joint_limits directly)
self._hand_dof_lower_limits = self._model.joint_limits[0, :]
self._hand_dof_upper_limits = self._model.joint_limits[1, :]
# Fingertip link indices (use get_link_index for pose/velocity access)
self._fingertip_link_ids = []
for name in cfg.fingertip_link_names:
link_id = self._model.get_link_index(name)
self._fingertip_link_ids.append(link_id)
self._num_fingertips = len(self._fingertip_link_ids)
# Get cube and target link indices
self._cube_link_id = self._model.get_link_index("cube")
self._cube_body = self._model.get_body("cube")
self._cube_dof_vel_indices = self._cube_body.get_dof_vel_indices()
# Target is a mocap body - access via Body object (no get_mocap_index API)
self._target_body = self._model.get_body("target")
self._target_link_id = self._model.get_link_index("target")
assert self._target_body.is_mocap, "Target must be a mocap body"
# Initial cube position (in hand)
self._in_hand_pos = np.array(cfg.cube_initial_pos, dtype=np.float32)
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def _extract_cube_states(self, data: mtx.SceneData, body: mtx.Body):
return body.get_position(data), body.get_rotation(data), data.dof_vel[:, body.get_dof_vel_indices()]
def _extract_link_states(self, data: mtx.SceneData, link_ids):
"""
Extract position, quaternion, and velocity for specified links.
Args:
data: SceneData object
link_ids: List of link indices or single int
Returns:
Tuple of (positions, quaternions, velocities)
- positions: (batch, num_links, 3)
- quaternions: (batch, num_links, 4) in (x, y, z, w) format
- velocities: (batch, num_links, 6) [linear_vel, angular_vel]
"""
# Ensure link_ids is a list
if isinstance(link_ids, int):
link_ids = [link_ids]
# Get all link poses: shape (batch, num_links_total, 7) [x, y, z, qx, qy, qz, qw]
all_poses = self._model.get_link_poses(data)
# Extract poses for requested links
poses = all_poses[:, link_ids, :] # (batch, num_requested_links, 7)
# Split into position and quaternion
positions = poses[:, :, :3]
quaternions = poses[:, :, 3:] # (qx, qy, qz, qw)
# TODO: Velocity computation - MotrixSim doesn't expose link velocities yet
# Temporary solution: use zero velocities
# Future options:
# 1. Finite differences (requires storing previous poses)
# 2. Compute from DOF velocities via Jacobian (if available)
# 3. Wait for API: model.get_link_velocities(data)
batch_size = data.shape[0]
num_links = len(link_ids)
velocities = np.zeros((batch_size, num_links, 6), dtype=np.float32)
for j in np.arange(num_links):
link = self._model.get_link(link_ids[j])
velocities[:, j] = np.concatenate(
(link.get_linear_velocity(data), link.get_angular_velocity(data)), axis=-1
)
return positions, quaternions, velocities
def apply_action(self, actions: np.ndarray, state: NpEnvState):
"""Apply actions to the hand actuators."""
cfg = self._cfg
# Scale actions from [-1, 1] to actuator control range
targets = utils.scale(actions, self._actuator_ctrl_lower, self._actuator_ctrl_upper)
# Apply action moving average for smoothness
if cfg.act_moving_average < 1.0:
targets = cfg.act_moving_average * targets + (1.0 - cfg.act_moving_average) * state.info["prev_actions"]
# Clamp to control limits
targets = np.clip(targets, self._actuator_ctrl_lower, self._actuator_ctrl_upper)
# Set actuator controls
state.data.actuator_ctrls = targets
state.info["prev_actions"] = targets.copy()
return state
def update_state(self, state: NpEnvState):
"""Update observations, rewards, and termination conditions."""
data = state.data
info = state.info
# compute obs
obs = self._compute_observation(state.data, info)
# Compute reward and termination
reward, terminated, goal_reached = self._compute_reward(state, info)
if np.any(goal_reached):
reset_goal_indices = np.where(goal_reached)[0]
self._reset_goal_pose(info, reset_goal_indices)
# Update target visualization
self._update_target_visualization(data, info)
state.obs = obs
state.reward = reward
state.terminated = terminated
return state
def _compute_observation(self, data: mtx.SceneData, info: dict):
cfg = self._cfg
num_envs = data.shape[0]
# Get hand DOF states
hand_dof_pos = data.dof_pos[:, : self._num_hand_dofs]
hand_dof_vel = data.dof_vel[:, : self._num_hand_dofs]
# Get cube state using link poses
cube_pos, cube_quat, cube_vel = self._extract_cube_states(data, self._cube_body)
cube_linvel = cube_vel[:, :3]
cube_angvel = cube_vel[:, 3:]
# Get fingertip states using link poses
fingertip_pos, fingertip_quat, fingertip_vel = self._extract_link_states(data, self._fingertip_link_ids)
# Flatten fingertip states (5 × 13 = 65)
fingertip_state = np.concatenate(
[
fingertip_pos.reshape(num_envs, -1), # 15
fingertip_quat.reshape(num_envs, -1), # 20
fingertip_vel.reshape(num_envs, -1), # 30
],
axis=-1,
) # Total: 65
# Compute relative quaternion
relative_quat = quaternion.mul(cube_quat, quaternion.conjugate(info["goal_rot"]))
scaled_hand_pos = utils.unscale(hand_dof_pos, self._hand_dof_lower_limits, self._hand_dof_upper_limits)
# Build observation (157 dims)
return np.concatenate(
[
scaled_hand_pos,
cfg.vel_obs_scale * hand_dof_vel, # 24
cube_pos, # 3
cube_quat, # 4
cube_linvel, # 3
cfg.vel_obs_scale * cube_angvel, # 3
info["goal_pos"], # 3
info["goal_rot"], # 4
relative_quat, # 4
fingertip_state, # 65
info["prev_actions"], # 20
],
axis=-1,
)
def _compute_reward(self, state: NpEnvState, info: dict):
"""
Reward components (3 core items):
1. Position distance penalty
2. Rotation alignment reward
3. Action regularization penalty
Additional rewards/penalties:
- Success bonus when goal is reached
- Fall penalty when cube drops
- Timeout penalty when episode ends without success
"""
cfg = self._cfg
num_envs = self._num_envs
# Get cube state using link poses
cube_pos, cube_quat, _ = self._extract_cube_states(state.data, self._cube_body)
# Distance from cube to goal position
goal_dist = np.linalg.norm(cube_pos - state.info["goal_pos"], axis=-1)
# Rotation distance
rot_dist = quaternion.rotation_distance(cube_quat, state.info["goal_rot"])
# Core reward components
dist_rew = goal_dist * cfg.dist_reward_scale
rot_rew = 1.0 / (np.abs(rot_dist) + cfg.rot_eps) * cfg.rot_reward_scale
action_penalty = np.sum(state.info["prev_actions"] ** 2, axis=-1) * cfg.action_penalty_scale
# Base reward
reward = dist_rew + rot_rew + action_penalty
# Check for success (only rotation tolerance)
goal_reached = np.abs(rot_dist) <= cfg.success_tolerance
# Update success counter
info["successes"] += goal_reached * 1
# Success bonus
reward = np.where(goal_reached, reward + cfg.reach_goal_bonus, reward)
# Fall penalty
fallen = goal_dist >= cfg.fall_dist
reward = np.where(fallen, reward + cfg.fall_penalty, reward)
# Termination conditions
terminated = np.zeros(num_envs, dtype=bool)
# 1. Fall termination
terminated = np.logical_or(terminated, fallen)
# 2. Success termination with hold mechanism
if cfg.max_consecutive_successes > 0:
# Reset progress on goal reached when max consecutive successes reached
new_pos = info["successes"] >= cfg.max_consecutive_successes
info["successes"] *= 1 - new_pos
# 3. NaN protection
terminated = np.logical_or(terminated, np.isnan(rot_dist))
terminated = np.logical_or(terminated, np.isnan(goal_dist))
return reward, terminated, new_pos
def _update_target_visualization(self, data: mtx.SceneData, info: dict):
"""Update the target mocap body to visualize the goal pose."""
cfg = self._cfg
# Compute visualization position (offset from goal position)
viz_pos = info["goal_pos"] + np.array(cfg.viz_target_offset, dtype=np.float32)
# Combine into pose array: [x, y, z, qx, qy, qz, qw]
viz_pose = np.concatenate([viz_pos, info["goal_rot"]], axis=-1)
# Update mocap body pose using correct API
self._target_body.mocap.set_pose(data, viz_pose)
def reset(self, data: mtx.SceneData):
"""Reset environments."""
cfg = self._cfg
# data is already filtered to contain only envs that need reset
num_resets = data.shape[0]
# Reset scene data
data.reset(self._model)
# Reset hand DOFs with noise
init_dof_pos = self._model.compute_init_dof_pos()
init_dof_vel = np.zeros(self._model.num_dof_vel, dtype=np.float32)
# Add noise to DOF positions
dof_pos_noise = np.random.uniform(
-cfg.reset_dof_pos_noise,
cfg.reset_dof_pos_noise,
(num_resets, self._num_hand_dofs),
)
# Add noise to DOF velocities
dof_vel_noise = np.random.uniform(
-cfg.reset_dof_vel_noise, cfg.reset_dof_vel_noise, (num_resets, self._num_hand_dofs)
)
# Set DOF states for all envs in data (already filtered)
dof_pos = np.tile(init_dof_pos, (num_resets, 1))
dof_vel = np.tile(init_dof_vel, (num_resets, 1))
dof_pos[:, : self._num_hand_dofs] += dof_pos_noise
dof_vel[:, : self._num_hand_dofs] += dof_vel_noise
data.set_dof_pos(dof_pos, self._model)
data.set_dof_vel(dof_vel)
# Reset cube position with small noise
cube_pos_noise = np.random.uniform(-cfg.reset_position_noise, cfg.reset_position_noise, (num_resets, 3))
cube_pos = np.tile(self._in_hand_pos, (num_resets, 1))
cube_pos += cube_pos_noise
# Randomize cube orientation
cube_quat = quaternion.generate_random_shoemake(num_resets)
# Set cube pose using body's set_dof_pos method
# Combine into DOF pose: [x, y, z, qx, qy, qz, qw]
cube_dof_pos = np.concatenate([cube_pos, cube_quat], axis=-1)
# Set cube DOF position
self._cube_body.set_dof_pos(data, cube_dof_pos)
# Set cube DOF velocity to zero
cube_dof_vel = np.zeros((num_resets, 6), dtype=np.float32) # 6DOF velocity
self._cube_body.set_dof_vel(data, cube_dof_vel)
# Reset goal pose
# Note: goal_pos and goal_rot are indexed by original env indices
info = {
"goal_pos": np.tile(self._in_hand_pos, num_resets).reshape(num_resets, 3),
"goal_rot": quaternion.generate_random_shoemake(num_resets),
"prev_actions": np.zeros((num_resets, self._num_actuators), dtype=np.float32),
"successes": np.zeros((num_resets), dtype=np.int32),
}
obs = self._compute_observation(data, info)
return obs, info
def _reset_goal_pose(self, info, env_ids):
"""Reset goal pose to random orientation with fixed position."""
num_resets = len(env_ids)
# Goal position is fixed
# Randomize goal orientation using Shoemake method for uniform SO(3) sampling
info["goal_rot"][env_ids] = quaternion.generate_random_shoemake(num_resets)

202
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/LICENSE Normal file
View File

@@ -0,0 +1,202 @@
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
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2022 Shadow Robot Company Ltd
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.

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/ShadowHand.png Normal file
View File

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

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/assets/Cube_Side.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/assets/Cube_Side.png Normal file
View File

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

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/assets/f_distal_pst.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/assets/f_knuckle.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/manipulation/shadow_hand/xmls/assets/f_middle.obj Normal file
View File

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

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