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

chore: release v0.1.0

Browse Source 
(cherry picked from commit 82525f882f3924a332d9ce40bf64255d0d14f6a4)
This commit is contained in:
motphys-developers
2026-01-04 04:43:04 +00:00
parent 13cfbce9a7
commit 62011bb24f
390 changed files with 18897 additions and 626 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.gitattributes vendored
View File

@@ -1,8 +1,11 @@
* text=auto * text=auto
*.sh text eol=lf *.sh text eol=lf
*.stl filter=lfs diff=lfs merge=lfs -text *.stl filter=lfs diff=lfs merge=lfs -text
*.STL filter=lfs diff=lfs merge=lfs -text
*.obj filter=lfs diff=lfs merge=lfs -text *.obj filter=lfs diff=lfs merge=lfs -text
*.pickle filter=lfs diff=lfs merge=lfs -text *.pickle filter=lfs diff=lfs merge=lfs -text
*.png filter=lfs diff=lfs merge=lfs -text *.png filter=lfs diff=lfs merge=lfs -text
*.mp4 filter=lfs diff=lfs merge=lfs -text *.mp4 filter=lfs diff=lfs merge=lfs -text
*.jpg filter=lfs diff=lfs merge=lfs -text *.jpg filter=lfs diff=lfs merge=lfs -text
*.hfield filter=lfs diff=lfs merge=lfs -text
*.ktx2 filter=lfs diff=lfs merge=lfs -text

81
docs/gen_poster.sh
View File

@@ -4,8 +4,6 @@
# Extracts the first frame from all videos in source/_static/videos/ # Extracts the first frame from all videos in source/_static/videos/
# and saves them as poster images in source/_static/images/poster/ # and saves them as poster images in source/_static/images/poster/
set -e
# Define directories # Define directories
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
SOURCE_DIR="$SCRIPT_DIR/source/_static/videos" SOURCE_DIR="$SCRIPT_DIR/source/_static/videos"
@@ -22,6 +20,17 @@ if ! command -v ffmpeg &> /dev/null; then
exit 1 exit 1
fi fi
# Check if timeout command is available
if ! command -v timeout &> /dev/null; then
echo "Warning: 'timeout' command not found. Script may hang on problematic videos."
echo "On Ubuntu/Debian: sudo apt-get install coreutils"
echo "On macOS: brew install coreutils"
FFMPEG_TIMEOUT=""
else
# Timeout for ffmpeg command (in seconds)
FFMPEG_TIMEOUT="timeout 30"
fi
# Check if source directory exists # Check if source directory exists
if [ ! -d "$SOURCE_DIR" ]; then if [ ! -d "$SOURCE_DIR" ]; then
echo "Error: Source directory '$SOURCE_DIR' does not exist." echo "Error: Source directory '$SOURCE_DIR' does not exist."
@@ -35,6 +44,7 @@ echo "Created target directory: $TARGET_DIR"
# Counter for processed files # Counter for processed files
processed=0 processed=0
skipped=0 skipped=0
failed=0
# Process all video files # Process all video files
echo "Processing videos in: $SOURCE_DIR" echo "Processing videos in: $SOURCE_DIR"
@@ -53,36 +63,79 @@ for video_file in "$SOURCE_DIR"/*; do
if [[ " ${VIDEO_EXTENSIONS[*]} " =~ " ${extension,,} " ]]; then if [[ " ${VIDEO_EXTENSIONS[*]} " =~ " ${extension,,} " ]]; then
output_file="$TARGET_DIR/${filename_noext}.jpg" output_file="$TARGET_DIR/${filename_noext}.jpg"
# Check if output file already exists # Check if output file already exists and is valid
if [ -f "$output_file" ]; then if [ -f "$output_file" ]; then
echo "<EFBFBD> Skipping '$filename' (poster already exists)" # Verify the existing poster is valid (not empty and is an image)
if [ -s "$output_file" ]; then
echo "✓ Skipping '$filename' (poster already exists)"
((skipped++)) ((skipped++))
continue continue
else
echo "⚠ Removing invalid poster for '$filename'"
rm -f "$output_file"
fi
fi fi
echo "<<3C> Processing '$filename'..." echo "⏳ Processing '$filename'..."
# Extract first frame using ffmpeg # Extract first frame using ffmpeg with timeout
if ffmpeg -i "$video_file" -vframes 1 -q:v 2 "$output_file" -y -loglevel error; then if [ -n "$FFMPEG_TIMEOUT" ]; then
echo " Created poster: ${filename_noext}.jpg" # 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
# No timeout available, run ffmpeg directly
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
if $ffmpeg_success; then
# Verify the output file was created and is valid
if [ -f "$output_file" ] && [ -s "$output_file" ]; then
echo "✓ Created poster: ${filename_noext}.jpg"
((processed++)) ((processed++))
else else
echo "L Failed to process '$filename'" echo "✗ Failed to create valid poster for '$filename'"
rm -f "$output_file" # Remove any partial output rm -f "$output_file" # Remove any partial output
((failed++))
fi fi
else else
echo "<EFBFBD> Skipping '$filename' (not a supported video format)" # ffmpeg failed
exit_code=$?
if [ $exit_code -eq 124 ]; then
echo "✗ Timeout processing '$filename' (after 30s)"
else
echo "✗ Failed to process '$filename' (exit code: $exit_code)"
fi
rm -f "$output_file" # Remove any partial output
((failed++))
fi
else
echo "⊘ Skipping '$filename' (not a supported video format)"
((skipped++)) ((skipped++))
fi fi
done done
echo "------------------------------------------------" echo "------------------------------------------------"
echo "Poster generation completed!" echo "Poster generation completed!"
echo "=<3D> Summary:" echo "📊 Summary:"
echo " Processed: $processed videos" echo " ✓ Processed: $processed videos"
echo " Skipped: $skipped files" echo " → Skipped: $skipped files"
if [ $failed -gt 0 ]; then
echo " ✗ Failed: $failed videos"
fi
echo " Posters saved to: $TARGET_DIR" echo " Posters saved to: $TARGET_DIR"
if [ $processed -eq 0 ]; then if [ $processed -eq 0 ]; then
echo "<22> No new posters were created." if [ $failed -eq 0 ]; then
echo "ℹ No new posters were created."
else
echo "⚠ Some posters failed to generate. Please check the error messages above."
fi
fi fi

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

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

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

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

4
docs/source/_static/images/poster/cartpole.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:10b78648c8a4ded8c56292377d5a0823a1c8e9cc5f8764fa8082f6a3341a41ad oid sha256:acd9f471a16c657e14eaa7afd47a97f0931c9abb6ff61fe313f0ed14bf023d70
size 176434 size 154248

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

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

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

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

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

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

4
docs/source/_static/images/poster/dm_walker.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:987c2ac4e86e1fe6634a16b3ea5fb1a633f07b41d39918c6a23df4da88d5b45c oid sha256:65f2d4284641a2a29b694a2e9f06e4baa0a4cdb24ef6d35e4d934d370db438a6
size 140437 size 68165

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

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

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

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

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

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

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

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

4
docs/source/_static/images/poster/go1_walk.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:194d9a0d40b04d9d41aff936503a1fc8a4d95e1bb3ba28e968996cc29793060a oid sha256:febee93d0c777a8b3ef90fcfbeb9d9c6735c4893a890f27681a5f36b42498eaf
size 161837 size 118851

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

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

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

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

4
docs/source/_static/videos/cartpole.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:723e272be6aff6e0c10f72f871b5674c509a3fa9b6b0ca26c0671b4b67d343ee oid sha256:b091b1abe3d85d76541ac6e1595490ea49acc98bda2bb7d1890cc8614dd25529
size 1816175 size 571580

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

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

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

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

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

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

4
docs/source/_static/videos/dm_walker.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:e877e592a3fcf323ca37aeced5e0d5383a128d82958dc3044816020c64e25d01 oid sha256:190b88b725ab42348d98b6eb6dd248a58d64fc4cd605436ae8d4d1355ce4ae82
size 1628617 size 2222983

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

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

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

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

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

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

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

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

4
docs/source/_static/videos/go1_walk.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:540a9fa3591f456c509f8c3383dc2dbe2106cdf75a7415d49307d74a9388105f oid sha256:0e7e2f80c3b3b65120328df49de6890f61526e75c7ab7b60636c04a00490cf59
size 4770163 size 3283972

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

@@ -35,6 +35,76 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:::{grid-item-card} :::{grid-item-card}
```{video} _static/videos/go1_rough_terrain_walk.mp4
:poster: _static/images/poster/go1_rough_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/go1_stairs_terrain_walk.mp4
:poster: _static/images/poster/go1_stairs_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/franka_lift_cube.mp4
:poster: _static/images/poster/franka_lift_cube.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/franka_open_cabinet.mp4
:poster: _static/images/poster/franka_open_cabinet.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/anymal_c.mp4
:poster: _static/images/poster/anymal_c.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_walker.mp4 ```{video} _static/videos/dm_walker.mp4
:poster: _static/images/poster/dm_walker.jpg :poster: _static/images/poster/dm_walker.jpg
:nocontrols: :nocontrols:
@@ -45,6 +115,62 @@ MotrixLab is a general-purpose machine learning architecture designed for robot
:width: 100% :width: 100%
``` ```
:::
:::{grid-item-card}
```{video} _static/videos/dm_cheetah.mp4
:poster: _static/images/poster/dm_cheetah.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_hopper.mp4
:poster: _static/images/poster/dm_hopper.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
::: :::
:::: ::::

316
docs/source/en/user_guide/demo/anymal_c.md Normal file
View File

@@ -0,0 +1,316 @@
# ANYmal-C Locomotion
## Overview
This document describes in detail the navigation task environment based on the ANYmal-C quadruped robot. This environment is part of the navigation task collection in the MotrixLab project, providing a complete implementation for training quadruped robots to navigate to target positions and orientations using reinforcement learning.
```{video} /_static/videos/anymal_c.mp4
:poster: _static/images/poster/anymal_c.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Environment Description
The ANYmal-C navigation task environment is built based on the real ANYmal-C quadruped robot, designed to train robots to navigate to specified target positions and orientations on flat terrain. This environment uses the MotrixSim physics engine for simulation, providing high-fidelity dynamic simulation.
### Robot Structure
ANYmal-C is a quadruped robot composed of the following main parts:
- **Base**: The core torso of the robot, containing sensor modules such as IMU, camera, and lidar
- **Four Legs**: Each leg contains three joints
- HAA (Hip Abduction/Adduction): Hip abduction/adduction joint
- HFE (Hip Flexion/Extension): Hip flexion/extension joint
- KFE (Knee Flexion/Extension): Knee flexion/extension
- **Four Feet**: Spherical contact geometries that generate frictional contact with the ground
### Task Objective
The robot needs to complete the following navigation objectives:
1. **Position Navigation**: Move to the specified target position (XY plane coordinates)
2. **Orientation Control**: Adjust robot orientation to the target heading angle (yaw angle)
3. **Stable Stop**: Maintain stable standing after reaching the target, with linear and angular velocities approaching zero
The environment provides visualization markers:
- **Green Arrow**: Indicates target position and orientation
- **Green Arrow Above Robot**: Current actual movement direction
- **Blue Arrow Above Robot**: Desired movement direction
---
## Action Space
The action space is `Box(-1.0, 1.0, (12,), float32)`, representing position control commands applied to 12 joints (offsets relative to the default standing posture).
### Control Mode
The environment uses position control mode. Actions are converted to joint target positions as follows:
```
Target Joint Angle = Default Joint Angle + (Action Value × Action Scale)
```
Where the action scale is specified by the configuration parameter `control_config.action_scale`.
### Action Dimension Details
| Index | Action Description | Control Range | Joint Name | Joint Type |
| ----- | -------------------- | ------------- | ---------- | ---------- |
| 0 | Left Front Hip HAA | -1.0 ~ 1.0 | LF_HAA | hinge |
| 1 | Left Front Hip HFE | -1.0 ~ 1.0 | LF_HFE | hinge |
| 2 | Left Front Knee KFE | -1.0 ~ 1.0 | LF_KFE | hinge |
| 3 | Right Front Hip HAA | -1.0 ~ 1.0 | RF_HAA | hinge |
| 4 | Right Front Hip HFE | -1.0 ~ 1.0 | RF_HFE | hinge |
| 5 | Right Front Knee KFE | -1.0 ~ 1.0 | RF_KFE | hinge |
| 6 | Left Hind Hip HAA | -1.0 ~ 1.0 | LH_HAA | hinge |
| 7 | Left Hind Hip HFE | -1.0 ~ 1.0 | LH_HFE | hinge |
| 8 | Left Hind Knee KFE | -1.0 ~ 1.0 | LH_KFE | hinge |
| 9 | Right Hind Hip HAA | -1.0 ~ 1.0 | RH_HAA | hinge |
| 10 | Right Hind Hip HFE | -1.0 ~ 1.0 | RH_HFE | hinge |
| 11 | Right Hind Knee KFE | -1.0 ~ 1.0 | RH_KFE | hinge |
### PD Control Parameters
The underlying system uses position actuators with PD control parameters defined in the XML file:
- **kp (Proportional Gain)**: 200
- **kv (Derivative Gain)**: 1
- **Torque Limit**: -140 N·m ~ 140 N·m
---
## Observation Space
The observation space is `Box(-inf, inf, (54,), float32)`, containing the robot's proprioceptive information, task-related information, and action history.
### Observation Components
The observation vector consists of the following parts (in order):
1. **Proprioceptive State (33 dimensions)**
- Base Linear Velocity (3 dim): Linear velocity of robot base in world frame [vx, vy, vz]
- Angular Velocity (3 dim): Angular velocity from gyroscope [ωx, ωy, ωz]
- Projected Gravity (3 dim): Gravity vector projected in robot body frame
- Joint Angles (12 dim): 12 joint angle offsets relative to default standing posture
- Joint Velocities (12 dim): 12 joint angular velocities
2. **Action History (12 dimensions)**
- Action executed at previous timestep
3. **Velocity Commands (3 dimensions)**
- Desired Linear Velocity XY (2 dim): Desired linear velocity calculated from position error
- Desired Angular Velocity Z (1 dim): Desired angular velocity calculated from orientation error
4. **Task State (6 dimensions)**
- Position Error Vector (2 dim): XY plane error vector to target position (normalized)
- Orientation Error (1 dim): Angle difference to target orientation (normalized to [-1, 1])
- Distance (1 dim): Euclidean distance to target (normalized)
- Arrival Flag (1 dim): Whether both position and orientation arrival conditions are satisfied (0 or 1)
- Stop Ready Flag (1 dim): Whether stop criteria are met (arrived and angular velocity near zero)
### Observation Details
| Index | Observation Content | Min | Max | Normalization Coefficient | Unit |
| ----- | ------------------------------------------ | ---- | --- | ------------------------- | ------------- |
| 0-2 | Base Linear Velocity (vx, vy, vz) | -inf | inf | normalization.lin_vel | m/s |
| 3-5 | Angular Velocity (ωx, ωy, ωz) | -inf | inf | normalization.ang_vel | rad/s |
| 6-8 | Projected Gravity (gx, gy, gz) | -1 | 1 | 1.0 | Dimensionless |
| 9-20 | Joint Angle Offsets (12 joints) | -inf | inf | normalization.dof_pos | rad |
| 21-32 | Joint Angular Velocities (12 joints) | -inf | inf | normalization.dof_vel | rad/s |
| 33-44 | Previous Action | -1 | 1 | 1.0 | Dimensionless |
| 45-47 | Velocity Commands (vx_cmd, vy_cmd, ωz_cmd) | -inf | inf | commands_scale | m/s, rad/s |
| 48-49 | Position Error Vector (Δx, Δy) | -inf | inf | 1/5.0 | m |
| 50 | Orientation Error | -1 | 1 | 1/π | rad |
| 51 | Distance to Target | 0 | 1 | 1/5.0 (clipped) | m |
| 52 | Arrival Flag | 0 | 1 | 1.0 | Boolean |
| 53 | Stop Ready Flag | 0 | 1 | 1.0 | Boolean |
### Sensor Information
The environment uses the following sensors to obtain state:
- **framelinvel** (name: base_linvel): Base linear velocity sensor
- **gyro** (name: base_gyro): Gyroscope sensor, mounted at IMU site
---
## Reward Function
The reward function uses a composite design, employing different reward strategies based on whether the robot has reached the target.
### Rewards Before Reaching Target
Total Reward = Velocity Tracking Reward + Approach Reward - Penalty Terms
**Main Reward Terms:**
1. **Linear Velocity Tracking Reward** (Weight: 1.5)
- Formula: `1.5 × exp(-||v_xy - v_cmd||² / 0.25)`
- Encourages robot to track desired XY plane linear velocity
2. **Angular Velocity Tracking Reward** (Weight: 0.3)
- Formula: `0.3 × exp(-(ωz - ωz_cmd)² / 0.25)`
- Encourages robot to track desired yaw angular velocity
3. **Approach Reward**
- Formula: `clip((Historical Minimum Distance - Current Distance) × 4.0, -1.0, 1.0)`
- Rewards robot for progress when getting closer to target
**Penalty Terms:**
- Z-axis Linear Velocity Penalty (Weight: 2.0): `-2.0 × vz²`
- XY-axis Angular Velocity Penalty (Weight: 0.05): `-0.05 × (ωx² + ωy²)`
- Torque Penalty (Weight: 0.00001): `-0.00001 × ||τ||²`
- Action Rate Penalty (Weight: 0.001): `-0.001 × ||Δa||²`
### Rewards After Reaching Target
Total Reward = Stop Reward + First Arrival Reward - Penalty Terms
**Main Reward Terms:**
1. **Stop Base Reward**
- Formula: `2 × [0.8 × exp(-(v_xy/0.2)²) + 1.2 × exp(-(ωz/0.1)⁴)]`
- Encourages robot to maintain low velocity and angular velocity after arrival
2. **Zero Angular Velocity Reward** (Extra Reward: 6.0)
- Condition: Arrived at target and |ωz| < 0.05 rad/s
- Encourages robot to completely stop rotation
3. **First Arrival Reward** (One-time: 10.0)
- Condition: First time both position and orientation arrival conditions are satisfied
- Provides clear signal for reaching target
**Penalty Terms:** (Same as before arrival)
### Termination Condition Penalties
An additional penalty of -20.0 is applied in the following cases:
- Joint velocity exceeds limit (exceeds `max_dof_vel` configuration value)
- Joint velocity is NaN or Inf
- Robot base contacts ground
- Robot rollover (tilt angle exceeds 75°)
### Arrival Criteria
- **Position Arrival**: Distance to target < 0.3 meters
- **Orientation Arrival**: Orientation error < 15°
- **Complete Arrival**: Both position arrival and orientation arrival conditions satisfied
- **Stop Ready**: Complete arrival AND |ωz| < 0.05 rad/s
### info Return Content
The info dictionary returned each step contains the following debug information:
- `pose_commands`: Current target position and orientation [x, y, yaw]
- `last_actions`: Previous action
- `current_actions`: Current action
- `steps`: Current episode step count
- `ever_reached`: Whether target has ever been reached
- `min_distance`: Historical minimum distance (used to calculate approach reward)
---
## Initial State
### Robot Initialization
**Position Initialization:**
The robot's initial position in world coordinates is randomly sampled within the range defined by configuration parameter `init_state.pos_randomization_range`:
- X coordinate: Uniform random sampling in [x_min, x_max]
- Y coordinate: Uniform random sampling in [y_min, y_max]
- Z coordinate: Fixed at 0.56 meters (to avoid falling sensation)
**Orientation Initialization:**
- Robot orientation (quaternion): Initialized to unit quaternion [0, 0, 0, 1], indicating forward orientation
- No random noise added to quaternion (ensures initial stability)
**Joint Initialization:**
Joint angles are set to default standing posture, defined by configuration parameter `init_state.default_joint_angles`. No random noise added to joint angles (ensures stable standing initially).
**Velocity Initialization:**
All linear and angular velocities are initialized to zero, ensuring robot starts from stationary state.
### Target Generation
**Target Position:**
Target position is generated relative to robot's initial position:
```
Target Position = Robot Initial Position + Random Offset
```
Random offset is sampled within the range defined by configuration parameter `commands.pose_command_range`:
- X direction offset: [pose_command_range[0], pose_command_range[3]]
- Y direction offset: [pose_command_range[1], pose_command_range[4]]
**Target Orientation:**
Target orientation (yaw angle) is randomly generated in absolute reference frame:
- Orientation angle: [pose_command_range[2], pose_command_range[5]]
### Visualization Marker Initialization
- **Target Marker** (green arrow): Set to target position and orientation
- **Movement Direction Arrows**: Initialized at 0.76 meters above robot
---
## Episode Termination
### Termination Conditions
The environment terminates an episode when any of the following conditions are met:
1. **Timeout Termination**
- Condition: Episode reaches `max_episode_second` configuration value
- Description: Prevents infinite episodes
2. **Joint Velocity Anomaly**
- Condition: Absolute value of any joint velocity exceeds `max_dof_vel`
- Condition: Joint velocity is NaN, Inf, or exceeds 1e6
- Description: Prevents numerical divergence and physical instability
3. **Base Contacts Ground**
- Condition: Robot base (geometries defined by `terminate_after_contacts_on` configuration parameter) contacts ground
- Description: Robot fell or pose failure
4. **Rollover**
- Condition: Robot tilt angle exceeds 75°
- Calculation: Tilt angle calculated via projected gravity vector `arctan2(||g_xy||, |g_z|)`
- Description: Robot severely rolled over
### Success Conditions
Although the environment does not terminate upon success, task success is defined as:
- Robot reaches target position and orientation (both position threshold < 0.3m and orientation threshold < 15° satisfied)
- Robot maintains stable stop (linear velocity < 0.05 m/s, angular velocity < 0.05 rad/s)

130
docs/source/en/user_guide/demo/bounce_ball.md Normal file
View File

@@ -0,0 +1,130 @@
# Ping Pong Ball Bouncing
Train a single-arm robotic manipulator to control a paddle for continuous ball bouncing, maintaining the ball at a target height and position.
```{video} /_static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
Bounce Ball is a single-arm robotic manipulation task using a 6-DOF Peitian AIR4-560 industrial robotic arm to control the position of an end-effector paddle. The agent controls the position changes of the arm's 6 joints as actions, making the ping pong ball bounce continuously on the paddle and keeping it as close as possible to the target height and target horizontal position.
---
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (6,), float32)` |
| **Dimension** | 6 |
The joints correspond as follows:
| Index | Action Meaning (Joint Position Change) | Min Value | Max Value | Corresponding XML Name |
| ----: | --------------------------------------- | :-------: | :-------: | :--------------------: |
| 0 | Joint1 (Base Rotation) Position Change | -1 | 1 | `Joint1` |
| 1 | Joint2 (Upper Arm) Position Change | -1 | 1 | `Joint2` |
| 2 | Joint3 (Forearm) Position Change | -1 | 1 | `Joint3` |
| 3 | Joint4 (Wrist Rotation) Position Change | -1 | 1 | `Joint4` |
| 4 | Joint5 (Wrist Pitch) Position Change | -1 | 1 | `Joint5` |
| 5 | Joint6 (Wrist Rotation) Position Change | -1 | 1 | `Joint6` |
---
## Observation Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (25,), float32)` |
| **Dimension** | 25 |
The observation space consists of the following parts (in order):
| 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_vel** | Velocity information for each degree of freedom | 12 | Velocity is derivative of position |
| Index | Observation | Min Value | Max Value | XML Name | Type (Unit) |
| ----- | --------------------------- | --------- | --------- | ----------- | ------------------------- |
| 0-5 | Arm Joint Angles | -Inf | Inf | Joint1-6 | Angle (rad) |
| 6 | Ball x-coordinate | -Inf | Inf | ball_x | Position (m) |
| 7 | Ball y-coordinate | -Inf | Inf | ball_y | Position (m) |
| 8 | Ball z-coordinate | -Inf | Inf | ball_z | Position (m) |
| 9-12 | Ball Orientation Quaternion | -Inf | Inf | ball_qw/xyz | Quaternion (w,x,y,z) |
| 13-24 | Joint and Ball Velocities | -Inf | Inf | - | Velocity/Angular Velocity |
---
## Reward Function Design
The reward function consists of the following components:
```python
# Position Control Reward: Keep ball above paddle center
# Controlled Upward Velocity Reward: Reward moderate upward velocity when ball is well-positioned
# Height Accuracy Reward: Ball is close to target height
# Consecutive Bounces Reward: Reward consecutive successful bounces
# Total Reward = Weighted combination of all components
```
---
## Initial State
- **Arm Initial Position**: [0, 40, 110, 0, -60, 0] degrees, with random noise
- **Ball Initial Position**: Above paddle center, with random noise
- **Ball Initial Velocity**: [0.0, 0.0, 0.0] m/s
---
## Episode Termination Conditions
- **Ball Falls**: Ball z-coordinate < 0.05m (near ground)
- **Ball Too High**: Ball z-coordinate > target height + 1.0m (lost control)
- **Horizontal Deviation Too Far**: Absolute value of ball x-coordinate > 1.5m
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env bounce_ball
```
### 2. Start Training
```bash
uv run scripts/train.py --env bounce_ball
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/bounce_ball
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env bounce_ball
```
---
## Expected Training Results
1. Consecutive Bouncing: Capable of achieving 3 or more consecutive bounces
2. Position Control: Ball's horizontal position (x-coordinate) stable within target position ± 0.05m 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)
5. Stable Control: Capable of maintaining stable bouncing for 20 seconds without dropping

61
docs/source/en/user_guide/demo/cartpole.md
View File

@@ -1,4 +1,4 @@
# CartPole Training Example # CartPole
CartPole is a classic control task in reinforcement learning. The goal is to keep the pole balanced by controlling the cart's left-right movement. CartPole is a classic control task in reinforcement learning. The goal is to keep the pole balanced by controlling the cart's left-right movement.
![cartpole](/_static/images/poster/cartpole.jpg) ![cartpole](/_static/images/poster/cartpole.jpg)
@@ -21,14 +21,7 @@ uv run scripts/view.py --env cartpole
### 2. Start Training ### 2. Start Training
```bash ```bash
# Train with default parameters
uv run scripts/train.py --env cartpole uv run scripts/train.py --env cartpole
# Customize number of environments
uv run scripts/train.py --env cartpole --num-envs 1024
# Enable rendering (visualize during training)
uv run scripts/train.py --env cartpole --render
``` ```
### 3. View Training Progress ### 3. View Training Progress
@@ -40,63 +33,11 @@ uv run tensorboard --logdir runs/cartpole
### 4. Test Training Results ### 4. Test Training Results
```bash ```bash
# Automatically find best policy for testing (recommended)
uv run scripts/play.py --env cartpole uv run scripts/play.py --env cartpole
# Manually specify policy file for testing
uv run scripts/play.py --env cartpole --policy runs/cartpole/nn/best_policy.pickle
``` ```
> **Tip**: The system will automatically find the latest and best policy files in the `runs/cartpole/` directory for testing. You can also manually specify specific policy files using the `--policy` parameter. > **Tip**: The system will automatically find the latest and best policy files in the `runs/cartpole/` directory for testing. You can also manually specify specific policy files using the `--policy` parameter.
## Configuration Parameters
Main configuration parameters for the CartPole environment:
```python
@dataclass
class CartPoleEnvCfg(EnvCfg):
model_file: str = "path/to/inverted_pendulum.xml" # MJCF model file
reset_noise_scale: float = 0.01 # Reset noise
max_episode_seconds: float = 10.0 # Maximum episode length
```
Training configuration parameters:
```python
from dataclasses import dataclass
from motrix_rl.skrl.cfg import PPOCfg
from motrix_rl import registry
@registry.rlcfg("cartpole")
@dataclass
class CartPolePPO(PPOCfg):
max_env_steps: int = 10_000_000 # Maximum environment steps
check_point_interval: int = 500 # Checkpoint interval
# Network structure (small network suitable for simple tasks)
policy_hidden_layer_sizes: tuple[int, ...] = (32, 32)
value_hidden_layer_sizes: tuple[int, ...] = (32, 32)
# PPO parameters
rollouts: int = 32 # Experience replay rounds
learning_epochs: int = 5 # Training rounds
mini_batches: int = 4 # Number of mini-batches
```
**Note**: CartPole is a simple task and currently uses universal configuration. If you need to create specialized configurations for different training backends (JAX/Torch), refer to the environment configuration documentation examples.
## Custom Training
You can override default configurations through command line arguments:
```bash
uv run scripts/train.py --env cartpole \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
```
## Expected Results ## Expected Results
- Pole angle stays within ±5 degrees most of the time - Pole angle stays within ±5 degrees most of the time

131
docs/source/en/user_guide/demo/dm_cheetah.md Normal file
View File

@@ -0,0 +1,131 @@
# Half-Cheetah Robot
The Half-Cheetah robot is a classic continuous control task in the DeepMind Control Suite. The goal is to train a simulated bipedal robot to run at high speed and stably by controlling its joint torques.
```{video} /_static/videos/dm_cheetah.mp4
:poster: _static/images/poster/dm_cheetah.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
HalfCheetah is a 2D half-cheetah running task, composed of 7 main body parts (1 torso and 3 sections for each of the front and rear legs), with 6 controlled joints (front and rear thighs [connected to the torso], shins [connected to the thighs], and feet [connected to the shins]). The agent applies torques to these joints as actions, aiming to make the cheetah run forward as fast and stably as possible.
---
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (6,), float32)` |
| **Dimension** | 6 |
The joints correspond as follows:
| Index | Action Meaning (Torque applied to the joint) | Min Value | Max Value | Corresponding XML Name |
| ----: | -------------------------------------------- | :-------: | :-------: | :--------------------: |
| 0 | Rear Thigh Joint Drive Torque | -1 | 1 | `bthigh` |
| 1 | Rear Shin Joint Drive Torque | -1 | 1 | `bshin` |
| 2 | Rear Foot Joint Drive Torque | -1 | 1 | `bfoot` |
| 3 | Front Thigh Joint Drive Torque | -1 | 1 | `fthigh` |
| 4 | Front Shin Joint Drive Torque | -1 | 1 | `fshin` |
| 5 | Front Foot Joint Drive Torque | -1 | 1 | `ffoot` |
---
## Observation Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (17,), float32)` |
| **Dimension** | 17 |
The observation space of the HalfCheetah environment consists of the following parts (in order):
| Part | Content Description | Dimension | Remarks |
| -------- | ------------- | -- | ------------ |
| **qpos** | Position information of each body joint and the root | 8 | Root x-coordinate is excluded by default |
| **qvel** | Velocity information of each body joint and the root | 9 | Velocity is the derivative of position |
| Index | Observation | Min Value | Max Value | XML Name | Joint Type | Type (Unit) |
| -------- | ---------------------------- | --------- | --------- | -------- | ---------- | ------------------------ |
| 0 | Front z-coordinate | -Inf | Inf | rootz | slide | Position (m) |
| 1 | Front angle | -Inf | Inf | rooty | hinge | Angle (rad) |
| 2 | Rear Thigh Angle | -Inf | Inf | bthigh | hinge | Angle (rad) |
| 3 | Rear Shin Angle | -Inf | Inf | bshin | hinge | Angle (rad) |
| 4 | Rear Foot Angle | -Inf | Inf | bfoot | hinge | Angle (rad) |
| 5 | Front Thigh Angle | -Inf | Inf | fthigh | hinge | Angle (rad) |
| 6 | Front Shin Angle | -Inf | Inf | fshin | hinge | Angle (rad) |
| 7 | Front Foot Angle | -Inf | Inf | ffoot | hinge | Angle (rad) |
| 8 | Front x-coordinate Velocity | -Inf | Inf | rootx | slide | Velocity (m/s) |
| 9 | Front z-coordinate Velocity | -Inf | Inf | rootz | slide | Velocity (m/s) |
| 10 | Front Angular Velocity | -Inf | Inf | rooty | hinge | Angular Velocity (rad/s) |
| 11 | Rear Thigh Angular Velocity | -Inf | Inf | bthigh | hinge | Angular Velocity (rad/s) |
| 12 | Rear Shin Angular Velocity | -Inf | Inf | bshin | hinge | Angular Velocity (rad/s) |
| 13 | Rear Foot Angular Velocity | -Inf | Inf | bfoot | hinge | Angular Velocity (rad/s) |
| 14 | Front Thigh Angular Velocity | -Inf | Inf | fthigh | hinge | Angular Velocity (rad/s) |
| 15 | Front Shin Angular Velocity | -Inf | Inf | fshin | hinge | Angular Velocity (rad/s) |
| 16 | Front Foot Angular Velocity | -Inf | Inf | ffoot | hinge | Angular Velocity (rad/s) |
| excluded | Front x-coordinate | -Inf | Inf | rootx | slide | Position (m) |
---
## Reward Function Design
The cheetah's reward function consists of the following parts:
```python
# Velocity Reward: Tracking target speed
# Posture Reward: Maintaining a stable posture
# Total Reward = Velocity Reward + Posture Reward
```
---
## Initial State
- Reset all finite joint angles to random values within their allowed ranges, keeping infinite range joints in their default state.
- Generate the initial observation vector by stabilizing the torso and leg positions through multi-step physics simulation.
## Episode Termination Conditions
- **No Fall Termination Condition** (Does not end directly due to instability)
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-cheetah
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-cheetah
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-cheetah
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-cheetah
```
---
## Expected Training Results
1. Stable horizontal speed approaching or exceeding 30.0 m/s
2. Maintaining an upright torso and coordinated gait

140
docs/source/en/user_guide/demo/dm_hopper.md Normal file
View File

@@ -0,0 +1,140 @@
# Single-Leg Hopping Robot
Hopper is a classic single-leg hopping control task in dm-control, simulating a 2D single-leg hopping robot.
```{video} /_static/videos/dm_hopper.mp4
:poster: _static/images/poster/dm_hopper.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
The 2D robot consists of four body segments: torso, pelvis, thigh, calf, and foot. Actions are generated through four articulated joints, including: waist, hip, knee, and ankle. Each joint is driven by a motor with different gear ratios, enabling behaviors such as standing, balancing, and hopping forward.
---
## Action Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-1.0, 1.0, (3,), float32)` |
| **Dimension** | 3 |
Joint mapping:
| Index | Action Description | Min | Max | XML Name |
| ----- | --------------------------------- | --- | --- | ----------- |
| 0 | Torque applied on the thigh rotor | -1 | 1 | thigh_joint |
| 1 | Torque applied on the leg rotor | -1 | 1 | leg_joint |
| 2 | Torque applied on the foot rotor | -1 | 1 | foot_joint |
---
## Observation Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (13,), float64)` |
| **Dimension** | 13 |
| Component | Description | Dim | Notes |
| ------------------- | ----------------------------- | --- | ------------------------------------ |
| **qpos** | Joint angles and torso height | 5 | torso x-position excluded by default |
| **qvel** | Joint and torso velocities | 6 | Velocity as derivative of position |
| **contact sensors** | Toe and heel ground sensors | 2 | Normalized by `log1p` |
The observation vector consists of joint positions (qpos), velocities (qvel), and contact force sensors. The full dimension is 13, including two contact sensors:
| Index | Observation | XML Name | Joint Type | Physical Meaning |
| ----: | ---------------------------- | ------------- | ---------- | ------------------------- |
| 0 | torso z-position | `rootz` | slide | torso height |
| 1 | torso angle | `rooty` | hinge | body pitch angle |
| 2 | thigh joint angle | `thigh_joint` | hinge | thigh rotation |
| 3 | leg joint angle | `leg_joint` | hinge | calf rotation |
| 4 | foot joint angle | `foot_joint` | hinge | foot rotation |
| 5 | torso x-velocity | `rootx` | slide | forward velocity |
| 6 | torso z-velocity | `rootz` | slide | vertical velocity |
| 7 | torso angular velocity | `rooty` | hinge | torso angular velocity |
| 8 | thigh joint angular velocity | `thigh_joint` | hinge | thigh angular velocity |
| 9 | leg joint angular velocity | `leg_joint` | hinge | calf angular velocity |
| 10 | foot joint angular velocity | `foot_joint` | hinge | foot angular velocity |
| 11 | toe touch sensor | `touch_toe` | sensor | toe ground contact force |
| 12 | heel touch sensor | `touch_heel` | sensor | heel ground contact force |
---
## Reward Function Design
The Hopper reward consists of the following terms:
### Stand Task
```python
# Stand reward: maintain stable height
```
### Hop Task
```python
# Stand reward: maintain stable height
# Hopping reward: achieve target forward velocity
# Leg movement reward: encourage moderate leg motion
# Knee extension reward: encourage proper knee extension
# Foot contact reward: encourage proper ground reaction forces
# Total reward = stand_reward + hop_reward + leg_motion_reward + knee_reward + contact_reward
```
---
## Initial State
- Randomize joint angles within allowed ranges during reset
---
## Episode Termination Conditions
- Observation values contain invalid numerical values (NaN)
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-hopper-stand
uv run scripts/view.py --env dm-hopper-hop
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-hopper-stand
uv run scripts/train.py --env dm-hopper-hop
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-hopper-stand
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-hopper-stand
uv run scripts/play.py --env dm-hopper-hop
```
---
## Expected Training Results
1. Maintain stable standing behavior
2. Achieve a target hopping speed of **2.0**

128
docs/source/en/user_guide/demo/dm_reacher.md Normal file
View File

@@ -0,0 +1,128 @@
# Two-Joint Robotic Arm Control
Reacher is a classic robotic arm control task, simulating a robotic arm composed of two links. The goal is to bring the end effector (fingertip) as close as possible to a randomly generated target point.
```{video} /_static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
The Reacher consists of two joints, with two links connected by hinge joints. The objective of the task is to move the end of the robotic arm to the target position. The target point is randomly sampled 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 the first joint (root link) | -1 | 1 | joint0 | hinge |
| 1 | Torque applied to the second joint (middle link) | -1 | 1 | joint1 | hinge |
---
## Observation Space
| Item | Details |
| ------------- | ------------------------------- |
| **Type** | `Box(-inf, inf, (6,), float32)` |
| **Dimension** | 6 |
The observation vector contains the following parts (in order):
- **qpos**: 2 joint angles
- **fingertip → target vector difference**: x and y dimensions
- **qvel**: 2 joint angular velocities
| Index | Observation | Min | Max | XML Name | Joint | Unit |
| ----- | ------------------------------- | ---- | --- | ---------- | ----- | ----- |
| 0 | First joint angle | -inf | inf | joint0_pos | hinge | rad |
| 1 | Second joint angle | -inf | inf | joint1_pos | hinge | rad |
| 2 | fingertip - target x difference | -inf | inf | NA | slide | m |
| 3 | fingertip - target y difference | -inf | inf | NA | slide | m |
| 4 | First joint angular velocity | -inf | inf | joint0_vel | hinge | rad/s |
| 5 | Second joint angular velocity | -inf | inf | joint1_vel | hinge | rad/s |
---
## Reward Function Design
The reward for this task is based on the **distance between the fingertip and the target**:
### Distance Reward (tolerance reward)
```text
reward = tolerance(|| fingertip - target ||)
```
- The closer the distance, the higher the reward
---
## Initial State
The initial state is sampled from random distributions:
- Arm angles: uniform distribution
- Arm angular velocities: small random values
- Target point position: random position in a circular area
---
## Episode Termination Conditions
### Termination
If `NaN` appears in the observations
### Termination Handling
```text
reward = 0
terminated = True
```
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env dm-reacher
```
### 2. Start Training
```bash
uv run scripts/train.py --env dm-reacher
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/dm-reacher
```
### 4. Test Training Results
```bash
uv run scripts/play.py --env dm-reacher
```
## Expected Training Results
The robotic arm quickly and accurately reaches the target point

52
docs/source/en/user_guide/demo/dm_walker.md
View File

@@ -1,4 +1,4 @@
# 2D Walker Robot Training Example # 2D Walker Robot
The 2D Walker Robot (Walker2D) is a classic robot control task from DeepMind Control Suite. The goal is to achieve standing, walking, and running by controlling the robot's joints. The 2D Walker Robot (Walker2D) is a classic robot control task from DeepMind Control Suite. The goal is to achieve standing, walking, and running by controlling the robot's joints.
@@ -46,33 +46,17 @@ uv run scripts/train.py --env dm-runner
### 1. Environment Preview ### 1. Environment Preview
```bash ```bash
# View standing task
uv run scripts/view.py --env dm-stander uv run scripts/view.py --env dm-stander
# View walking task
uv run scripts/view.py --env dm-walker uv run scripts/view.py --env dm-walker
# View running task
uv run scripts/view.py --env dm-runner uv run scripts/view.py --env dm-runner
``` ```
### 2. Start Training ### 2. Start Training
```bash ```bash
# Train standing task
uv run scripts/train.py --env dm-stander uv run scripts/train.py --env dm-stander
# Train walking task (default)
uv run scripts/train.py --env dm-walker uv run scripts/train.py --env dm-walker
# Train running task
uv run scripts/train.py --env dm-runner uv run scripts/train.py --env dm-runner
# Customize number of environments
uv run scripts/train.py --env dm-walker --num-envs 512
# Enable rendering (visualize during training)
uv run scripts/train.py --env dm-walker --render
``` ```
### 3. View Training Progress ### 3. View Training Progress
@@ -84,39 +68,9 @@ uv run tensorboard --logdir runs/dm-walker
### 4. Test Training Results ### 4. Test Training Results
```bash ```bash
# Automatically find best policy for testing (recommended) uv run scripts/play.py --env dm-stander
uv run scripts/play.py --env dm-walker uv run scripts/play.py --env dm-walker
uv run scripts/play.py --env dm-runner
# Manually specify policy file for testing
uv run scripts/play.py --env dm-walker --policy runs/dm-walker/nn/best_policy.pickle
```
> **Tip**: The system will automatically find the latest and best policy files in the `runs/dm-walker/` directory for testing. Supports dm-stander, dm-walker, dm-runner three task modes.
## Configuration Parameters
### Environment Configuration
```python
@dataclass
class WalkerEnvCfg(EnvCfg):
model_file: str = "walker.xml" # MJCF model file
max_episode_seconds: float = 25.0 # Maximum episode length
sim_dt: float = 0.0125 # Simulation time step
ctrl_dt: float = 0.025 # Control time step
move_speed: float = 1.0 # Target movement speed
stand_height: float = 1.2 # Target standing height
```
### Training Configuration
```python
@dataclass
class WalkerRLCfg(BaseRLCfg):
num_envs: int = 512 # Number of parallel environments
learning_rate: float = 3e-4 # Learning rate
batch_size: int = 512 # Batch size
max_epochs: int = 1000 # Maximum training epochs
``` ```
## Reward Function Design ## Reward Function Design

249
docs/source/en/user_guide/demo/franka_lift_cube.md Normal file
View File

@@ -0,0 +1,249 @@
# Franka Lift Cube
## Overview
This document describes in detail the cube grasping task environment based on the Franka Emika Panda robotic arm.
```{video} /_static/videos/franka_lift_cube.mp4
:poster: _static/images/poster/franka_lift_cube.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Environment Description
The Franka lift cube task environment is built based on the real Franka Emika Panda 7-DOF robotic arm, designed to train robots to grasp a cube on a table and lift it to a specified target position.
### Robot Structure
Franka Emika Panda is a 7-DOF robotic arm composed of the following main parts:
- **Base**: Robot base fixed to the table
- **7 Joints**:
- joint1 ~ joint4: Shoulder and arm rotation joints
- joint5 ~ joint7: Wrist rotation joints
- **Gripper**: Two-finger gripper, containing two finger joints
- finger_joint1: Left finger joint
- finger_joint2: Right finger joint
- **End Effector (TCP)**: Center point of gripper, used for grasping operations
### Task Objective
The robot needs to complete the following operation objectives:
1. **Approach Target**: Move from initial position to cube position
2. **Grasp Cube**: Close gripper to grasp cube
3. **Lift Cube**: Lift cube to target height
4. **Precise Positioning**: Move cube to specified target position (XYZ 3D coordinates)
The environment provides visualization aids:
- **Cube**: Red cube that can be grasped, initially at random position on table
- **Target Position**: 3D position where the cube should finally reach
---
## Action Space
The action space is `Box(-inf, inf, (8,), float32)`, representing position control commands applied to 8 joints (offsets relative to current joint positions).
### Control Mode
The environment uses position control mode. Actions are converted to joint target positions as follows:
```
Target Joint Angle = Current Joint Angle + Action Value
```
### Action Dimension Details
| Index | Action Description | Control Range | Joint Name | Joint Type |
| ----- | --------------------- | ------------- | -------------- | ---------- |
| 0 | Joint 1 Offset | -inf ~ inf | joint1 | revolve |
| 1 | Joint 2 Offset | -inf ~ inf | joint2 | hinge |
| 2 | Joint 3 Offset | -inf ~ inf | joint3 | hinge |
| 3 | Joint 4 Offset | -inf ~ inf | joint4 | hinge |
| 4 | Joint 5 Offset | -inf ~ inf | joint5 | hinge |
| 5 | Joint 6 Offset | -inf ~ inf | joint6 | hinge |
| 6 | Joint 7 Offset | -inf ~ inf | joint7 | hinge |
| 7 | Gripper Action (Prob) | -inf ~ inf | finger_joint\* | hinge |
### Gripper Control
The gripper action uses probabilistic control:
1. **Sigmoid Mapping**: Map action value to probability in [0, 1] interval
```
p = 1 / (1 + exp(-action))
```
2. **Bernoulli Sampling**: Random sampling based on probability p
- Sample result < p: Gripper closes (0.0)
- Sample result >= p: Gripper opens (0.04)
### Joint Position Limits
All joint positions are clamped to the following ranges after execution:
| Joint | Min | Max |
| ------- | ------- | ------- |
| 1 | -2.8973 | 2.8973 |
| 2 | -1.7628 | 1.7628 |
| 3 | -2.8973 | 2.8973 |
| 4 | -3.0718 | -0.0698 |
| 5 | -2.8973 | 2.8973 |
| 6 | -0.0175 | 3.7525 |
| 7 | -π/2 | π/2 |
| Gripper | 0 | 0.04 |
---
## Observation Space
The observation space is `Box(-inf, inf, (36,), float32)`, containing the robot's proprioceptive information, object state, and action history.
### Observation Components
The observation vector consists of the following parts (in order):
1. **Joint Angles (9 dimensions)**
- 7 robot arm joint angle offsets relative to default pose
- 2 gripper joint angles
2. **Joint Velocities (9 dimensions)**
- Angular velocities of 9 joints
3. **Cube Current Pose (9 dimensions)**
- Position (3 dim): [x, y, z]
- Quaternion (4 dim): [qx, qy, qz, qw]
- Rotation (Euler, 2 dim): [roll, pitch]
4. **Target Position Command (7 dimensions)**
- Target XYZ coordinates (3 dim)
- Target quaternion (4 dim)
5. **Previous Action (8 dimensions)**
### Observation Details
| Index | Observation Content | Dimensions | Unit |
| ----- | ----------------------------------------------- | ---------- | ------------- |
| 0-8 | Joint Angle Offsets (9 joints) | 9 | rad |
| 9-17 | Joint Angular Velocities (9 joints) | 9 | rad/s |
| 18-26 | Cube Current Pose (position + orientation) | 9 | rad |
| 27-33 | Target Position Command (position + quaternion) | 7 | Dimensionless |
| 34-41 | Previous Action (8 dimensions) | 8 | Dimensionless |
---
## Reward Function
The reward function uses a composite design with multiple reward and penalty terms.
### Main Reward Terms
1. **Approach Reward** (Weight: 1.5)
- Formula: `1.5 × (1 - tanh(d_hand_cube / 0.1))`
- Encourages robot end-effector to approach cube
- d_hand_cube: Euclidean distance from end-effector to cube
2. **Lifting Reward** (Weight: 30)
- Condition: Cube height > 0.04m AND end-effector to cube distance < 0.05m
- Encourages robot to grasp and lift cube
3. **Target Tracking Reward** (Variable Weight)
- **Coarse Tracking** (Weight: 10): Uses Sigmoid function, center distance 0.3m
- **Fine Tracking** (Weight: 20): Uses tanh function, scale factor 0.4m
- **Approach Reward** (Weight: 10): Used when distance < 0.2m, scale factor 0.05m
- **Approach Bonus** (Weight: 200): Extra reward, encourages approaching target
- All tracking rewards only active when cube height > 0.04m and grasp successful
### Penalty Terms
Penalty coefficients adjust with training progress:
| Penalty Term | Early Weight (steps < 10000) | Late Weight (steps >= 10000) |
| ---------------------------------- | ---------------------------- | ---------------------------- |
| Action Rate Penalty | 1e-4 | 1e-1 |
| Joint Velocity Squared Sum Penalty | 1e-4 | 1e-1 |
### Calculation Formulas
```
Action Rate = ||current_action - last_action||²
Joint Velocity Squared Sum = ||joint_vel||²
```
---
## Initial State
### Robot Initialization
**Position Initialization:**
The robot's initial position in world coordinates is fixed:
- Base position: Fixed on table
- Joint angles: Set to default pose with random noise added
**Joint Angle Noise:**
Each joint angle has uniform random noise added in range `[-0.125, 0.125]` radians.
**Velocity Initialization:**
All linear and angular velocities are initialized to zero.
### Cube Initialization
Cube position on table is randomly sampled:
- X coordinate: `[-0.1, 0.1]`
- Y coordinate: `[-0.25, 0.25]`
- Z coordinate: Fixed at 0.05 (above table)
### Target Position Generation
Target position is randomly sampled in the following range:
- X coordinate: `[0.4, 0.6]`
- Y coordinate: `[-0.25, 0.25]`
- Z coordinate: `[0.25, 0.5]`
---
## Usage
### Training
```bash
uv run scripts/train.py --env franka-lift-cube
```
### Policy Evaluation
```bash
uv run scripts/play.py --env franka-lift-cube
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/franka-lift-cube
```

250
docs/source/en/user_guide/demo/franka_open_cabinet.md Normal file
View File

@@ -0,0 +1,250 @@
# Franka Open Cabinet
## Overview
This document describes in detail the cabinet opening task environment based on the Franka Emika Panda robotic arm.
```{video} /_static/videos/franka_open_cabinet.mp4
:poster: _static/images/poster/franka_open_cabinet.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## Environment Description
The Franka open cabinet task environment is built based on the real Franka Emika Panda 7-DOF robotic arm, designed to train robots to approach cabinet door handles, grasp them, and pull open drawers.
### Robot Structure
Franka Emika Panda is a 7-DOF robotic arm composed of the following main parts:
- **Base**: Robot base fixed to the ground
- **7 Joints**:
- joint1 ~ joint4: Shoulder and arm rotation joints
- joint5 ~ joint7: Wrist rotation joints
- **Gripper**: Two-finger gripper, containing two finger joints
- finger_joint1: Left finger joint, with contact pad (left_finger_pad)
- finger_joint2: Right finger joint, with contact pad (right_finger_pad)
- **End Effector (TCP)**: Center point of gripper, used for grasping operations
### Environment Objects
- **Cabinet**: Contains one openable drawer
- **Drawer Handle (drawer_top_handle)**: Target part the robot needs to grasp
- **Drawer Joint (drawer_top_joint)**: Sliding joint of drawer, 1 DOF
### Task Objective
The robot needs to complete the following operation objectives:
1. **Approach Handle**: Move from initial position to drawer handle position
2. **Pose Alignment**: Adjust end-effector pose to align with handle
3. **Grasp Handle**: Close gripper to grasp drawer handle
4. **Open Drawer**: Pull backward to open drawer
---
## Action Space
The action space is `Box(-inf, inf, (8,), float32)`, representing position control commands applied to 8 joints (offsets relative to current joint positions).
### Control Mode
The environment uses position control mode. Actions are converted to joint target positions as follows:
```
Target Joint Angle = Current Joint Angle + Action Value
```
### Action Dimension Details
| Index | Action Description | Control Range | Joint Name | Joint Type |
| ----- | --------------------- | ------------- | -------------- | ---------- |
| 0 | Joint 1 Offset | -inf ~ inf | joint1 | revolve |
| 1 | Joint 2 Offset | -inf ~ inf | joint2 | hinge |
| 2 | Joint 3 Offset | -inf ~ inf | joint3 | hinge |
| 3 | Joint 4 Offset | -inf ~ inf | joint4 | hinge |
| 4 | Joint 5 Offset | -inf ~ inf | joint5 | hinge |
| 5 | Joint 6 Offset | -inf ~ inf | joint6 | hinge |
| 6 | Joint 7 Offset | -inf ~ inf | joint7 | hinge |
| 7 | Gripper Action (Prob) | -inf ~ inf | finger_joint\* | hinge |
### Gripper Control
The gripper action uses probabilistic control:
1. **Sigmoid Mapping**: Map action value to probability in [0, 1] interval
```
p = 1 / (1 + exp(-action))
```
2. **Bernoulli Sampling**: Random sampling based on probability p
- Sample result < p: Gripper closes (0.0)
- Sample result >= p: Gripper opens (0.04)
---
## Observation Space
The observation space is `Box(-5, 5, (25,), float32)`, containing the robot's proprioceptive information, task-related information, and drawer state.
### Observation Components
The observation vector consists of the following parts (in order):
1. **Joint Angles (8 dimensions)**
- 7 robot arm joint angles (normalized to [-1, 1])
- Normalization formula: `2 × (Joint Angle - Lower Bound) / (Upper Bound - Lower Bound) - 1`
2. **Joint Velocities (8 dimensions)**
- Angular velocities of 8 joints (divided by 2 for scaling)
3. **Target Relative Pose (7 dimensions)**
- Position Offset (3 dim): Handle position - End-effector position [Δx, Δy, Δz]
- Orientation Offset (4 dim): Handle orientation - End-effector orientation (quaternion)
4. **Drawer Joint Position (1 dimension)**
- Current open distance of drawer
5. **Drawer Joint Velocity (1 dimension)**
- Current opening velocity of drawer
### Observation Details
| Index | Observation Content | Dimensions | Range | Unit |
| ----- | ------------------------------------------- | ---------- | ------------ | ------------- |
| 0-7 | Normalized Joint Angles (8 joints) | 8 | [-1, 1] | Dimensionless |
| 8-15 | Normalized Joint Velocities (8 joints) | 8 | ≈[-π/2, π/2] | rad/s |
| 16-18 | Relative Position to Handle | 3 | [-5, 5] | m |
| 19-22 | Relative Orientation to Handle (quaternion) | 4 | [-5, 5] | Dimensionless |
| 23 | Drawer Joint Position | 1 | [-5, 5] | m |
| 24 | Drawer Joint Velocity | 1 | [-5, 5] | m/s |
All observation values are clipped to [-5, 5] range for numerical stability.
---
## Reward Function
The reward function uses a composite design with multiple reward and penalty terms.
### Main Reward Terms
1. **Distance Reward** (Weight: 10)
- Formula: `10 × (1 - tanh(d_gripper_handle / 0.1))`
- Encourages robot end-effector to approach drawer handle
- d_gripper_handle: Euclidean distance from end-effector to handle
2. **Orientation Matching Reward**
- Formula: Quaternion similarity function
- Encourages robot end-effector orientation to align with handle orientation
3. **Gripper Close Reward** (Conditional Reward)
- When distance < 0.025m: Closing gripper receives +100 reward
- When distance >= 0.025m: Closing gripper receives -20 penalty
- Opening gripper: No reward (0)
- Encourages robot to close gripper to grasp when approaching
4. **Open Drawer Reward** (Exponential Reward)
- Formula: `20 × (exp(open_dist) - 1)`
- open_dist: Drawer open distance (clipped to [0, 1] range)
- Reward grows exponentially as drawer opens more
5. **Prevent Illegal Opening**
- When drawer is already open (open_dist > 0) but end-effector not contacting handle (distance > 0.03m), cancel open reward
- Prevents robot from using other methods to force open drawer
### Penalty Terms
1. **Action Rate Penalty**
- Formula: `||current_action - last_action||²`
2. **Joint Velocity Penalty**
- Formula: `||joint_vel||²`
3. **Finger Position Penetration Penalty**
- Applied when finger contact pads are below handle surface
- Prevents finger model from penetrating drawer
### Penalty Coefficient Scheduling
Penalty coefficients adjust with training progress:
| Penalty Term | Early Weight (steps < 8000) | Late Weight (steps >= 8000) |
| ---------------------- | --------------------------- | --------------------------- |
| Action Rate | 1e-3 | 2e-3 |
| Joint Velocity Squared | 0 | 2e-7 |
### Termination Penalty
When termination condition is triggered, additional -10.0 penalty is applied.
---
## Initial State
### Robot Initialization
**Position Initialization:**
The robot's initial position in world coordinates is fixed:
- Base position: Fixed on ground
- Joint angles: Set to default pose
**Default Joint Pose:**
```
[0.0, -30°, 0°, -156°, 0.0, 186°, -45°, 0.04, 0.04] (radians)
```
**Joint Angle Noise:**
Each joint angle has uniform random noise added in range `[-0.125, 0.125]` radians.
**Velocity Initialization:**
All linear and angular velocities are initialized to zero.
### Cabinet Initialization
Cabinet is fixed on ground with drawer in closed state (joint position at 0).
---
## Usage
### Training
```bash
uv run scripts/train.py --env franka-open-cabinet
```
### Policy Evaluation
```bash
uv run scripts/play.py --env franka-open-cabinet
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/franka-open-cabinet
```

86
docs/source/en/user_guide/demo/locomotion_unitree_go1.md
View File

@@ -1,4 +1,4 @@
# Unitree GO1 Robot Walking Training Example # Unitree GO1 Locomotion
Unitree GO1 is a quadruped robot platform. This example demonstrates how to train GO1 to achieve stable gait walking on flat terrain. Unitree GO1 is a quadruped robot platform. This example demonstrates how to train GO1 to achieve stable gait walking on flat terrain.
@@ -21,82 +21,32 @@ The GO1 quadruped robot has 12 degrees of freedom (3 joints per leg) and needs t
- **Reward Function**: Composite reward including speed tracking, posture stability, energy efficiency, and other components - **Reward Function**: Composite reward including speed tracking, posture stability, energy efficiency, and other components
- **Termination Conditions**: Robot trunk contacts ground or other unstable states - **Termination Conditions**: Robot trunk contacts ground or other unstable states
### Training Task ---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env go1-flat-terrain-walk
```
### 2. Start Training
```bash ```bash
uv run scripts/train.py --env go1-flat-terrain-walk uv run scripts/train.py --env go1-flat-terrain-walk
``` ```
## Configuration Parameters ### 3. View Training Progress
### Environment Configuration ```bash
uv run tensorboard --logdir runs/go1-flat-terrain-walk
```python
@dataclass
class Go1WalkNpEnvCfg(EnvCfg):
max_episode_seconds: float = 20.0 # Maximum episode length
model_file: str = "scene_motor_actuator.xml"
sim_dt: float = 0.01 # Simulation time step
ctrl_dt: float = 0.01 # Control time step
``` ```
### Training Configuration ### 4. Test Training Results
```python ```bash
from dataclasses import dataclass uv run scripts/play.py --env go1-flat-terrain-walk
from motrix_rl.skrl.cfg import PPOCfg
from motrix_rl import registry
@registry.rlcfg("go1-flat-terrain-walk")
@dataclass
class Go1WalkPPO(PPOCfg):
"""
GO1 quadruped robot walking training configuration
"""
seed = 42
max_env_steps: int = 40960000 # Maximum training steps
num_envs: int = 2048 # Number of parallel environments
# Large network structure (suitable for complex robot control tasks)
policy_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
value_hidden_layer_sizes: tuple[int, ...] = (512, 256, 128)
# PPO parameters (optimized for robot tasks)
learning_epochs: int = 2 # Training rounds
mini_batches: int = 32 # Number of mini-batches
learning_rate: float = 1e-3 # Learning rate
```
**Note**: GO1 is a complex task that uses large network structures. If you need to create specialized configurations for different training backends (JAX/Torch), refer to the environment configuration documentation examples.
### Control Configuration
```python
@dataclass
class ControlConfig:
stiffness = 80 # PD controller stiffness [N*m/rad]
damping = 1 # PD controller damping [N*m*s/rad]
action_scale = 0.1 # Action scaling factor
```
### Initial Joint Angles
```python
default_joint_angles = {
"FL_hip": 0.0, # Front left hip joint
"RL_hip": 0.0, # Rear left hip joint
"FR_hip": -0.0, # Front right hip joint
"RR_hip": -0.0, # Rear right hip joint
"FL_thigh": 0.9, # Front left thigh
"RL_thigh": 0.9, # Rear left thigh
"FR_thigh": 0.9, # Front right thigh
"RR_thigh": 0.9, # Rear right thigh
"FL_calf": -1.8, # Front left calf
"RL_calf": -1.8, # Rear left calf
"FR_calf": -1.8, # Front right calf
"RR_calf": -1.8, # Rear right calf
}
``` ```
## Reward Function Design ## Reward Function Design

202
docs/source/en/user_guide/demo/locomotion_unitree_go1_rough_terrain.md Normal file
View File

@@ -0,0 +1,202 @@
# Unitree GO1 Complex Terrain Locomotion
The Unitree GO1 Complex Terrain Walking Environment is a quadruped robot reinforcement learning task designed to train robots to achieve stable walking on challenging terrain. This environment includes two main terrain types: rough terrain and stairs terrain.
```{video} /_static/videos/go1_rough_terrain_walk.mp4
:poster: _static/images/poster/go1_rough_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
```{video} /_static/videos/go1_stairs_terrain_walk.mp4
:poster: _static/images/poster/go1_stairs_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## Task Description
Train the Unitree GO1 quadruped robot to achieve stable and efficient quadruped walking on complex terrain. This environment uses MotrixSim physics engine for simulation, providing high-fidelity dynamic simulation. The agent controls target positions of each joint to achieve velocity tracking and attitude stability while adapting to different terrain challenges.
### Task Objectives
- **Velocity Tracking**: Accurately track given linear and angular velocity commands
- **Attitude Stability**: Maintain body attitude stability under various terrain conditions
- **Energy Efficiency**: Achieve walking tasks with minimal energy consumption
- **Terrain Adaptability**: Adapt to different challenges of rough terrain and stairs terrain
---
## Action Space
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-1.0, 1.0, (12,), float32)` |
| **Dimension** | 12 |
Actions correspond to position control commands for 12 joints, including hip joints, thigh joints, and calf joints for all four legs.
---
## Observation Space
### Rough Terrain Observation Space (48-dimensional)
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (48,), float32)` |
| **Dimension** | 48 |
| Part | Content Description | Dim | Notes |
| --------------------- | ------------------------------------------- | --- | ------------------ |
| **noisy_linvel** | Linear velocity in body coordinate system | 3 | With noise |
| **noisy_gyro** | Angular velocity in body coordinate system | 3 | With noise |
| **local_gravity** | Local gravity direction | 3 | Gravity vector |
| **noisy_joint_angle** | Joint angle deviation from default angles | 12 | 12 joints |
| **noisy_joint_vel** | Joint angular velocities | 12 | With noise |
| **last_actions** | Control actions from previous time step | 12 | Historical actions |
| **command** | Target linear velocity and angular velocity | 3 | [vx, vy, vyaw] |
### Stairs Terrain Observation Space (60-dimensional)
| Item | Details |
| ------------- | -------------------------------- |
| **Type** | `Box(-inf, inf, (60,), float32)` |
| **Dimension** | 60 |
In addition to the rough terrain observations, it includes:
| Part | Content Description | Dim | Notes |
| ---------------------- | ----------------------------------- | --- | ------------------------ |
| **feet_contact_force** | Contact force vectors for four feet | 12 | 3D per foot (Fx, Fy, Fz) |
---
## Reward Function Design
GO1 complex terrain reward function adopts multi-objective weighted design:
```python
# Core reward components
reward_config.scales = {
"tracking_lin_vel": 1.0, # Linear velocity tracking accuracy
"tracking_ang_vel": 0.5, # Angular velocity tracking accuracy
"orientation": -0.0, # Body attitude stability penalty
"torques": -0.00001, # Joint torque penalty (energy efficiency)
"dof_acc": -2.5e-7, # Joint acceleration penalty
"action_rate": -0.001, # Action smoothness penalty
"feet_air_time": 1.0, # Foot air time reward (encourage large strides)
"stand_still": 0.0, # Joint position maintenance for stationary commands
"hip_pos": -1, # Hip joint position preference
"calf_pos": -0.3, # Calf joint position preference
"feet_stumble": -0.5, # Penalty when foot laterally touches obstacle
}
# Total reward = weighted combination of all above terms
```
---
## Initial State
### Rough Terrain Initialization
- **Terrain Generation**: Use height maps to generate random terrain
- **Terrain Height Levels**: Three preset height levels: -2.5m, 0.5m, 2.0m
- **Position Randomization**: At basic training level, robot position is fixed; at advanced level, randomly selects from 25 preset positions in cycles
### Stairs Terrain Initialization
- **Terrain Type**: Various step-based small terrain blocks arranged on continuous terrain
- **Position Randomization**: Similar position randomization strategy as rough terrain
### Robot Initialization
- **Joint Angles**: Set to default standing posture with [-0.125, 0.125] radian noise
- **Velocity Initialization**: All linear and angular velocities initialized to zero
---
## Episode Termination Conditions
- **Body Contact**: Robot trunk makes unexpected contact with ground
- **Velocity Anomaly**: Sum of squared linear velocities exceeds threshold (1e8)
---
## Usage Guide
### 1. Environment Preview
```bash
uv run scripts/view.py --env go1-rough-terrain-walk
uv run scripts/view.py --env go1-stairs-terrain-walk
```
### 2. Start Training
```bash
uv run scripts/train.py --env go1-rough-terrain-walk
uv run scripts/train.py --env go1-stairs-terrain-walk
```
### 3. View Training Progress
```bash
uv run tensorboard --logdir runs/go1-rough-terrain-walk
```
### 4. Test Training Results
Since the rough terrain scene generates both an infinite plane and a rugged terrain height field, when testing training results, the agent will first be spawned on the flat terrain following the training process, complete one round of walking, and then be spawned onto the rugged terrain. Users need to actively adjust camera perspective and position to observe the agent's status.
```bash
uv run scripts/play.py --env go1-rough-terrain-walk
uv run scripts/play.py --env go1-stairs-terrain-walk
```
---
## Expected Training Results
### Rough Terrain Task (go1-rough-terrain-walk)
1. Capable of adapting to different rough terrain heights
2. High velocity tracking accuracy and stable posture
3. Coordinated gait with minimal foot slipping
### Stairs Terrain Task (go1-stairs-terrain-walk)
1. Capable of stably going up and down stairs
2. Capable of adapting to stairs of different heights and widths
3. Smooth movements without obvious stuttering
---
## Training Performance Reference
### go1-rough-terrain-walk
| Operating System | Training Backend | CPU | GPU | Num Environments | Training Time (30000 steps) |
| ---------------- | ---------------- | ----------------- | ----------- | ---------------- | --------------------------- |
| Ubuntu 22.04 | JAX | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 7m20s |
| Ubuntu 22.04 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 8m30s |
| Windows 11 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 10m42s |
### go1-stairs-terrain-walk
| Operating System | Training Backend | CPU | GPU | Num Environments | Training Time (30000 steps) |
| ---------------- | ---------------- | ----------------- | ----------- | ---------------- | --------------------------- |
| Ubuntu 22.04 | JAX | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 7m18s |
| Ubuntu 22.04 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 8m41s |
| Windows 11 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 10m52s |

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

@@ -26,7 +26,15 @@ tutorial/training_and_result
:maxdepth: 1 :maxdepth: 1
demo/cartpole demo/cartpole
demo/bounce_ball
demo/dm_walker demo/dm_walker
demo/dm_cheetah
demo/dm_hopper
demo/dm_reacher
demo/locomotion_unitree_go1 demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c
demo/franka_lift_cube
demo/franka_open_cabinet
``` ```

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

@@ -35,6 +35,76 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:::{grid-item-card} :::{grid-item-card}
```{video} _static/videos/go1_rough_terrain_walk.mp4
:poster: _static/images/poster/go1_rough_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/go1_stairs_terrain_walk.mp4
:poster: _static/images/poster/go1_stairs_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/franka_lift_cube.mp4
:poster: _static/images/poster/franka_lift_cube.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/franka_open_cabinet.mp4
:poster: _static/images/poster/franka_open_cabinet.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/anymal_c.mp4
:poster: _static/images/poster/anymal_c.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_walker.mp4 ```{video} _static/videos/dm_walker.mp4
:poster: _static/images/poster/dm_walker.jpg :poster: _static/images/poster/dm_walker.jpg
:nocontrols: :nocontrols:
@@ -45,6 +115,62 @@ MotrixLab 是一个为机器人训练设计的机器学习通用架构。它基
:width: 100% :width: 100%
``` ```
:::
:::{grid-item-card}
```{video} _static/videos/dm_cheetah.mp4
:poster: _static/images/poster/dm_cheetah.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_hopper.mp4
:poster: _static/images/poster/dm_hopper.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
:::
:::{grid-item-card}
```{video} _static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
::: :::
:::: ::::

316
docs/source/zh_CN/user_guide/demo/anymal_c.md Normal file
View File

@@ -0,0 +1,316 @@
# ANYmal-C Locomotion
## 概述
本文档详细描述了基于 ANYmal-C 四足机器人的导航任务环境。该环境是 MotrixLab 项目中导航任务集的一部分，提供了使用强化学习训练四足机器人执行目标位置和朝向导航的完整实现。
```{video} /_static/videos/anymal_c.mp4
:poster: _static/images/poster/anymal_c.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 环境描述
ANYmal-C 导航任务环境基于真实的 ANYmal-C 四足机器人构建，旨在训练机器人在平面环境中导航至指定的目标位置和朝向。该环境使用 MotrixSim 物理引擎进行仿真，提供高保真的动力学模拟。
### 机器人结构
ANYmal-C 是一个四足机器人，由以下主要部分组成：
- **基座（Base）**：机器人的核心躯干，包含 IMU 传感器、摄像头、激光雷达等传感器模块
- **四条腿**：每条腿包含三个关节
- HAA（Hip Abduction/Adduction）：髋部外展/内收关节
- HFE（Hip Flexion/Extension）：髋部屈伸关节
- KFE（Knee Flexion/Extension）：膝关节屈伸
- **四个足端（Feet）**：球形接触几何体，与地面产生摩擦接触
### 任务目标
机器人需要完成以下导航目标：
1. **位置导航**：移动到指定的目标位置（XY 平面坐标）
2. **朝向控制**：调整机器人朝向至目标航向角（yaw 角）
3. **稳定停止**：到达目标后保持稳定站立，线速度和角速度接近零
环境提供可视化标记：
- **绿色箭头**：指示目标位置和朝向
- **机器人上方绿色箭头**：当前实际运动方向
- **机器人上方蓝色箭头**：期望运动方向
---
## 动作空间
动作空间为 `Box(-1.0, 1.0, (12,), float32)`，表示施加到 12 个关节的位置控制指令（相对于默认站立姿态的偏移量）。
### 控制模式
环境使用位置控制模式，动作通过以下方式转换为关节目标位置：
```
目标关节角度 = 默认关节角度 + (动作值 × 动作缩放系数)
```
其中动作缩放系数由配置参数 `control_config.action_scale` 指定。
### 动作维度详细说明
| 编号 | 动作说明 | 控制范围 | 对应关节名称 | 关节类型 |
| ---- | ------------------- | ---------- | ------------ | -------- |
| 0 | 左前腿髋部外展/内收 | -1.0 ~ 1.0 | LF_HAA | hinge |
| 1 | 左前腿髋部屈伸 | -1.0 ~ 1.0 | LF_HFE | hinge |
| 2 | 左前腿膝关节屈伸 | -1.0 ~ 1.0 | LF_KFE | hinge |
| 3 | 右前腿髋部外展/内收 | -1.0 ~ 1.0 | RF_HAA | hinge |
| 4 | 右前腿髋部屈伸 | -1.0 ~ 1.0 | RF_HFE | hinge |
| 5 | 右前腿膝关节屈伸 | -1.0 ~ 1.0 | RF_KFE | hinge |
| 6 | 左后腿髋部外展/内收 | -1.0 ~ 1.0 | LH_HAA | hinge |
| 7 | 左后腿髋部屈伸 | -1.0 ~ 1.0 | LH_HFE | hinge |
| 8 | 左后腿膝关节屈伸 | -1.0 ~ 1.0 | LH_KFE | hinge |
| 9 | 右后腿髋部外展/内收 | -1.0 ~ 1.0 | RH_HAA | hinge |
| 10 | 右后腿髋部屈伸 | -1.0 ~ 1.0 | RH_HFE | hinge |
| 11 | 右后腿膝关节屈伸 | -1.0 ~ 1.0 | RH_KFE | hinge |
### PD 控制参数
底层使用位置执行器，PD 控制参数在 XML 文件中定义：
- **kp（比例增益）**：200
- **kv（微分增益）**：1
- **力矩限制**：-140 N·m ~ 140 N·m
---
## 观测空间
观测空间为 `Box(-inf, inf, (54,), float32)`，包含机器人的本体感知信息、任务相关信息和历史动作。
### 观测组成部分
观测向量由以下部分组成（按顺序）：
1. **本体感知状态（33 维）**
- 基座线速度（3 维）：机器人基座在世界坐标系中的线速度 [vx, vy, vz]
- 角速度（3 维）：从陀螺仪读取的角速度 [ωx, ωy, ωz]
- 投影重力（3 维）：重力向量在机器人本体坐标系中的投影
- 关节角度（12 维）：12 个关节相对于默认站立姿态的角度偏移
- 关节速度（12 维）：12 个关节的角速度
2. **历史动作（12 维）**
- 上一时刻执行的动作
3. **速度命令（3 维）**
- 期望线速度 XY（2 维）：根据位置误差计算的期望线速度
- 期望角速度 Z（1 维）：根据朝向误差计算的期望角速度
4. **任务状态（6 维）**
- 位置误差向量（2 维）：到目标位置的 XY 平面误差向量（归一化）
- 朝向误差（1 维）：到目标朝向的角度差（归一化到[-1, 1]）
- 距离（1 维）：到目标的欧氏距离（归一化）
- 到达标志（1 维）：是否同时满足位置和朝向到达条件（0 或 1）
- 停止就绪标志（1 维）：是否达到停止标准（到达且角速度接近零）
### 观测详细说明
| 编号 | 观测内容 | 最小值 | 最大值 | 归一化系数 | 单位 |
| ----- | --------------------------------- | ------ | ------ | --------------------- | ---------- |
| 0-2 | 基座线速度 (vx, vy, vz) | -inf | inf | normalization.lin_vel | m/s |
| 3-5 | 角速度 (ωx, ωy, ωz) | -inf | inf | normalization.ang_vel | rad/s |
| 6-8 | 投影重力 (gx, gy, gz) | -1 | 1 | 1.0 | 无量纲 |
| 9-20 | 关节角度偏移（12 个关节） | -inf | inf | normalization.dof_pos | rad |
| 21-32 | 关节角速度（12 个关节） | -inf | inf | normalization.dof_vel | rad/s |
| 33-44 | 上一时刻动作 | -1 | 1 | 1.0 | 无量纲 |
| 45-47 | 速度命令 (vx_cmd, vy_cmd, ωz_cmd) | -inf | inf | commands_scale | m/s, rad/s |
| 48-49 | 位置误差向量 (Δx, Δy) | -inf | inf | 1/5.0 | m |
| 50 | 朝向误差 | -1 | 1 | 1/π | rad |
| 51 | 到目标距离 | 0 | 1 | 1/5.0 (截断) | m |
| 52 | 到达标志 | 0 | 1 | 1.0 | 布尔值 |
| 53 | 停止就绪标志 | 0 | 1 | 1.0 | 布尔值 |
### 传感器信息
环境使用以下传感器获取状态：
- **framelinvel**（名称：base_linvel）：基座线速度传感器
- **gyro**（名称：base_gyro）：陀螺仪传感器，安装在 IMU 站点
---
## 奖励函数
奖励函数采用复合设计，根据机器人是否到达目标使用不同的奖励策略。
### 未到达目标时的奖励
总奖励 = 速度跟踪奖励 + 接近奖励 - 惩罚项
**主要奖励项：**
1. **线速度跟踪奖励**（权重：1.5）
- 计算公式：`1.5 × exp(-||v_xy - v_cmd||² / 0.25)`
- 激励机器人跟踪期望的 XY 平面线速度
2. **角速度跟踪奖励**（权重：0.3）
- 计算公式：`0.3 × exp(-(ωz - ωz_cmd)² / 0.25)`
- 激励机器人跟踪期望的偏航角速度
3. **接近奖励**
- 计算公式：`clip((历史最小距离 - 当前距离) × 4.0, -1.0, 1.0)`
- 奖励机器人每次距离目标更近时的进步
**惩罚项：**
- Z 轴线速度惩罚（权重：2.0）：`-2.0 × vz²`
- XY 轴角速度惩罚（权重：0.05）：`-0.05 × (ωx² + ωy²)`
- 力矩惩罚（权重：0.00001）：`-0.00001 × ||τ||²`
- 动作变化率惩罚（权重：0.001）：`-0.001 × ||Δa||²`
### 到达目标后的奖励
总奖励 = 停止奖励 + 首次到达奖励 - 惩罚项
**主要奖励项：**
1. **停止基础奖励**
- 计算公式：`2 × [0.8 × exp(-(v_xy/0.2)²) + 1.2 × exp(-(ωz/0.1)⁴)]`
- 激励机器人在到达后保持低速度和角速度
2. **零角速度奖励**（额外奖励：6.0）
- 条件：到达目标且 |ωz| < 0.05 rad/s
- 激励机器人完全停止旋转
3. **首次到达奖励**（一次性：10.0）
- 条件：首次同时满足位置和朝向到达条件
- 提供到达目标的明确信号
**惩罚项：**（与未到达时相同）
### 终止条件惩罚
以下情况会额外施加 -20.0 的惩罚：
- 关节速度超限（超过 `max_dof_vel` 配置值）
- 关节速度出现 NaN 或 Inf
- 机器人基座接触地面
- 机器人侧翻（倾斜角度超过 75°）
### 到达判定标准
- **位置到达**：距离目标 < 0.3 米
- **朝向到达**：朝向误差 < 15°
- **完全到达**：同时满足位置到达和朝向到达
- **停止就绪**：完全到达 且 |ωz| < 0.05 rad/s
### info 返回内容
每步返回的 info 字典包含以下调试信息：
- `pose_commands`：当前目标位置和朝向 [x, y, yaw]
- `last_actions`：上一时刻的动作
- `current_actions`：当前时刻的动作
- `steps`：当前回合步数
- `ever_reached`：是否曾经到达过目标
- `min_distance`：历史最小距离（用于计算接近奖励）
---
## 初始状态
### 机器人初始化
**位置初始化：**
机器人在世界坐标系中的初始位置由配置参数 `init_state.pos_randomization_range` 定义的范围内随机采样：
- X 坐标：在 [x_min, x_max] 范围内均匀随机采样
- Y 坐标：在 [y_min, y_max] 范围内均匀随机采样
- Z 坐标：固定在 0.56 米（避免坠落感）
**姿态初始化：**
- 机器人朝向（四元数）：初始化为单位四元数 [0, 0, 0, 1]，表示正向朝向
- 无随机噪声添加到四元数（保证初始稳定性）
**关节初始化：**
关节角度设置为默认站立姿态，由配置参数 `init_state.default_joint_angles` 定义。无随机噪声添加到关节角度（保证初始稳定站立）。
**速度初始化：**
所有线速度和角速度初始化为零，确保机器人从静止状态开始。
### 目标生成
**目标位置：**
目标位置相对于机器人初始位置生成：
```
目标位置 = 机器人初始位置 + 随机偏移
```
随机偏移在配置参数 `commands.pose_command_range` 定义的范围内采样：
- X 方向偏移：[pose_command_range[0], pose_command_range[3]]
- Y 方向偏移：[pose_command_range[1], pose_command_range[4]]
**目标朝向：**
目标朝向（yaw 角）在绝对参考系中随机生成：
- 朝向角度：[pose_command_range[2], pose_command_range[5]]
### 可视化标记初始化
- **目标标记**（绿色箭头）：设置为目标位置和朝向
- **运动方向箭头**：初始化在机器人上方 0.76 米高度处
---
## 回合终止
### 终止条件（Termination）
环境在以下任一条件满足时终止回合：
1. **超时终止**
- 条件：回合达到 `max_episode_second` 配置值
- 说明：防止无限回合
2. **关节速度异常**
- 条件：任一关节速度的绝对值超过 `max_dof_vel`
- 条件：关节速度出现 NaN、Inf 或超过 1e6
- 说明：防止数值发散和物理不稳定
3. **基座接触地面**
- 条件：机器人基座（配置参数 `terminate_after_contacts_on` 定义的几何体）与地面接触
- 说明：机器人跌倒或姿态失败
4. **侧翻倾覆**
- 条件：机器人倾斜角度超过 75°
- 计算：通过投影重力向量计算倾斜角 `arctan2(||g_xy||, |g_z|)`
- 说明：机器人严重侧翻
### 成功条件
虽然环境不会因成功而终止，但任务成功的定义为：
- 机器人到达目标位置和朝向（同时满足位置阈值 < 0.3m 和朝向阈值 < 15°）
- 机器人保持稳定停止（线速度 < 0.05 m/s，角速度 < 0.05 rad/s）

130
docs/source/zh_CN/user_guide/demo/bounce_ball.md Normal file
View File

@@ -0,0 +1,130 @@
# 乒乓球颠球
使用单臂机器人控制挡板颠球，实现连续弹跳并保持球在目标位置。
```{video} /_static/videos/bounce_ball.mp4
:poster: _static/images/poster/bounce_ball.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
Bounce Ball 是一个单臂机器人操作任务，使用 6 自由度的配天 AIR4-560 工业机械臂控制末端挡板（球拍）的位置。智能体通过控制机械臂 6 个关节的位置变化作为动作，使乒乓球在挡板上持续弹跳，并尽可能将球保持在目标高度和目标水平位置。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (6,), float32)` |
| **维度** | 6 |
关节对应如下：
| 序号 | 动作含义（关节位置变化） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | -------------------------- | :----: | :----: | :-------------: |
| 0 | Joint1（基座旋转）位置变化 | -1 | 1 | `Joint1` |
| 1 | Joint2（大臂）位置变化 | -1 | 1 | `Joint2` |
| 2 | Joint3（小臂）位置变化 | -1 | 1 | `Joint3` |
| 3 | Joint4（手腕旋转）位置变化 | -1 | 1 | `Joint4` |
| 4 | Joint5（手腕俯仰）位置变化 | -1 | 1 | `Joint5` |
| 5 | Joint6（手腕旋转）位置变化 | -1 | 1 | `Joint6` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (25,), float32)` |
| **维度** | 25 |
观察空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| ----------- | -------------------- | ---- | --------------------------------------------------------------- |
| **dof_pos** | 各关节自由度位置信息 | 13 | 前 6 个为机械臂关节，后 7 个为球的自由关节（3 位置 + 4 四元数） |
| **dof_vel** | 各关节自由度速度信息 | 12 | 速度为位置导数 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 类型 (单位) |
| ----- | -------------- | ------ | ------ | ----------- | ---------------- |
| 0-5 | 机械臂关节角度 | -Inf | Inf | Joint1-6 | 角度 (rad) |
| 6 | 球 x 坐标 | -Inf | Inf | ball_x | 位置 (m) |
| 7 | 球 y 坐标 | -Inf | Inf | ball_y | 位置 (m) |
| 8 | 球 z 坐标 | -Inf | Inf | ball_z | 位置 (m) |
| 9-12 | 球姿态四元数 | -Inf | Inf | ball_qw/xyz | 四元数 (w,x,y,z) |
| 13-24 | 关节和球速度 | -Inf | Inf | - | 速度/角速度 |
---
## 奖励函数设计
奖励函数由以下几个部分组成：
```python
# 位置控制奖励：保持球在挡板中心上方
# 受控向上速度奖励：在球位置良好时奖励适中的向上速度
# 高度精度奖励：球接近目标高度
# 连续弹跳奖励：奖励连续成功的弹跳
# 总奖励 = 各项加权组合
```
---
## 初始状态
- **机械臂初始位置**：[0, 40, 110, 0, -60, 0] 度，带有随机噪声
- **球的初始位置**：挡板中心上方，带有随机噪声
- **球的初始速度**：[0.0, 0.0, 0.0] m/s
---
## Episode 终止条件
- **球掉落**：球的 z 坐标 < 0.05m（接近地面）
- **球过高**：球的 z 坐标 > 目标高度 + 1.0m（失去控制）
- **水平偏离过远**：球的 x 坐标绝对值 > 1.5m
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env bounce_ball
```
### 2. 开始训练
```bash
uv run scripts/train.py --env bounce_ball
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/bounce_ball
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env bounce_ball
```
---
## 预期训练结果
1. 连续弹跳：能够实现 3 次以上的连续弹跳
2. 位置控制：球的水平位置（x 坐标）稳定在目标位置 ± 0.05m 范围内
3. 高度控制：球的高度稳定在目标高度 0.8 ± 0.1m 范围内
4. 速度控制：球的向上速度保持在合理范围（0.1-1.5 m/s）内
5. 稳定控制：能够持续 20 秒的稳定弹跳而不掉落

153
docs/source/zh_CN/user_guide/demo/cartpole.md
View File

@@ -1,16 +1,89 @@
# 倒立摆训练示例 # 倒立摆
倒立摆（CartPole）是强化学习中的经典控制任务，目标是通过控制小车左右移动来保持杆子平衡。 倒立摆（CartPole）是强化学习中的经典控制任务。其目标是训练一个通过控制小车左右移动来保持杆子平衡的智能体。
![cartpole](/_static/images/poster/cartpole.jpg)
```{video} /_static/videos/cartpole.mp4
:poster: _static/images/poster/cartpole.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述 ## 任务描述
- **状态空间**：小车位置、小车速度、杆子角度、杆子角速度 CartPole 是一个经典的平衡控制任务。环境由一个小车和一根通过铰链连接在小车上的杆组成。智能体通过向左或向右施加力来控制小车移动，使杆子保持直立不倒。该任务要求精确的时序控制和平衡能力。
- **动作空间**：向左或向右施加力
- **奖励函数**：每一步保持杆子不倒下获得+1 奖励
- **终止条件**：杆子角度超过 ±15 度或 episode 长度超过 10 秒
## 快速开始 ---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-3.0, 3.0, (1,), float32)` |
| **维度** | 1 |
动作对应如下：
| 序号 | 动作含义（施加在小车上的力） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 水平方向的力 | -3.0 | 3.0 | `slider` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (4,), float32)` |
| **维度** | 4 |
CartPole 环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| -------- | -------------------- | ---- | -------------- |
| **qpos** | 小车位置和杆子角度 | 2 | 位置和角度信息 |
| **qvel** | 小车速度和杆子角速度 | 2 | 速度为位置导数 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 关节 | 类型 (单位) |
| ---- | ---------- | ------ | ------ | -------- | ----- | -------------- |
| 0 | 小车位置 | -Inf | Inf | slider | slide | 位置 (m) |
| 1 | 杆子角度 | -Inf | Inf | hinge | hinge | 角度 (rad) |
| 2 | 小车速度 | -Inf | Inf | slider | slide | 速度 (m/s) |
| 3 | 杆子角速度 | -Inf | Inf | hinge | hinge | 角速度 (rad/s) |
---
## 奖励函数设计
cartpole 的奖励函数设计如下：
```python
# 每步保持杆子不倒下获得 +1 奖励
reward = 1.0 # 每步固定奖励
```
---
## 初始状态
- **小车初始位置**：0.0 米（中心位置）
- **杆子初始角度**：0.0 弧度（直立）
- **初始速度**：均为 0
- 在初始状态上添加小幅随机噪声（`reset_noise_scale = 0.01`）以增加训练多样性
## Episode 终止条件
- 杆子角度超过 ±0.2 弧度（约 ±11.5 度）
- 小车位置超出 [-0.8, 0.8] 米范围
- 杆子角度出现 NaN 值
- Episode 最大时长：10 秒
---
## 使用指南
### 1. 环境预览 ### 1. 环境预览
@@ -21,14 +94,7 @@ uv run scripts/view.py --env cartpole
### 2. 开始训练 ### 2. 开始训练
```bash ```bash
# 使用默认参数训练
uv run scripts/train.py --env cartpole uv run scripts/train.py --env cartpole
# 自定义环境数量
uv run scripts/train.py --env cartpole --num-envs 1024
# 启用渲染（训练时可视化）
uv run scripts/train.py --env cartpole --render
``` ```
### 3. 查看训练进度 ### 3. 查看训练进度
@@ -40,60 +106,13 @@ uv run tensorboard --logdir runs/cartpole
### 4. 测试训练结果 ### 4. 测试训练结果
```bash ```bash
# 自动寻找最佳策略测试（推荐）
uv run scripts/play.py --env cartpole uv run scripts/play.py --env cartpole
# 手动指定策略文件测试
uv run scripts/play.py --env cartpole --policy runs/cartpole/nn/best_agent.pickle
``` ```
> **提示**：系统会自动在 `runs/cartpole/` 目录下寻找最新、最佳的策略文件进行测试。您也可以通过 `--policy` 参数手动指定特定的策略文件。 ---
## 配置参数 ## 预期训练结果
倒立摆环境的主要配置参数： 1. 杆子角度大部分时间保持在 ±5 度以内
2. Episode 时长接近或超过 10 秒
```python 3. 小车在合理范围内移动以保持平衡
@dataclass
class CartPoleEnvCfg(EnvCfg):
model_file: str = "path/to/inverted_pendulum.xml" # MJCF模型文件
reset_noise_scale: float = 0.01 # 重置噪声
max_episode_seconds: float = 10.0 # 最大episode长度
```
训练配置参数：
```python
@dataclass
class CartPoleRLCfg(BaseRLCfg):
num_envs: int = 2048 # 并行环境数量
learning_rate: float = 3e-4 # 学习率
batch_size: int = 2048 # 批大小
max_epochs: int = 500 # 最大训练轮数
```
## 自定义训练
您可以通过命令行参数覆盖默认配置：
```bash
uv run scripts/train.py --env cartpole \
--num-envs 1024 \
--train-backend jax \
--sim-backend np
```
## 预期结果
- 杆子角度大部分时间保持在 ±5 度以内
- 小车位移范围适中
## 故障排除
如果训练效果不佳，可以尝试：
1. 调整学习率（尝试 1e-4 到 1e-3）
2. 增加环境数量（更多并行训练）
3. 调整奖励函数权重
4. 检查物理参数设置是否合理

131
docs/source/zh_CN/user_guide/demo/dm_cheetah.md Normal file
View File

@@ -0,0 +1,131 @@
# 半猎豹机器人
半猎豹机器人(Cheetah)是 DeepMind Control Suite 中的经典连续控制任务。其目标是训练一个模拟的双足机器人，通过控制其关节力矩，实现高速、稳定地奔跑
```{video} /_static/videos/dm_cheetah.mp4
:poster: _static/images/poster/dm_cheetah.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
HalfCheetah 是一个二维半身猎豹奔跑任务。，由 7 个主要 Body 部位组成（1 个躯干和前后腿各 3 节），拥有 6 个受控关节(前后大腿[连接躯干]、胫骨[连接大腿]和脚[连接胫骨])，智能体通过向这些关节施加扭矩作为动作，让猎豹尽可能快速且稳定地向前奔跑。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (6,), float32)` |
| **维度** | 6 |
关节对应如下：
| 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 后腿大腿关节驱动扭矩 | -1 | 1 | `bthigh` |
| 1 | 后腿小腿关节驱动扭矩 | -1 | 1 | `bshin` |
| 2 | 后腿脚部关节驱动扭矩 | -1 | 1 | `bfoot` |
| 3 | 前腿大腿关节驱动扭矩 | -1 | 1 | `fthigh` |
| 4 | 前腿小腿关节驱动扭矩 | -1 | 1 | `fshin` |
| 5 | 前腿脚部关节驱动扭矩 | -1 | 1 | `ffoot` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (17,), float32)` |
| **维度** | 17 |
HalfCheetah 环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| -------- | ------------- | -- | ------------ |
| **qpos** | 各身体关节与根部的位置信息 | 8 | 默认不包括根部 x 坐标 |
| **qvel** | 各身体关节与根部的速度信息 | 9 | 速度为位置导数 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 关节 | 类型 (单位) |
| -------- | --------------- | ------ | ------ | -------- | ----- | -------------- |
| 0 | 前端 z 坐标 | -Inf | Inf | rootz | slide | 位置 (m) |
| 1 | 前端角度 | -Inf | Inf | rooty | hinge | 角度 (rad) |
| 2 | 后腿大腿角度 | -Inf | Inf | bthigh | hinge | 角度 (rad) |
| 3 | 后腿小腿角度 | -Inf | Inf | bshin | hinge | 角度 (rad) |
| 4 | 后脚角度 | -Inf | Inf | bfoot | hinge | 角度 (rad) |
| 5 | 前腿大腿角度 | -Inf | Inf | fthigh | hinge | 角度 (rad) |
| 6 | 前腿小腿角度 | -Inf | Inf | fshin | hinge | 角度 (rad) |
| 7 | 前脚角度 | -Inf | Inf | ffoot | hinge | 角度 (rad) |
| 8 | 前端 x 坐标速度 | -Inf | Inf | rootx | slide | 速度 (m/s) |
| 9 | 前端 z 坐标速度 | -Inf | Inf | rootz | slide | 速度 (m/s) |
| 10 | 前端角速度 | -Inf | Inf | rooty | hinge | 角速度 (rad/s) |
| 11 | 后腿大腿角速度 | -Inf | Inf | bthigh | hinge | 角速度 (rad/s) |
| 12 | 后腿小腿角速度 | -Inf | Inf | bshin | hinge | 角速度 (rad/s) |
| 13 | 后脚角速度 | -Inf | Inf | bfoot | hinge | 角速度 (rad/s) |
| 14 | 前腿大腿角速度 | -Inf | Inf | fthigh | hinge | 角速度 (rad/s) |
| 15 | 前腿小腿角速度 | -Inf | Inf | fshin | hinge | 角速度 (rad/s) |
| 16 | 前脚角速度 | -Inf | Inf | ffoot | hinge | 角速度 (rad/s) |
| excluded | 前端 x 坐标 | -Inf | Inf | rootx | slide | 位置 (m) |
---
## 奖励函数设计
cheetah 的奖励函数由以下几个部分组成：
```python
# 速度奖励：追踪目标速度
# 姿势奖励：保持稳定的姿势
# 总奖励 = 速度奖励 + 姿势奖励
```
---
## 初始状态
- 重置所有有限关节角度到其允许范围内的随机值，并保持无限幅度关节默认状态。
- 通过多步物理模拟稳定躯干和腿部位置，生成初始观测向量返回。
## Episode 终止条件
- **无跌倒终止条件**（不会因失稳直接结束）
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-cheetah
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-cheetah
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-cheetah
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-cheetah
```
---
## 预期训练结果
1. 接近或超过 30.0 m/s 的稳定水平速度
2. 保持躯干直立且步态协调

149
docs/source/zh_CN/user_guide/demo/dm_hopper.md Normal file
View File

@@ -0,0 +1,149 @@
# 单脚跳跃机器人
单脚跳跃机器人（Hopper）是 DeepMind Control Suite 中的经典连续控制任务。其目标是训练一个模拟的单腿跳跃机器人，通过控制其关节力矩，实现站立平衡或向前跳跃。
```{video} /_static/videos/dm_hopper.mp4
:poster: _static/images/poster/dm_hopper.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
Hopper 是一个二维单腿跳跃机器人任务。由 5 段身体结构组成（torso 躯干、pelvis 骨盆、thigh 大腿、calf 小腿、foot 脚部），拥有 3 个受控关节（thigh_joint 大腿关节、leg_joint 小腿关节、foot_joint 脚部关节），智能体通过向这些关节施加扭矩作为动作，让机器人完成站立平衡或向前跳跃。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (3,), float32)` |
| **维度** | 3 |
关节对应如下：
| 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 大腿转子驱动扭矩 | -1 | 1 | `thigh_joint` |
| 1 | 小腿转子驱动扭矩 | -1 | 1 | `leg_joint` |
| 2 | 脚部转子驱动扭矩 | -1 | 1 | `foot_joint` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (13,), float64)` |
| **维度** | 13 |
Hopper 环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| ------------------- | -------------------- | ---- | ------------------------------- |
| **qpos** | 关节角度与躯干高度 | 5 | 不包括 torso x 坐标（默认隐藏） |
| **qvel** | 关节角速度与躯干速度 | 6 | 所有关节及躯干的速度 |
| **contact sensors** | 脚尖与脚跟触地传感器 | 2 | 使用 `log1p` 正规化处理 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 关节 | 类型 (单位) |
| ---- | --------------- | ------ | ------ | ----------- | ------ | -------------- |
| 0 | 躯干 z 坐标 | -Inf | Inf | rootz | slide | 位置 (m) |
| 1 | 躯干角度 | -Inf | Inf | rooty | hinge | 角度 (rad) |
| 2 | 大腿关节角度 | -Inf | Inf | thigh_joint | hinge | 角度 (rad) |
| 3 | 小腿关节角度 | -Inf | Inf | leg_joint | hinge | 角度 (rad) |
| 4 | 脚部关节角度 | -Inf | Inf | foot_joint | hinge | 角度 (rad) |
| 5 | 躯干 x 坐标速度 | -Inf | Inf | rootx | slide | 速度 (m/s) |
| 6 | 躯干 z 坐标速度 | -Inf | Inf | rootz | slide | 速度 (m/s) |
| 7 | 躯干角速度 | -Inf | Inf | rooty | hinge | 角速度 (rad/s) |
| 8 | 大腿关节角速度 | -Inf | Inf | thigh_joint | hinge | 角速度 (rad/s) |
| 9 | 小腿关节角速度 | -Inf | Inf | leg_joint | hinge | 角速度 (rad/s) |
| 10 | 脚部关节角速度 | -Inf | Inf | foot_joint | hinge | 角速度 (rad/s) |
| 11 | 前脚触地传感器 | -Inf | Inf | touch_toe | sensor | 压力 (无量纲) |
| 12 | 后脚触地传感器 | -Inf | Inf | touch_heel | sensor | 压力 (无量纲) |
---
## 奖励函数设计
hopper 的奖励函数由以下几个部分组成：
### stand 任务（站立）
```python
# 站立奖励：维持目标站立高度稳定性
# 高度奖励：保持躯干在目标高度附近
# 总奖励 = 站立奖励
```
### hop 任务（跳跃）
```python
# 站立奖励：维持目标站立高度稳定性
# 跳跃奖励：达到目标前进速度
# 腿部运动奖励：适度的腿部摆动（抑制过度运动）
# 膝盖伸展奖励：适度的膝盖伸展动作
# 足部接触奖励：保持适当的足部接触力
# 总奖励 = 站立奖励 + 跳跃奖励 + 腿部运动奖励 + 膝盖伸展奖励 + 接触奖励
```
---
## 初始状态
- 重置部分关节角度到其允许范围内的随机值
- 保持有限幅度关节默认状态
## Episode 终止条件
- 机器人的状态观测值出现异常数值（NaN）
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-hopper-stand
uv run scripts/view.py --env dm-hopper-hop
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-hopper-stand
uv run scripts/train.py --env dm-hopper-hop
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-hopper-hop
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-hopper-stand
uv run scripts/play.py --env dm-hopper-hop
```
---
## 预期训练结果
### 站立任务 (dm-hopper-stand)
1. 保持机器人稳定站立
2. 躯干高度保持在 0.6 米附近
### 跳跃任务 (dm-hopper-hop)
1. 机器人实现稳定的向前跳跃
2. 跳跃运动达到 2.0 m/s 的目标速度
3. 步态协调，无明显摔倒

123
docs/source/zh_CN/user_guide/demo/dm_reacher.md Normal file
View File

@@ -0,0 +1,123 @@
# 双关节机械臂控制
双关节机械臂（Reacher）是 DeepMind Control Suite 中的经典操作任务。其目标是训练一个由两段连杆组成的机械臂，通过控制关节力矩，使末端执行器（fingertip）尽可能靠近随机生成的目标点。
```{video} /_static/videos/dm_reacher.mp4
:poster: _static/images/poster/dm_reacher.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
Reacher 是一个二维平面内的双关节机械臂控制任务。由两个通过铰链关节（hinge）连接的连杆组成，拥有 2 个受控关节（joint0 根部关节、joint1 中间关节），智能体通过向这些关节施加扭矩作为动作，使机械臂末端移动到目标点位置。目标点在每个 episode 初始化时随机采样。
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (2,), float32)` |
| **维度** | 2 |
动作对应如下：
| 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 根部关节驱动扭矩 | -1 | 1 | `joint0` |
| 1 | 中间关节驱动扭矩 | -1 | 1 | `joint1` |
---
## 观察空间
| 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-inf, inf, (6,), float32)` |
| **维度** | 6 |
Reacher 环境的观测空间由以下部分组成（按顺序）：
| 部分 | 内容说明 | 维度 | 备注 |
| ---------------------- | -------------------- | ---- | ------------ |
| **qpos** | 2 个关节角度 | 2 | 关节位置信息 |
| **fingertip → target** | 末端到目标的向量差值 | 2 | x、y 两维 |
| **qvel** | 2 个关节角速度 | 2 | 关节速度信息 |
| 序号 | 观察量 | 最小值 | 最大值 | XML 名称 | 关节 | 类型 (单位) |
| ---- | ---------------------------- | ------ | ------ | ---------- | ----- | -------------- |
| 0 | 第一个关节角度 | -Inf | Inf | joint0_pos | hinge | 角度 (rad) |
| 1 | 第二个关节角度 | -Inf | Inf | joint1_pos | hinge | 角度 (rad) |
| 2 | fingertip - target 的 x 差值 | -Inf | Inf | NA | slide | 位置 (m) |
| 3 | fingertip - target 的 y 差值 | -Inf | Inf | NA | slide | 位置 (m) |
| 4 | 第一个关节角速度 | -Inf | Inf | joint0_vel | hinge | 角速度 (rad/s) |
| 5 | 第二个关节角速度 | -Inf | Inf | joint1_vel | hinge | 角速度 (rad/s) |
---
## 奖励函数设计
reacher 的奖励函数基于指尖与目标的距离：
```python
# 距离奖励：指尖越接近目标，奖励越高
reward = tolerance(|| fingertip - target ||)
# 其中 tolerance 是一个单调递减函数
# 距离为 0 时奖励最大，距离越大奖励越小
```
---
## 初始状态
初始状态通过随机分布采样：
- **手臂角度**：均匀分布
- **手臂角速度**：小范围随机值
- **目标点位置**：随机一个圆形区域
## Episode 终止条件
- 若观测中出现 `NaN` 值则终止
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-reacher
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-reacher
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-reacher
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-reacher
```
---
## 预期训练结果
1. 机械臂迅速精准触达目标
2. 末端与目标的平均距离小于 0.01 米
3. 动作平滑，无震荡

154
docs/source/zh_CN/user_guide/demo/dm_walker.md
View File

@@ -1,6 +1,6 @@
# 二维步行机器人训练示例 # 二维步行机器人
二维步行机器人（Walker2D）是基于 DeepMind Control Suite 的经典机器人控制任务，目标是通过控制机器人关节来实现站立、行走和奔跑。 二维步行机器人（Walker2D）是 DeepMind Control Suite 中的经典双足行走任务。其目标是训练一个模拟的双足机器人，通过控制其关节力矩，实现站立、行走和奔跑。
```{video} /_static/videos/dm_walker.mp4 ```{video} /_static/videos/dm_walker.mp4
:poster: _static/images/poster/dm_walker.jpg :poster: _static/images/poster/dm_walker.jpg
@@ -14,89 +14,131 @@
## 任务描述 ## 任务描述
Walker2D 是一个二维平面的双足机器人，具有多个关节和执行器： Walker2D 是一个二维平面的双足机器人任务。由多个身体部位组成，拥有多个受控关节，智能体通过向这些关节施加扭矩作为动作，让机器人实现站立平衡、向前行走或快速奔跑。该任务要求协调的双足步态和平衡控制能力。
- **状态空间**：包括机器人各部位的旋转角度、角速度、躯干高度和速度等 ---
- **动作空间**：控制各个关节的力矩
- **奖励函数**：主要由保持站立、前进速度等组成
- **终止条件**：机器人摔倒或关节达到极限位置
### 三种任务模式 ## 动作空间（Action Space）
1. **dm-stander**: 静止站立任务 (move_speed = 0.0) | 项目 | 详细信息 |
| -------- | ------------------------------- |
| **类型** | `Box(-1.0, 1.0, (6,), float32)` |
| **维度** | 6 |
```bash 动作对应如下：
uv run scripts/train.py --env dm-stander
```
2. **dm-walker**: 行走任务 (move_speed = 1.0) | 序号 | 动作含义（施加在关节的力矩） | 最小值 | 最大值 | 对应 XML 中名称 |
| ---: | ---------------------------- | :----: | :----: | :-------------: |
| 0 | 右大腿关节驱动扭矩 | -1 | 1 | `right_thigh` |
| 1 | 右小腿关节驱动扭矩 | -1 | 1 | `right_leg` |
| 2 | 右脚关节驱动扭矩 | -1 | 1 | `right_foot` |
| 3 | 左大腿关节驱动扭矩 | -1 | 1 | `left_thigh` |
| 4 | 左小腿关节驱动扭矩 | -1 | 1 | `left_leg` |
| 5 | 左脚关节驱动扭矩 | -1 | 1 | `left_foot` |
```bash ---
uv run scripts/train.py --env dm-walker
```
3. **dm-runner**: 奔跑任务 (move_speed = 5.0) ## 观察空间
```bash | 项目 | 详细信息 |
uv run scripts/train.py --env dm-runner | -------- | -------------------------------- |
``` | **类型** | `Box(-inf, inf, (17,), float32)` |
| **维度** | 17 |
## 配置参数 Walker2D 环境的观测空间由以下部分组成（按顺序）：
### 环境配置 | 部分 | 内容说明 | 维度 | 备注 |
| -------- | -------------------------- | ---- | -------------------- |
| **qpos** | 各身体关节与根部的位置信息 | 9 | 包括躯干高度和角度 |
| **qvel** | 各身体关节与根部的速度信息 | 8 | 所有关节及躯干的速度 |
```python ---
@dataclass
class WalkerEnvCfg(EnvCfg):
model_file: str = "walker.xml" # MJCF模型文件
max_episode_seconds: float = 25.0 # 最大episode长度
sim_dt: float = 0.0125 # 仿真时间步
ctrl_dt: float = 0.025 # 控制时间步
move_speed: float = 1.0 # 目标移动速度
stand_height: float = 1.2 # 目标站立高度
```
### 训练配置
```python
@dataclass
class WalkerRLCfg(BaseRLCfg):
num_envs: int = 512 # 并行环境数量
learning_rate: float = 3e-4 # 学习率
batch_size: int = 512 # 批大小
max_epochs: int = 1000 # 最大训练轮数
```
## 奖励函数设计 ## 奖励函数设计
Walker2D 的奖励函数由以下几个部分组成： walker 的奖励函数由以下几个部分组成：
### 基础站立奖励 ### 基础站立奖励
```python ```python
# 高度奖励：保持躯干在目标高度 # 高度奖励：保持躯干在目标高度
# 直立奖励：保持躯干直立 # 直立奖励：保持躯干直立
# 总奖励 = 高度奖励 + 直立奖励
``` ```
### 移动奖励（行走和奔跑任务） ### 移动奖励（行走和奔跑任务）
```python ```python
# 速度奖励：追踪目标速度 # 速度奖励：追踪目标前进速度
# 总奖励 = 站立奖励 * 移动权重 # 站立奖励：保持躯干在目标高度
# 直立奖励：保持躯干直立
# 总奖励 = 速度奖励 + 站立奖励 + 直立奖励
``` ```
## 预期结果 ---
1. **dm-stander**： ## 初始状态
- 躯干高度保持在 1.0-1.4m 范围 - 重置所有有限关节角度到其允许范围内的随机值
- 躯干直立角度偏差小于 15 度 - 保持无限幅度关节默认状态
2. **dm-walker**： ## Episode 终止条件
- 实际行走速度接近 1.0 m/s - 机器人的状态观测值出现异常数值（NaN）
- 步态协调，无明显摔倒 - 机器人躯干接触地面（跌倒）
3. **dm-runner**： ---
- 奔跑速度达到 4.0-5.0 m/s
- 出现飞行相（双脚同时离地） ## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env dm-stander
uv run scripts/view.py --env dm-walker
uv run scripts/view.py --env dm-runner
```
### 2. 开始训练
```bash
uv run scripts/train.py --env dm-stander
uv run scripts/train.py --env dm-walker
uv run scripts/train.py --env dm-runner
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/dm-walker
```
### 4. 测试训练结果
```bash
uv run scripts/play.py --env dm-stander
uv run scripts/play.py --env dm-walker
uv run scripts/play.py --env dm-runner
```
---
## 预期训练结果
### 站立任务 (dm-stander)
1. 躯干高度保持在 1.0-1.4m 范围
2. 躯干直立角度偏差小于 15 度
3. 能够稳定站立不倒
### 行走任务 (dm-walker)
1. 实际行走速度接近 1.0 m/s
2. 步态协调，无明显摔倒
3. 能够持续稳定行走
### 奔跑任务 (dm-runner)
1. 奔跑速度达到 4.0-5.0 m/s
2. 出现飞行相（双脚同时离地）
3. 步态协调稳定

255
docs/source/zh_CN/user_guide/demo/franka_lift_cube.md Normal file
View File

@@ -0,0 +1,255 @@
# Franka 抓取立方体
## 概述
本文档详细描述了基于 Franka Emika Panda 机械臂的抓取立方体任务环境。
```{video} /_static/videos/franka_lift_cube.mp4
:poster: _static/images/poster/franka_lift_cube.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 环境描述
Franka 抓取立方体任务环境基于真实的 Franka Emika Panda 7 自由度机械臂构建，旨在训练机器人在桌面上抓取立方体并将其提升到指定目标位置。
### 机器人结构
Franka Emika Panda 是一个 7 自由度机械臂，由以下主要部分组成：
- **基座（Base）**：固定在桌面上的机器人基座
- **7 个关节**：
- joint1 ~ joint4：肩部和臂部旋转关节
- joint5 ~ joint7：手腕部旋转关节
- **夹爪（Gripper）**：双指夹爪，包含两个手指关节
- finger_joint1：左指关节
- finger_joint2：右指关节
- **末端执行器（TCP）**：夹爪中心点，用于抓取操作
### 任务目标
机器人需要完成以下操作目标：
1. **接近目标**：从初始位置移动到立方体位置
2. **抓取立方体**：闭合夹爪抓取立方体
3. **提升立方体**：将立方体提升到目标高度
4. **精确定位**：将立方体移动到指定的目标位置（XYZ 三维坐标）
环境提供可视化辅助：
- **立方体**：可抓取的红色立方体，初始位于桌面上的随机位置
- **目标位置**：任务指定的立方体最终应到达的三维位置
---
## 动作空间
动作空间为 `Box(-inf, inf, (8,), float32)`，表示施加到 8 个关节的位置控制指令（相对于当前关节位置的偏移量）。
### 控制模式
环境使用位置控制模式，动作通过以下方式转换为关节目标位置：
```
目标关节角度 = 当前关节角度 + 动作值
```
### 动作维度详细说明
| 编号 | 动作说明 | 控制范围 | 对应关节名称 | 关节类型 |
| ---- | ---------------- | ---------- | -------------- | -------- |
| 0 | 关节 1 偏移量 | -inf ~ inf | joint1 | revolve |
| 1 | 关节 2 偏移量 | -inf ~ inf | joint2 | hinge |
| 2 | 关节 3 偏移量 | -inf ~ inf | joint3 | hinge |
| 3 | 关节 4 偏移量 | -inf ~ inf | joint4 | hinge |
| 4 | 关节 5 偏移量 | -inf ~ inf | joint5 | hinge |
| 5 | 关节 6 偏移量 | -inf ~ inf | joint6 | hinge |
| 6 | 关节 7 偏移量 | -inf ~ inf | joint7 | hinge |
| 7 | 夹爪动作（概率） | -inf ~ inf | finger_joint\* | hinge |
### 夹爪控制
夹爪动作使用概率控制：
1. **Sigmoid 映射**：将动作值映射到 [0, 1] 区间的概率
```
p = 1 / (1 + exp(-action))
```
2. **伯努利采样**：根据概率 p 进行随机采样
- 采样结果 < p：夹爪闭合（0.0）
- 采样结果 >= p：夹爪打开（0.04）
### 关节位置限制
所有关节位置在执行后被限制在以下范围内：
| 关节 | 最小值 | 最大值 |
| ---- | ------- | ------- |
| 1 | -2.8973 | 2.8973 |
| 2 | -1.7628 | 1.7628 |
| 3 | -2.8973 | 2.8973 |
| 4 | -3.0718 | -0.0698 |
| 5 | -2.8973 | 2.8973 |
| 6 | -0.0175 | 3.7525 |
| 7 | -π/2 | π/2 |
| 夹爪 | 0 | 0.04 |
---
## 观测空间
观测空间为 `Box(-inf, inf, (36,), float32)`，包含机器人的本体感知信息、物体状态和历史动作。
### 观测组成部分
观测向量由以下部分组成（按顺序）：
1. **关节角度（9 维）**
- 7 个机械臂关节相对于默认姿态的角度偏移
- 2 个夹爪关节角度
2. **关节速度（9 维）**
- 9 个关节的角速度
3. **立方体当前位姿（9 维）**
- 位置（3 维）：[x, y, z]
- 四元数（4 维）：[qx, qy, qz, qw]
- 旋转（Euler，2 维）：[roll, pitch]
4. **目标位置命令（7 维）**
- 目标 XYZ 坐标（3 维）
- 目标四元数（4 维）
5. **上一时刻动作（8 维）**
### 观测详细说明
| 编号 | 观测内容 | 维度 | 单位 |
| ----- | --------------------------- | ---- | ------ |
| 0-8 | 关节角度偏移（9 个关节） | 9 | rad |
| 9-17 | 关节角速度（9 个关节） | 9 | rad/s |
| 18-26 | 立方体当前位姿（位置+姿态） | 9 | rad |
| 27-33 | 目标位置命令（位置+四元数） | 7 | 无量纲 |
| 34-41 | 上一时刻动作（8 维） | 8 | 无量纲 |
---
## 奖励函数
奖励函数采用复合设计，包含多个奖励和惩罚项。
### 主要奖励项
1. **接近奖励**（权重：1.5）
- 计算公式：`1.5 × (1 - tanh(d_hand_cube / 0.1))`
- 激励机器人末端接近立方体
- d_hand_cube：末端到立方体的欧氏距离
2. **提升奖励**（权重：30）
- 条件：立方体高度 > 0.04m 且末端与立方体距离 < 0.05m
- 激励机器人抓取并提升立方体
3. **目标跟踪奖励**（变权重）
- **粗略跟踪**（权重：10）：使用 Sigmoid 函数，中心距离 0.3m
- **精细跟踪**（权重：20）：使用 tanh 函数，比例系数 0.4m
- **接近奖励**（权重：10）：距离 < 0.2m 时使用，比例系数 0.05m
- **接近加成**（权重：200）：额外奖励，鼓励接近目标
- 所有跟踪奖励仅在立方体高度 > 0.04m 且抓取成功时生效
### 惩罚项
惩罚项系数随训练进度调整：
| 惩罚项 | 前期权重（steps < 10000） | 后期权重（steps >= 10000） |
| ------------------ | ------------------------- | -------------------------- |
| 动作变化率惩罚 | 1e-4 | 1e-1 |
| 关节速度平方和惩罚 | 1e-4 | 1e-1 |
### 计算公式
```
动作变化率 = ||current_action - last_action||²
关节速度平方和 = ||joint_vel||²
```
---
## 初始状态
### 机器人初始化
**位置初始化：**
机器人在世界坐标系中的初始位置固定：
- 基座位置：固定在桌面上
- 关节角度：设置为默认姿态，并添加随机噪声
**关节角度噪声：**
每个关节角度在 `[-0.125, 0.125]` 弧度范围内添加均匀随机噪声。
**速度初始化：**
所有线速度和角速度初始化为零。
### 立方体初始化
立方体在桌面上的位置随机采样：
- X 坐标：`[-0.1, 0.1]`
- Y 坐标：`[-0.25, 0.25]`
- Z 坐标：固定为 0.05 桌面以上）
### 目标位置生成
目标位置在以下范围内随机采样：
- X 坐标：`[0.4, 0.6]`
- Y 坐标：`[-0.25, 0.25]`
- Z 坐标：`[0.25, 0.5]`
---
## 使用方法
### 训练
```bash
uv run scripts/train.py --env franka-lift-cube
```
### 策略评估
```bash
uv run scripts/play.py --env franka-lift-cube
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/franka-lift-cube
```

250
docs/source/zh_CN/user_guide/demo/franka_open_cabinet.md Normal file
View File

@@ -0,0 +1,250 @@
# Franka 打开抽屉
## 概述
本文档详细描述了基于 Franka Emika Panda 机械臂的打开抽屉任务环境。
```{video} /_static/videos/franka_open_cabinet.mp4
:poster: _static/images/poster/franka_open_cabinet.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
## 环境描述
Franka 打开柜门任务环境基于真实的 Franka Emika Panda 7 自由度机械臂构建，旨在训练机器人接近柜门把手、抓取并拉开抽屉。
### 机器人结构
Franka Emika Panda 是一个 7 自由度机械臂，由以下主要部分组成：
- **基座（Base）**：固定在地面上的机器人基座
- **7 个关节**：
- joint1 ~ joint4：肩部和臂部旋转关节
- joint5 ~ joint7：手腕部旋转关节
- **夹爪（Gripper）**：双指夹爪，包含两个手指关节
- finger_joint1：左指关节，带有接触垫（left_finger_pad）
- finger_joint2：右指关节，带有接触垫（right_finger_pad）
- **末端执行器（TCP）**：夹爪中心点，用于抓取操作
### 环境物体
- **柜子**：包含一个可打开的抽屉
- **抽屉把手（drawer_top_handle）**：机器人需要抓取的目标部位
- **抽屉关节（drawer_top_joint）**：抽屉的滑动关节，1 个自由度
### 任务目标
机器人需要完成以下操作目标：
1. **接近把手**：从初始位置移动到抽屉把手位置
2. **姿态对齐**：调整末端姿态与把手对齐
3. **抓取把手**：闭合夹爪抓取抽屉把手
4. **拉开抽屉**：向后拉动抽屉将其打开
---
## 动作空间
动作空间为 `Box(-inf, inf, (8,), float32)`，表示施加到 8 个关节的位置控制指令（相对于当前关节位置的偏移量）。
### 控制模式
环境使用位置控制模式，动作通过以下方式转换为关节目标位置：
```
目标关节角度 = 当前关节角度 + 动作值
```
### 动作维度详细说明
| 编号 | 动作说明 | 控制范围 | 对应关节名称 | 关节类型 |
| ---- | ---------------- | ---------- | -------------- | -------- |
| 0 | 关节 1 偏移量 | -inf ~ inf | joint1 | revolve |
| 1 | 关节 2 偏移量 | -inf ~ inf | joint2 | hinge |
| 2 | 关节 3 偏移量 | -inf ~ inf | joint3 | hinge |
| 3 | 关节 4 偏移量 | -inf ~ inf | joint4 | hinge |
| 4 | 关节 5 偏移量 | -inf ~ inf | joint5 | hinge |
| 5 | 关节 6 偏移量 | -inf ~ inf | joint6 | hinge |
| 6 | 关节 7 偏移量 | -inf ~ inf | joint7 | hinge |
| 7 | 夹爪动作（概率） | -inf ~ inf | finger_joint\* | hinge |
### 夹爪控制
夹爪动作使用概率控制：
1. **Sigmoid 映射**：将动作值映射到 [0, 1] 区间的概率
```
p = 1 / (1 + exp(-action))
```
2. **伯努利采样**：根据概率 p 进行随机采样
- 采样结果 < p：夹爪闭合（0.0）
- 采样结果 >= p：夹爪打开（0.04）
---
## 观测空间
观测空间为 `Box(-5, 5, (25,), float32)`，包含机器人的本体感知信息、任务相关信息和抽屉状态。
### 观测组成部分
观测向量由以下部分组成（按顺序）：
1. **关节角度（8 维）**
- 7 个机械臂关节角度（归一化到 [-1, 1]）
- 归一化公式：`2 × (关节角度 - 下限) / (上限 - 下限) - 1`
2. **关节速度（8 维）**
- 8 个关节的角速度（除以 2 进行缩放）
3. **目标相对位姿（7 维）**
- 位置偏差（3 维）：把手位置 - 末端位置 [Δx, Δy, Δz]
- 姿态偏差（4 维）：把手姿态 - 末端姿态（四元数）
4. **抽屉关节位置（1 维）**
- 抽屉当前打开距离
5. **抽屉关节速度（1 维）**
- 抽屉当前打开速度
### 观测详细说明
| 编号 | 观测内容 | 维度 | 范围 | 单位 |
| ----- | -------------------------- | ---- | ------------ | ------ |
| 0-7 | 归一化关节角度（8 个关节） | 8 | [-1, 1] | 无量纲 |
| 8-15 | 归一化关节速度（8 个关节） | 8 | ≈[-π/2, π/2] | rad/s |
| 16-18 | 到把手的相对位置 | 3 | [-5, 5] | m |
| 19-22 | 到把手的相对姿态（四元数） | 4 | [-5, 5] | 无量纲 |
| 23 | 抽屉关节位置 | 1 | [-5, 5] | m |
| 24 | 抽屉关节速度 | 1 | [-5, 5] | m/s |
所有观测值被裁剪在 [-5, 5] 范围内以保持数值稳定性。
---
## 奖励函数
奖励函数采用复合设计，包含多个奖励和惩罚项。
### 主要奖励项
1. **距离奖励**（权重：10）
- 计算公式：`10 × (1 - tanh(d_gripper_handle / 0.1))`
- 激励机器人末端接近抽屉把手
- d_gripper_handle：末端到把手的欧氏距离
2. **姿态匹配奖励**
- 计算公式：四元数相似度函数
- 激励机器人末端姿态与把手姿态对齐
3. **夹爪闭合奖励**（条件奖励）
- 当距离 < 0.025m：闭合夹爪获得 +100 奖励
- 当距离 >= 0.025m：闭合夹爪获得 -20 惩罚
- 打开夹爪：无奖励（0）
- 激励机器人在接近时闭合夹爪抓取
4. **打开抽屉奖励**（指数奖励）
- 计算公式：`20 × (exp(open_dist) - 1)`
- open_dist：抽屉打开距离（裁剪在 [0, 1] 范围）
- 随着抽屉打开程度增加，奖励指数增长
5. **防止非法打开**
- 当抽屉已打开（open_dist > 0）但末端未接触把手（距离 > 0.03m）时，取消打开奖励
- 防止机器人使用其他方式强行打开抽屉
### 惩罚项
1. **动作变化率惩罚**
- 计算公式：`||current_action - last_action||²`
2. **关节速度惩罚**
- 计算公式：`||joint_vel||²`
3. **手指位置穿透惩罚**
- 当手指接触垫低于把手表面时施加惩罚
- 防止手指模型穿透抽屉
### 惩罚系数调度
惩罚项系数随训练进度调整：
| 惩罚项 | 前期权重（steps < 8000） | 后期权重（steps >= 8000） |
| -------------- | ------------------------ | ------------------------- |
| 动作变化率 | 1e-3 | 2e-3 |
| 关节速度平方和 | 0 | 2e-7 |
### 终止惩罚
当触发终止条件时，额外施加 -10.0 惩罚。
---
## 初始状态
### 机器人初始化
**位置初始化：**
机器人在世界坐标系中的初始位置固定：
- 基座位置：固定在地面上
- 关节角度：设置为默认姿态
**默认关节姿态：**
```
[0.0, -30°, 0°, -156°, 0.0, 186°, -45°, 0.04, 0.04] (弧度)
```
**关节角度噪声：**
每个关节角度在 `[-0.125, 0.125]` 弧度范围内添加均匀随机噪声。
**速度初始化：**
所有线速度和角速度初始化为零。
### 柜子初始化
柜子固定在地面上，抽屉处于关闭状态（关节位置为 0）。
---
## 使用方法
### 训练
```bash
uv run scripts/train.py --env franka-open-cabinet
```
### 策略评估
```bash
uv run scripts/play.py --env franka-open-cabinet
```
### TensorBoard
```bash
uv run tensorboard --logdir runs/franka-open-cabinet
```

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

@@ -1,4 +1,4 @@
# Unitree GO1 机器人行走训练示例 # Unitree GO1 平地行走
Unitree GO1 是一个四足机器人平台，本示例展示了如何训练 GO1 在平坦地形上实现稳定的步态行走。 Unitree GO1 是一个四足机器人平台，本示例展示了如何训练 GO1 在平坦地形上实现稳定的步态行走。
@@ -14,68 +14,55 @@ Unitree GO1 是一个四足机器人平台，本示例展示了如何训练 GO1
## 任务描述 ## 任务描述
GO1 四足机器人具有 12 个自由度（每条腿 3 个关节），需要通过深度强化学习学习协调的步态控制： GO1 四足机器人具有 12 个自由度（每条腿 3 个关节），需要通过深度强化学习学习协调的步态控制。该环境使用 MotrixSim 物理引擎进行仿真，提供高保真的动力学模拟。智能体通过控制各关节的目标位置（通过 PD 控制器转换为力矩）来实现速度跟踪和姿态稳定。
- **状态空间**：48 维，包含机器人线速度、角速度、姿态、关节角度、关节速度、动作和命令等 ---
- **动作空间**：12 维，控制各个关节的目标位置（通过 PD 控制器转换为力矩）
- **奖励函数**：复合奖励，包含速度跟踪、姿态稳定、能量效率等多个组件
- **终止条件**：机器人躯干接触地面或其他不稳定状态
### 训练任务 ## 动作空间（Action Space）
```bash | 项目 | 详细信息 |
uv run scripts/train.py --env go1-flat-terrain-walk | -------- | -------------------------------- |
``` | **类型** | `Box(-1.0, 1.0, (12,), float32)` |
| **维度** | 12 |
## 配置参数 动作对应 12 个关节的位置控制指令（相对于默认站立姿态的偏移量），包括：
### 环境配置 | 序号 | 动作含义（关节位置变化） | 对应肢体 |
| ---: | -------------------------- | :------: |
| 0-2 | 髋关节、大腿关节、小腿关节 | 前左腿 |
| 3-5 | 髋关节、大腿关节、小腿关节 | 后左腿 |
| 6-8 | 髋关节、大腿关节、小腿关节 | 前右腿 |
| 9-11 | 髋关节、大腿关节、小腿关节 | 后右腿 |
```python ---
@dataclass
class Go1WalkNpEnvCfg(EnvCfg):
max_episode_seconds: float = 20.0 # 最大episode长度
model_file: str = "scene_motor_actuator.xml"
sim_dt: float = 0.01 # 仿真时间步
ctrl_dt: float = 0.01 # 控制时间步
```
### 控制配置 ## 观察空间
```python | 项目 | 详细信息 |
@dataclass | -------- | -------------------------------- |
class ControlConfig: | **类型** | `Box(-inf, inf, (48,), float32)` |
stiffness = 80 # PD 控制器刚度 [N*m/rad] | **维度** | 48 |
damping = 1 # PD 控制器阻尼 [N*m*s/rad]
action_scale = 0.05 # 动作缩放因子
```
### 初始关节角度 GO1 环境的观测空间由以下部分组成（按顺序）：
```python | 部分 | 内容说明 | 维度 | 备注 |
default_joint_angles = { | --------------------- | ---------------- | ---- | ------------------ |
"FL_hip": 0.0, # 前左髋关节 | **noisy_linvel** | 局部坐标系线速度 | 3 | 带噪声的线速度 |
"RL_hip": 0.0, # 后左髋关节 | **noisy_gyro** | 陀螺仪数据 | 3 | 带噪声的角速度 |
"FR_hip": -0.0, # 前右髋关节 | **local_gravity** | 局部重力方向 | 3 | 重力向量投影 |
"RR_hip": -0.0, # 后右髋关节 | **noisy_joint_angle** | 关节角度 | 12 | 相对于默认值的偏差 |
"FL_thigh": 0.9, # 前左大腿 | **noisy_joint_vel** | 关节速度 | 12 | 带噪声的关节速度 |
"RL_thigh": 0.9, # 后左大腿 | **last_actions** | 上一帧动作 | 12 | 历史动作信息 |
"FR_thigh": 0.9, # 前右大腿 | **command** | 速度命令 | 3 | [vx, vy, vyaw] |
"RR_thigh": 0.9, # 后右大腿
"FL_calf": -1.8, # 前左小腿 ---
"RL_calf": -1.8, # 后左小腿
"FR_calf": -1.8, # 前右小腿
"RR_calf": -1.8, # 后右小腿
}
```
## 奖励函数设计 ## 奖励函数设计
GO1 的奖励函数是一个复杂的复合函数，包含多个组件： GO1 的奖励函数是一个复杂的复合函数，包含多个组件：
### 主要奖励组件
```python ```python
# 主要奖励组件
reward_config.scales = { reward_config.scales = {
"tracking_lin_vel": 1.0, # 线速度跟踪奖励 "tracking_lin_vel": 1.0, # 线速度跟踪奖励
"tracking_ang_vel": 0.5, # 角速度跟踪奖励 "tracking_ang_vel": 0.5, # 角速度跟踪奖励
@@ -89,51 +76,57 @@ reward_config.scales = {
"hip_pos": -1, # 髋关节位置惩罚 "hip_pos": -1, # 髋关节位置惩罚
"calf_pos": -0.3, # 腿关节位置惩罚 "calf_pos": -0.3, # 腿关节位置惩罚
} }
# 总奖励 = 加权组合以上所有项
``` ```
### 关键奖励函数 ---
#### 速度跟踪奖励 ## 初始状态
```python - **机器人位置**：固定在初始位置
# 跟踪线速度命令（xy平面） - **关节角度**：设置为默认站立姿态
def _reward_tracking_lin_vel(self, data, commands): - **关节角度噪声**：每个关节在 [-0.125, 0.125] 弧度范围内添加随机噪声
- **速度初始化**：所有线速度和角速度初始化为零
# 跟踪角速度命令（偏航） ## Episode 终止条件
def _reward_tracking_ang_vel(self, data, commands):
- **身体接触地面**：机器人躯干与地面发生非预期接触
- **速度异常**：线速度平方和超过阈值（1e8）
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env go1-flat-terrain-walk
``` ```
#### 足部空中时间奖励 ### 2. 开始训练
```python ```bash
def _reward_feet_air_time(self, commands, info): uv run scripts/train.py --env go1-flat-terrain-walk
``` ```
## 观察空间构成 ### 3. 查看训练进度
GO1 的观察空间为 48 维，包含以下信息： ```bash
uv run tensorboard --logdir runs/go1-flat-terrain-walk
```python
obs = np.hstack([
noisy_linvel, # 3维：局部坐标系线速度
noisy_gyro, # 3维：陀螺仪数据
local_gravity, # 3维：局部重力方向
noisy_joint_angle, # 12维：关节角度（相对于默认值）
noisy_joint_vel, # 12维：关节速度
last_actions, # 12维：上一帧动作
command, # 3维：速度命令 [vx, vy, vyaw]
])
``` ```
## 运动速度命令生成 ### 4. 测试训练结果
训练过程中随机生成速度命令，确保智能体能够跟踪不同的移动速度： ```bash
uv run scripts/play.py --env go1-flat-terrain-walk
```python
def resample_commands(self, num_envs: int):
``` ```
---
## 预期训练结果 ## 预期训练结果
1. 稳定的四足步态 1. 稳定的四足步态（trot 步态或其他协调步态）
2. 良好的速度跟踪 2. 良好的速度跟踪能力
3. 能够跟踪不同的速度命令（前进、转向）
4. 姿态稳定，无明显侧翻

198
docs/source/zh_CN/user_guide/demo/locomotion_unitree_go1_rough_terrain.md Normal file
View File

@@ -0,0 +1,198 @@
# Unitree GO1 复杂地形行走
Unitree GO1 复杂地形行走环境是一个四足机器人强化学习任务，旨在训练机器人在具有挑战性的地形上实现稳定行走。该环境包含两种主要的地形类型：粗糙地形和台阶地形。
```{video} /_static/videos/go1_rough_terrain_walk.mp4
:poster: _static/images/poster/go1_rough_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
---
```{video} /_static/videos/go1_stairs_terrain_walk.mp4
:poster: _static/images/poster/go1_stairs_terrain_walk.jpg
:nocontrols:
:autoplay:
:playsinline:
:muted:
:loop:
:width: 100%
```
## 任务描述
训练 Unitree GO1 四足机器人在复杂地形上实现稳定、高效的双足行走。该环境使用 MotrixSim 物理引擎进行仿真，提供高保真的动力学模拟。智能体通过控制各关节的目标位置来实现速度跟踪和姿态稳定，同时适应不同的地形挑战。
### 任务目标
- **速度跟踪**：准确跟踪给定的线速度和角速度命令
- **姿态稳定**：在各种地形条件下保持身体姿态稳定
- **能量效率**：以最小的能耗实现行走任务
- **地形适应性**：适应粗糙地形和台阶地形的不同挑战
---
## 动作空间（Action Space）
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-1.0, 1.0, (12,), float32)` |
| **维度** | 12 |
动作对应 12 个关节的位置控制指令，包括四条腿的髋关节、大腿关节和小腿关节。
---
## 观察空间
### 粗糙地形观察空间（48 维）
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (48,), float32)` |
| **维度** | 48 |
| 部分 | 内容说明 | 维度 | 备注 |
| --------------------- | ------------------------ | ---- | -------------- |
| **noisy_linvel** | 机体坐标系下的线速度 | 3 | 带噪声的线速度 |
| **noisy_gyro** | 机体坐标系下的角速度 | 3 | 带噪声的角速度 |
| **local_gravity** | 局部坐标系下的重力方向 | 3 | 重力向量投影 |
| **noisy_joint_angle** | 关节角度与默认角度的偏差 | 12 | 12 个关节 |
| **noisy_joint_vel** | 关节角速度 | 12 | 带噪声的速度 |
| **last_actions** | 上一时间步的控制动作 | 12 | 历史动作 |
| **command** | 目标线速度和角速度 | 3 | [vx, vy, vyaw] |
### 台阶地形观察空间（60 维）
| 项目 | 详细信息 |
| -------- | -------------------------------- |
| **类型** | `Box(-inf, inf, (60,), float32)` |
| **维度** | 60 |
在粗糙地形基础上增加了：
| 部分 | 内容说明 | 维度 | 备注 |
| ---------------------- | -------------------- | ---- | ---------------------- |
| **feet_contact_force** | 四个足部的接触力向量 | 12 | 每足 3 维 (Fx, Fy, Fz) |
---
## 奖励函数设计
GO1 复杂地形的奖励函数采用多目标加权设计：
```python
# 核心奖励组件
reward_config.scales = {
"tracking_lin_vel": 1.0, # 线速度跟踪精度
"tracking_ang_vel": 0.5, # 角速度跟踪精度
"orientation": -0.0, # 身体姿态稳定性惩罚
"torques": -0.00001, # 关节力矩惩罚（能量效率）
"dof_acc": -2.5e-7, # 关节加速度惩罚
"action_rate": -0.001, # 动作平滑性惩罚
"feet_air_time": 1.0, # 足部空中时间奖励（鼓励大步幅）
"stand_still": 0.0, # 静止指令的关节位置保持
"hip_pos": -1, # 髋关节位置偏好
"calf_pos": -0.3, # 小腿关节位置偏好
"feet_stumble": -0.5, # 当足部横向触碰障碍物时施加惩罚
}
# 总奖励 = 加权组合以上所有项
```
---
## 初始状态
### 粗糙地形初始化
- **地形生成**：使用高度图生成随机地形
- **地形高度级别**：预设 -2.5m、0.5m、2.0m 三个高度级别
- **位置随机化**：在基础训练级别时机器人固定位置，高级别时按预设的 25 个位置周期循环随机选择
### 台阶地形初始化
- **地形类型**：连续地形上布置多种以台阶为主的小地块
- **位置随机化**：与粗糙地形类似的位置随机化策略
### 机器人初始化
- **关节角度**：设置为默认站立姿态，添加 [-0.125, 0.125] 弧度噪声
- **速度初始化**：所有线速度和角速度初始化为零
## Episode 终止条件
- **身体接触地面**：机器人躯干与地面发生非预期接触
- **速度异常**：线速度平方和超过阈值（1e8）
---
## 使用指南
### 1. 环境预览
```bash
uv run scripts/view.py --env go1-rough-terrain-walk
uv run scripts/view.py --env go1-stairs-terrain-walk
```
### 2. 开始训练
```bash
uv run scripts/train.py --env go1-rough-terrain-walk
uv run scripts/train.py --env go1-stairs-terrain-walk
```
### 3. 查看训练进度
```bash
uv run tensorboard --logdir runs/go1-rough-terrain-walk
```
### 4. 测试训练结果
由于粗糙地形场景中同时生成了了一个无限大平面和一个崎岖地形高度场，测试训练结果时会仿照训练过程先将智能体生成在平面上，完成一轮行走后再生成到崎岖地形上。用户需要主动调整相机视角和位置来观察智能体的状态。
```bash
uv run scripts/play.py --env go1-rough-terrain-walk
uv run scripts/play.py --env go1-stairs-terrain-walk
```
---
## 预期训练结果
### 粗糙地形任务 (go1-rough-terrain-walk)
1. 能够适应不同的粗糙地形高度
2. 速度跟踪精度高，姿态稳定
3. 步态协调，足部打滑少
### 台阶地形任务 (go1-stairs-terrain-walk)
1. 能够稳定上下台阶
2. 能够适应不同高度和宽度的台阶
3. 动作流畅，无明显卡顿
## 训练性能参考
### go1-rough-terrain-walk
| 操作系统 | 训练后端 | CPU | GPU | 环境数 | 训练时间 (30000 steps) |
| ------------ | -------- | ----------------- | ----------- | ------ | ---------------------- |
| Ubuntu 22.04 | JAX | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 7m20s |
| Ubuntu 22.04 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 8m30s |
| Windows 11 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 10m42s |
### go1-stairs-terrain-walk
| 操作系统 | 训练后端 | CPU | GPU | 环境数 | 训练时间 (30000 steps) |
| ------------ | -------- | ----------------- | ----------- | ------ | ---------------------- |
| Ubuntu 22.04 | JAX | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 7m18s |
| Ubuntu 22.04 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 8m41s |
| Windows 11 | PyTorch | AMD Ryzen 7 9700X | RTX 5070 Ti | 2048 | 10m52s |

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

@@ -26,7 +26,15 @@ tutorial/training_and_result
:maxdepth: 1 :maxdepth: 1
demo/cartpole demo/cartpole
demo/bounce_ball
demo/dm_walker demo/dm_walker
demo/dm_cheetah
demo/dm_hopper
demo/dm_reacher
demo/locomotion_unitree_go1 demo/locomotion_unitree_go1
demo/locomotion_unitree_go1_rough_terrain
demo/anymal_c
demo/franka_lift_cube
demo/franka_open_cabinet
``` ```

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.0.2" version = "0.1.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.4.0", "motrixsim>=0.5.0b2",
] ]

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

@@ -13,4 +13,4 @@
# limitations under the License. # limitations under the License.
# ============================================================================== # ==============================================================================
from . import basic, locomotion # noqa: F401 from . import basic, locomotion, manipulation # noqa: F401

1
motrix_envs/src/motrix_envs/base.py
View File

@@ -31,6 +31,7 @@ class EnvCfg:
sim_dt: float = 0.01 sim_dt: float = 0.01
max_episode_seconds: float = None max_episode_seconds: float = None
ctrl_dt: float = 0.01 ctrl_dt: float = 0.01
render_spacing: float = 1.0
@property @property
def max_episode_steps(self) -> Optional[int]: def max_episode_steps(self) -> Optional[int]:

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

@@ -13,4 +13,4 @@
# limitations under the License. # limitations under the License.
# ============================================================================== # ==============================================================================
from . import cartpole, walker # noqa: F401 import to register envs from . import bounce_ball, cartpole, cheetah, hopper, reacher, walker # noqa: F401 import to register envs

17
motrix_envs/src/motrix_envs/basic/bounce_ball/__init__.py Normal file
View File

@@ -0,0 +1,17 @@
# 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 bounce_ball_np # noqa: F401 import to register envs
from .cfg import BounceBallEnvCfg # noqa: F401

83
motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_ctrl.xml Normal file
View File

@@ -0,0 +1,83 @@
<mujoco model="air4a">
<compiler angle="radian" meshdir="meshes"/>
<option gravity="0 0 -9.8" timestep="0.001"/>
<asset>
<mesh name="base_link" file="base_link.STL"/>
<mesh name="Link1" file="Link1.STL"/>
<mesh name="Link2" file="Link2.STL"/>
<mesh name="Link3" file="Link3.STL"/>
<mesh name="Link4" file="Link4.STL"/>
<mesh name="Link5" file="Link5.STL"/>
<mesh name="Link6" file="Link6.STL"/>
<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>
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<global elevation="-10"/>
</visual>
<default>
<position kp="20000" kv="200" inheritrange="1"/>
<geom friction="0 0 0" contype="2" hard="true" solref="0.8 0"/>
<default class="visual">
<geom contype="0" conaffinity="0"/>
</default>
</default>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom type="plane" size="0 0 .01" material="motphys-ground"/>
<geom type="mesh" rgba="0.25098 0.25098 0.25098 1" mesh="base_link" class="visual"/>
<body name="Link1" pos="0 0 0.1">
<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"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link1" class="visual"/>
<body name="Link2" quat="0.5 -0.5 -0.5 -0.5">
<inertial pos="0.093605 -0.011533 0.010727" quat="0.490047 0.523104 0.543931 0.436298" mass="1.1745" diaginertia="0.0163844 0.0143076 0.00562255"/>
<joint name="Joint2" pos="0 0 0" axis="0 0 1" range="-1.91986 2.0944" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link2" class="visual"/>
<body name="Link3" pos="0.26 0 0" quat="0.707107 0 0 0.707107">
<inertial pos="0.0012743 0.0012257 0.016465" quat="0.449628 -0.144988 -0.276019 0.837034" mass="0.44924" diaginertia="0.0014142 0.00119048 0.00116662"/>
<joint name="Joint3" pos="0 0 0" axis="0 0 1" range="-1.88496 2.6529" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link3" class="visual"/>
<body name="Link4" pos="-0.021 -0.2995 0" quat="0.707107 0.707107 0 0">
<inertial pos="-0.001146 -0.00073967 -0.13576" quat="0.999984 0.000512404 0.000118537 0.00557572" mass="0.52995" diaginertia="0.00482651 0.0047764 0.000898226"/>
<joint name="Joint4" pos="0 0 0" axis="0 0 1" range="-3.49066 3.49066" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link4" class="visual"/>
<body name="Link5" quat="0.707107 -0.707107 0 0">
<inertial pos="1.2093e-06 -0.013401 0.0058598" quat="0.459225 0.53864 -0.53747 0.458372" mass="0.24301" diaginertia="0.000294721 0.00025665 0.000171719"/>
<joint name="Joint5" pos="0 0 0" axis="0 0 1" range="-2.05949 2.05949" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="1 0.69804 0 1" mesh="Link5" class="visual"/>
<body name="Link6" pos="0 -0.0685 0" quat="0.707107 0.707107 0 0">
<inertial pos="-5.74917e-05 0 0.11545" quat="1 0 0.000720664 0" mass="0.223253" diaginertia="0.0211302 0.0211301 0.0200061"/>
<joint name="Joint6" pos="0 0 0" axis="0 0 1" range="-6.10865 6.10865" actuatorfrcrange="-300 300"/>
<geom type="mesh" rgba="0.25098 0.25098 0.25098 1" mesh="Link6" class="visual"/>
<geom size="0.01 0.07" pos="0 0 0.07" type="cylinder" rgba="0.2 0.2 0.2 1"/>
<geom name="blocker" size="0.05 0.065 0.0035" pos="0.0005 0 0.19" quat="0.707107 0 -0.707107 0" type="box" rgba="1 0.423529 0.0392157 1"/>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="ball_link" pos="0 0 0.07">
<freejoint/>
<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"/>
</body>
</worldbody>
<actuator>
<position name="act1" joint="Joint1" />
<position name="act2" joint="Joint2" />
<position name="act3" joint="Joint3" />
<position name="act4" joint="Joint4" />
<position name="act5" joint="Joint5" />
<position name="act6" joint="Joint6" />
</actuator>
</mujoco>

308
motrix_envs/src/motrix_envs/basic/bounce_ball/bounce_ball_np.py Normal file
View File

@@ -0,0 +1,308 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import BounceBallEnvCfg
@registry.env("bounce_ball", "np")
class BounceBallEnv(NpEnv):
_cfg: BounceBallEnvCfg
def __init__(self, cfg: BounceBallEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
# Action space: 6D normalized paddle velocity (dx, dy, dz, dr_x, dr_y, dr_z)
self._action_space = gym.spaces.Box(-1.0, 1.0, (6,), dtype=np.float32)
# Observation space: simplified version using only DOF information
# DOF pos (13) + DOF vel (12) = 25 (this includes ball state implicitly)
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_vel = self._model.num_dof_vel
# Initial arm joint positions (degrees converted to radians)
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)
# Initialize full DOF positions (6 arm joints + 7 for ball free joint)
self._init_dof_pos = np.zeros(self._model.num_dof_pos, dtype=np.float32)
self._init_dof_pos[:6] = self._init_arm_qpos
# Get body and geom IDs
self._paddle_geom_id = self._model.geom_names.index("blocker")
self._ball_body_id = self._model.body_names.index("ball_link")
# Action scaling parameters
self._action_scale = np.array(self._cfg.action_scale, dtype=np.float32)
self._action_bias = np.array(self._cfg.action_bias, dtype=np.float32)
# Track ball initial position for reset
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)
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def _denormalize_action(self, action: np.ndarray) -> np.ndarray:
"""Denormalize action to get actual paddle velocity changes"""
return self._action_scale * action + self._action_bias
def _compute_observation(self, data: mtx.SceneData) -> np.ndarray:
"""Compute 25-dimensional observation vector from DOF states"""
# Use DOF positions and velocities directly
dof_pos = data.dof_pos
dof_vel = data.dof_vel
# Concatenate DOF positions (13) and velocities (12)
obs = np.concatenate([dof_pos, dof_vel], axis=-1)
return obs.astype(np.float32)
def _compute_reward(
self, obs: np.ndarray, data: mtx.SceneData = None, consecutive_bounces: np.ndarray = None
) -> np.ndarray:
"""Compute reward based on ball height, position, and controlled upward velocity"""
# Extract ball position and velocity from DOF
ball_x = obs[:, 6] # Ball x position
ball_z = obs[:, 8] # Ball z position
ball_vz = obs[:, 13 + 8] # Ball z velocity (13 pos + 8 vel)
# Target positions
target_ball_x = 0.58856 # Target x position
target_height = self._cfg.target_ball_height
tolerance = self._cfg.height_tolerance
# 1. Position control reward - MOST IMPORTANT for keeping ball centered
# Strong reward for ball being at the right x position (paddle center)
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
# 2. Height-based reward - less important than position control
height_error = np.abs(ball_z - target_height)
height_reward = np.exp(-(height_error**2) / (2 * tolerance**2))
# 3. Controlled upward velocity reward - only when ball is in good position
# Only reward upward velocity when ball is well-positioned horizontally
well_positioned = x_position_error < 0.02 # Ball must be very close to target x
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(
x_position_error > 0.1,
-2.0, # Heavy penalty for being far from center
0.0,
)
# 5. Velocity penalties - discourage excessive speeds
excessive_upward_penalty = np.where(ball_vz > 2.0, -1.0, 0.0)
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)
high_bounce_bonus = np.where(
good_position_for_bounce & (consecutive_bounces >= 3),
consecutive_bounces * 0.1, # Reduced bonus
0.0,
)
# Combine all rewards with corrected priorities
total_reward = (
x_position_reward * 2.0 # X position control (200%) - MOST IMPORTANT
+ controlled_upward_reward * 1.0 # Controlled upward velocity (100%)
+ height_reward * 0.3 # Height accuracy (30%) - less important
+ consecutive_bounces_reward * 1.0 # Consecutive bounces (100%) - reduced
+ high_bounce_bonus * 0.3 # High bounce bonus (30%) - reduced
+ out_of_position_penalty * 1.0 # Out of position penalty (100%)
+ excessive_upward_penalty * 1.0 # Excessive upward penalty (100%)
+ downward_velocity_penalty * 1.0 # Downward penalty (100%)
+ overshoot_penalty # Height overshoot penalty
+ undershoot_penalty # Height undershoot penalty
)
return total_reward
def _compute_terminated(self, obs: np.ndarray) -> np.ndarray:
"""Check if episode should terminate based on DOF states"""
# Extract ball position from DOF (indices 6-8 for x,y,z)
ball_x = obs[:, 6] # Ball x 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
terminated = (ball_z < 0.05) | (ball_z > target_height + 1.0)
# Also terminate if ball goes too far horizontally
terminated |= np.abs(ball_x) > 1.5
return terminated
def apply_action(self, actions: np.ndarray, state: NpEnvState) -> NpEnvState:
"""Apply action to control paddle position"""
# Get current joint positions
current_joint_pos = state.data.dof_pos[:, :6] # First 6 DOFs are arm joints
# Denormalize actions to get actual position changes
delta_positions = self._denormalize_action(actions)
# Calculate target positions = current positions + position changes
target_positions = current_joint_pos + delta_positions
# Apply target positions as actuator controls (position control)
state.data.actuator_ctrls = target_positions
return state
def update_state(self, state: NpEnvState) -> NpEnvState:
"""Update state with new observations, rewards, and termination flags"""
data = state.data
# Compute observation
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))
ball_was_upward = state.info.get("ball_was_upward", np.zeros(data.shape[0], dtype=bool))
# Detect bounces and update consecutive bounce count
current_ball_z = obs[:, 8] # Ball z position
current_ball_vz = obs[:, 21] # Ball z velocity
# Detect bounces: ball moving upward after being near paddle height
near_paddle = (current_ball_z < 0.4) & (current_ball_z > 0.15)
moving_upward = current_ball_vz > 0.01
# A bounce is detected when ball was going down and now goes up near paddle height
bounce_detected = ~ball_was_upward & moving_upward & near_paddle
# Update consecutive bounce count
consecutive_bounces = np.where(bounce_detected, consecutive_bounces + 1, consecutive_bounces)
# Reset count if ball is falling too much (not bouncing properly)
falling = (current_ball_vz < -0.5) & (current_ball_z < 0.4)
consecutive_bounces = np.where(falling, 0, consecutive_bounces)
# Update tracking variables in info
state.info["consecutive_bounces"] = consecutive_bounces
state.info["ball_was_upward"] = moving_upward
# Track maximum bounces achieved
max_current = np.max(consecutive_bounces)
if "max_consecutive_bounces" not in state.info:
state.info["max_consecutive_bounces"] = 0
if max_current > state.info["max_consecutive_bounces"]:
state.info["max_consecutive_bounces"] = max_current
# For simplicity, use raw observation without normalization for now
# Could add proper normalization later
normalized_obs = obs
# Compute reward and termination
reward = self._compute_reward(obs, data, consecutive_bounces)
terminated = self._compute_terminated(obs)
state.obs = normalized_obs
state.reward = reward
state.terminated = terminated
return state
def reset(self, data: mtx.SceneData) -> tuple:
"""Reset environment to initial state"""
cfg: BounceBallEnvCfg = self._cfg
num_reset = data.shape[0]
# Add noise to initial arm joint positions only (not ball)
arm_noise_pos = np.random.uniform(
-cfg.reset_noise_scale,
cfg.reset_noise_scale,
(num_reset, 6), # Only 6 arm joints
)
noise_vel = np.random.uniform(
-cfg.reset_noise_scale,
cfg.reset_noise_scale,
(num_reset, self._num_dof_vel),
)
# Reset simulation first to get proper DOF structure
data.reset(self._model)
# Get current DOF positions and modify only the arm joints
current_dof_pos = data.dof_pos
current_dof_vel = data.dof_vel
# 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_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)
for i in range(num_reset):
ball_noise_pos = np.random.uniform(-0.01, 0.01, 3)
ball_pos = self._ball_init_pos + ball_noise_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
data.set_dof_pos(current_dof_pos, self._model)
# Initialize info dict with bounce tracking variables
info = {
"consecutive_bounces": np.zeros(num_reset, dtype=np.int32),
"ball_was_upward": np.zeros(num_reset, dtype=bool),
"max_consecutive_bounces": 0,
}
# Compute initial observation
obs = self._compute_observation(data)
normalized_obs = obs # No normalization for now
return normalized_obs, info

64
motrix_envs/src/motrix_envs/basic/bounce_ball/cfg.py Normal file
View File

@@ -0,0 +1,64 @@
# 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__) + "/bounce_ball_ctrl.xml"
@registry.envcfg("bounce_ball")
@dataclass
class BounceBallEnvCfg(EnvCfg):
model_file: str = model_file
reset_noise_scale: float = 0.01
max_episode_seconds: float = 20.0
# Ball and paddle physics parameters
ball_restitution: float = 0.9 # Slightly less than perfect for realistic bouncing
ball_linear_damping: float = 0.55
ball_lateral_friction: float = 1.1
paddle_restitution: float = 0.8
paddle_linear_damping: float = 0.55
paddle_lateral_friction: float = 1.1
# Initial conditions
ball_init_pos: list = None
ball_init_vel: list = None
arm_init_qpos: list = None
# Target height for bouncing (configurable parameter)
target_ball_height: float = 0.8 # Default target height in meters
height_tolerance: float = 0.1 # Tolerance for reward calculation
# Action scaling parameters
action_scale: list = None
action_bias: list = None
def __post_init__(self):
if self.ball_init_pos is None:
self.ball_init_pos = [0.58856, 0, 1.27796] # Slightly above paddle (paddle z=0.2803)
if self.ball_init_vel is None:
self.ball_init_vel = [0.0, 0.0, 0.0]
if self.arm_init_qpos is None:
self.arm_init_qpos = [0, 40, 110, 0, -60, 0]
if self.action_scale is None:
self.action_scale = [0.0008] * 6
if self.action_bias is None:
self.action_bias = [0.0] * 6

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link1.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link2.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link3.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link4.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link5.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/Link6.STL Normal file
View File

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

3
motrix_envs/src/motrix_envs/basic/bounce_ball/meshes/base_link.STL Normal file
View File

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

16
motrix_envs/src/motrix_envs/basic/cartpole/cartpole.xml
View File

@@ -7,16 +7,18 @@
<motor ctrlrange="-3 3" /> <motor ctrlrange="-3 3" />
</default> </default>
<asset> <asset>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" <texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
rgb1="0.4314 0.5294 0.6431" rgb2="0.8157 0.8549 0.9059" width="512"
markrgb="0.8 0.8 0.8" width="300" height="300" /> height="512" />
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="1 1" <texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
reflectance="0.2" /> <material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset> </asset>
<worldbody> <worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" /> <light pos="0 0 1.5" dir="0 1 -1" directional="true" />
<geom name="floor" size="0 0 0.01" type="plane" material="groundplane" pos="0 0 -1" /> <geom name="floor" size="0 0 0.01" type="plane" material="motphys-ground" pos="0 0 -1" />
</worldbody> </worldbody>
<worldbody> <worldbody>
<geom name="rail" pos="0 0 0" quat="0.707 0 0.707 0" rgba="0.3 0.3 0.7 1" size="0.02 1" <geom name="rail" pos="0 0 0" quat="0.707 0 0.707 0" rgba="0.3 0.3 0.7 1" size="0.02 1"

1
motrix_envs/src/motrix_envs/basic/cartpole/cfg.py
View File

@@ -28,3 +28,4 @@ class CartPoleEnvCfg(EnvCfg):
model_file: str = model_file model_file: str = model_file
reset_noise_scale: float = 0.01 reset_noise_scale: float = 0.01
max_episode_seconds: float = 10 max_episode_seconds: float = 10
render_spacing: float = 2.0

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

33
motrix_envs/src/motrix_envs/basic/cheetah/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__) + "/cheetah.xml"
@registry.envcfg("dm-cheetah")
@dataclass
class CheetahEnvCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 10.0
render_spacing: float = 2.0
sim_dt: float = 0.01
ctrl_dt: float = 0.025
run_speed: float = 30.0

80
motrix_envs/src/motrix_envs/basic/cheetah/cheetah.xml Normal file
View File

@@ -0,0 +1,80 @@
<mujoco model="cheetah">
<include file="../../common/materials.xml" />
<compiler settotalmass="14" />
<default>
<default class="cheetah">
<joint limited="true" damping=".01" armature=".1" stiffness="8" type="hinge" axis="0 1 0" />
<geom contype="1" conaffinity="1" condim="3" friction=".4 .1 .1" material="self" />
</default>
<default class="free">
<joint limited="false" damping="0" armature="0" stiffness="0" />
</default>
<motor ctrllimited="true" ctrlrange="-1 1" />
</default>
<statistic center="0 0 .7" extent="2" />
<option timestep="0.01" />
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light name="light" dir="0 1 -1" directional="true" />
<geom name="ground" type="plane" conaffinity="1" pos="0 0 0" size="0 0 0.01"
material="motphys-ground" />
<body name="torso" pos="0 0 .7" childclass="cheetah">
<camera name="side" pos="0 -3 0" quat="0.707 0.707 0 0" mode="trackcom" />
<camera name="back" pos="-1.8 -1.3 0.8" xyaxes="0.45 -0.9 0 0.3 0.15 0.94" mode="trackcom" />
<joint name="rootx" type="slide" axis="1 0 0" class="free" />
<joint name="rootz" type="slide" axis="0 0 1" class="free" />
<joint name="rooty" type="hinge" axis="0 1 0" class="free" />
<geom name="torso" type="capsule" fromto="-.5 0 0 .5 0 0" size="0.046" />
<geom name="head" type="capsule" pos=".6 0 .1" euler="0 50 0" size="0.046 .15" />
<body name="bthigh" pos="-.5 0 0">
<joint name="bthigh" range="-30 60" stiffness="240" damping="6" />
<geom name="bthigh" type="capsule" pos=".1 0 -.13" euler="0 -218 0" size="0.046 .145" />
<body name="bshin" pos=".16 0 -.25">
<joint name="bshin" range="-50 50" stiffness="180" damping="4.5" />
<geom name="bshin" type="capsule" pos="-.14 0 -.07" euler="0 -116 0" size="0.046 .15" />
<body name="bfoot" pos="-.28 0 -.14">
<joint name="bfoot" range="-230 50" stiffness="120" damping="3" />
<geom name="bfoot" type="capsule" pos=".03 0 -.097" euler="0 -15 0" size="0.046 .094" />
</body>
</body>
</body>
<body name="fthigh" pos=".5 0 0">
<joint name="fthigh" range="-57 .40" stiffness="180" damping="4.5" />
<geom name="fthigh" type="capsule" pos="-.07 0 -.12" euler="0 30 0" size="0.046 .133" />
<body name="fshin" pos="-.14 0 -.24">
<joint name="fshin" range="-70 50" stiffness="120" damping="3" />
<geom name="fshin" type="capsule" pos=".065 0 -.09" euler="0 -34 0" size="0.046 .106" />
<body name="ffoot" pos=".13 0 -.18">
<joint name="ffoot" range="-28 28" stiffness="60" damping="1.5" />
<geom name="ffoot" type="capsule" pos=".045 0 -.07" euler="0 -34 0" size="0.046 .07" />
</body>
</body>
</body>
</body>
</worldbody>
<sensor>
<subtreelinvel name="torso_subtreelinvel" body="torso" />
</sensor>
<actuator>
<motor name="bthigh" joint="bthigh" gear="120" />
<motor name="bshin" joint="bshin" gear="90" />
<motor name="bfoot" joint="bfoot" gear="60" />
<motor name="fthigh" joint="fthigh" gear="90" />
<motor name="fshin" joint="fshin" gear="60" />
<motor name="ffoot" joint="ffoot" gear="30" />
</actuator>
</mujoco>

120
motrix_envs/src/motrix_envs/basic/cheetah/cheetah_np.py Normal file
View File

@@ -0,0 +1,120 @@
# 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.cheetah.cfg import CheetahEnvCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-cheetah", "np")
class CheetahEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: CheetahEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_obs_space()
self._init_action_space()
self._torso = self._model.get_link("torso")
self._run_speed = cfg.run_speed
self._joint_limits = self._model.joint_limits
def _init_obs_space(self):
obs_dim = (self._model.num_dof_pos - 1) + self._model.num_dof_vel
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (obs_dim,), dtype=np.float64)
def _init_action_space(self):
model = self._model
self._action_space = gym.spaces.Box(
low=-1.0,
high=1.0,
shape=(model.num_actuators,),
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, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
pos = qpos[:, 1:].copy() # exclude x position
vel = data.dof_vel
obs = np.concatenate([pos, vel], axis=-1)
return obs
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
# === compute obs ===
obs = self._get_obs(data)
# === Terminated ===
terminated = np.zeros(data.shape[0], dtype=bool)
# ==== compute reward ====
vel = self._model.get_sensor_value("torso_subtreelinvel", data)
rwd_speed = reward.tolerance(
vel[:, 0],
bounds=(self._run_speed, float("inf")),
margin=self._run_speed,
value_at_margin=0.0,
sigmoid="linear",
)
torso_height = self._torso.get_position(data)[:, 2]
rwd_posture = -1.5 * (torso_height - 0.75) ** 2
rwd_posture = np.clip(rwd_posture, -1.0, 1.0)
rwd = rwd_speed + rwd_posture
return state.replace(
obs=obs,
reward=rwd,
terminated=terminated,
)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num = data.shape[0]
limited_idx = np.where(self._model.joint_limits == 1)[0]
low = self._joint_limits[0, limited_idx]
high = self._joint_limits[1, limited_idx]
qpos = data.dof_pos
qpos[:, limited_idx] = np.random.uniform(low, high, size=(num, len(limited_idx)))
data.set_dof_pos(qpos, self._model)
for _ in range(200):
self._model.step(data)
obs = self._get_obs(data)
return obs, {}

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

39
motrix_envs/src/motrix_envs/basic/hopper/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
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/hopper.xml"
@registry.envcfg("dm-hopper-stand")
@dataclass
class HopperStandCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 20.0
sim_dt: float = 0.02
ctrl_dt: float = 0.02
stand_height: float = 0.6
hop_speed: float = 0.0
@registry.envcfg("dm-hopper-hop")
@dataclass
class HopperHopCfg(HopperStandCfg):
hop_speed: float = 2.0

70
motrix_envs/src/motrix_envs/basic/hopper/hopper.xml Normal file
View File

@@ -0,0 +1,70 @@
<mujoco model="planar hopper">
<include file="../../common/materials.xml" />
<statistic extent="2" center="0 0 .5" />
<option timestep="0.005" />
<default>
<default class="hopper">
<joint type="hinge" axis="0 1 0" limited="true" damping=".05" armature=".2" />
<geom type="capsule" material="self" />
<site type="sphere" size="0.05" group="3" />
</default>
<default class="free">
<joint limited="false" damping="0" armature="0" stiffness="0" />
</default>
<motor ctrlrange="-1 1" ctrllimited="true" />
</default>
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<camera name="cam0" pos="0 -2.8 0.8" euler="90 0 0" mode="trackcom" />
<camera name="back" pos="-2 -.2 1.2" xyaxes="0.2 -1 0 .5 0 2" mode="trackcom" />
<geom name="ground" type="plane" pos="0 0 -0.02" size="0 0 0.01" material="motphys-ground"
conaffinity="1" />
<body name="torso" pos="0 0 1" childclass="hopper">
<light name="light" dir="0 1 -1" directional="true" />
<joint name="rootx" type="slide" axis="1 0 0" class="free" />
<joint name="rootz" type="slide" axis="0 0 1" class="free" />
<joint name="rooty" type="hinge" axis="0 1 0" class="free" />
<geom name="torso" fromto="0 0 -.05 0 0 .2" size="0.0653" />
<geom name="nose" fromto=".08 0 .13 .15 0 .14" size="0.03" />
<body name="pelvis" pos="0 0 -.05">
<joint name="waist" range="-30 30" />
<geom name="pelvis" fromto="0 0 0 0 0 -.15" size="0.065" />
<body name="thigh" pos="0 0 -.2">
<joint name="hip" range="-170 10" />
<geom name="thigh" fromto="0 0 0 0 0 -.33" size="0.04" />
<body name="calf" pos="0 0 -.33">
<joint name="knee" range="5 150" />
<geom name="calf" fromto="0 0 0 0 0 -.32" size="0.03" />
<body name="foot" pos="0 0 -.32">
<joint name="ankle" range="-45 45" />
<geom name="foot" fromto="-.08 0 0 .17 0 0" size="0.04" />
<site name="touch_toe" pos=".17 0 0" />
<site name="touch_heel" pos="-.08 0 0" />
</body>
</body>
</body>
</body>
</body>
</worldbody>
<sensor>
<subtreelinvel name="torso_subtreelinvel" body="torso" />
<touch name="touch_toe" site="touch_toe" />
<touch name="touch_heel" site="touch_heel" />
</sensor>
<actuator>
<motor name="waist" joint="waist" gear="30" />
<motor name="hip" joint="hip" gear="40" />
<motor name="knee" joint="knee" gear="30" />
<motor name="ankle" joint="ankle" gear="10" />
</actuator>
</mujoco>

200
motrix_envs/src/motrix_envs/basic/hopper/hopper_np.py Normal file
View File

@@ -0,0 +1,200 @@
# 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.hopper.cfg import HopperStandCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-hopper-stand", "np")
@registry.env("dm-hopper-hop", "np")
class HopperEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: HopperStandCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_obs_space()
self._init_action_space()
self._torso = self._model.get_link("torso")
self._foot = self._model.get_link("foot")
self._stand_height = cfg.stand_height
self._hop_speed = cfg.hop_speed
self._joint_limits = self._model.joint_limits
def _init_obs_space(self):
model = self._model
num = 0
num += model.num_dof_pos - 1
num += model.num_dof_vel
num += 2
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num,), 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, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos[:, 1:]
qvel = data.dof_vel
num_env = int(data.shape[0])
toe = np.asarray(self._model.get_sensor_value("touch_toe", data)).reshape(num_env, -1)[:, 0]
heel = np.asarray(self._model.get_sensor_value("touch_heel", data)).reshape(num_env, -1)[:, 0]
toe = np.log1p(toe)
heel = np.log1p(heel)
touch = np.stack([toe, heel], axis=-1) # shape -> (num_env, 2)
return np.concatenate([qpos, qvel, touch], axis=-1)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
# === obs ===
obs = self._get_obs(data)
num_env = int(data.shape[0])
toe = np.asarray(self._model.get_sensor_value("touch_toe", data)).reshape(num_env, -1)[:, 0]
heel = np.asarray(self._model.get_sensor_value("touch_heel", data)).reshape(num_env, -1)[:, 0]
toe = np.log1p(toe)
heel = np.log1p(heel)
# === physical values ===
torso_pos = self._torso.get_position(data)
foot_pos = self._foot.get_position(data)
torso_height = torso_pos[:, 2] - foot_pos[:, 2]
torso_vel = self._model.get_sensor_value("torso_subtreelinvel", data)
speed = torso_vel[:, 0]
# === terminated ===
over_speed = np.sum(np.square(data.dof_vel[:, 4:7]), axis=-1) > 1e8
terminated = np.isnan(obs).any(axis=-1)
terminated |= over_speed
standing = reward.tolerance(
torso_height,
bounds=(self._stand_height, 2.0),
margin=self._stand_height * 0.5,
)
if self._hop_speed > 0.0:
hopping = reward.tolerance(
speed,
bounds=(self._hop_speed * 0.3, float("inf")),
margin=self._hop_speed * 0.3,
value_at_margin=0.0,
sigmoid="linear",
)
leg_vel = np.linalg.norm(data.dof_vel[:, 4:7], axis=-1)
leg_bonus = np.tanh(leg_vel * 0.3) * 0.2 * standing
knee_vel = data.dof_vel[:, 5]
extend_reward = np.maximum(knee_vel, 0) * 0.2 * standing
stand_condition = (torso_height > self._stand_height * 0.8).astype(np.float32)
effective_hop_reward = hopping * stand_condition
contact_strength = toe + heel
contact_reward = np.clip(contact_strength, 0.0, 1.0) * 0.1 * standing
rwd = standing * 0.8 + effective_hop_reward * 0.8 + leg_bonus * 0.5 + extend_reward + contact_reward
if np.average(rwd) > 1000:
print(
"standing",
np.sum(standing),
"effective_hop_reward",
np.sum(effective_hop_reward),
"leg_bonus",
np.sum(leg_bonus),
"extend_reward",
np.sum(extend_reward),
"contact_reward",
np.sum(contact_reward),
)
else:
control_magnitude = np.linalg.norm(data.actuator_ctrls, axis=-1)
small_control = reward.tolerance(
control_magnitude,
bounds=(0, 1),
margin=1,
value_at_margin=0,
sigmoid="quadratic",
)
small_control = (small_control + 4) / 5
rwd = standing * small_control
state.info["Reward"] = {"stand": standing, "control": small_control, "total": rwd}
rwd[terminated] = 0.0
return state.replace(
obs=obs,
reward=rwd,
terminated=terminated,
)
def reset(self, data: mtx.SceneData):
data.reset(self._model)
num_env = data.shape[0]
dof_pos = np.zeros((num_env, self._model.num_dof_pos))
dof_pos[:, 2] = 0
if self._model.num_dof_pos > 3:
dof_pos[:, 3:] = np.random.uniform(
low=self._joint_limits[0, 3:],
high=self._joint_limits[1, 3:],
size=(num_env, self._model.num_dof_pos - 3),
)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
obs = self._get_obs(data)
rewards = {"stand": np.zeros((num_env,))}
if self._hop_speed > 0.0:
rewards["hop"] = np.zeros((num_env,))
return obs, {"Reward": rewards}

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

34
motrix_envs/src/motrix_envs/basic/reacher/cfg.py Normal file
View File

@@ -0,0 +1,34 @@
# 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__) + "/reacher.xml"
@registry.envcfg("dm-reacher")
@dataclass
class ReacherEnvCfg(EnvCfg):
model_file: str = model_file
max_episode_seconds: float = 6.0
render_spacing: float = 0.5
sim_dt: float = 0.0125
move_speed: float = 1.0
ctrl_dt: float = 0.025
target_size: float = 0.02

54
motrix_envs/src/motrix_envs/basic/reacher/reacher.xml Normal file
View File

@@ -0,0 +1,54 @@
<mujoco model="two-link planar reacher">
<include file="../../common/visual.xml" />
<include file="../../common/skybox.xml" />
<include file="../../common/materials.xml" />
<option timestep="0.02">
<flag contact="disable" />
</option>
<default>
<joint type="hinge" axis="0 0 1" damping="0.01" />
<motor gear=".05" ctrlrange="-1 1" ctrllimited="true" />
</default>
<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>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<camera name="fixed" pos="0 0 .75" quat="1 0 0 0" />
<!-- Arena -->
<geom name="ground" type="plane" pos="0 0 -0.02" size="0 0 0.01" material="motphys-ground"
contype="0" conaffinity="0" />
<!-- Arm -->
<geom name="root" type="cylinder" fromto="0 0 0 0 0 0.02" size=".011" material="decoration" />
<body name="arm" pos="0 0 .01">
<geom name="arm" type="capsule" fromto="0 0 0 0.12 0 0" size=".01" material="self" />
<joint name="shoulder" />
<body name="hand" pos=".12 0 0">
<geom name="hand" type="capsule" fromto="0 0 0 0.1 0 0" size=".01" material="self" />
<joint name="wrist" limited="true" range="-160 160" />
<body name="finger" pos=".12 0 0">
<camera name="hand" pos="0 0 .2" mode="track" />
<geom name="finger" type="sphere" size=".01" material="effector" />
</body>
</body>
</body>
<!-- Target -->
<body name="target" pos="0 0 0">
<joint name="target_x" type="slide" axis="1 0 0" />
<joint name="target_y" type="slide" axis="0 1 0" />
<geom name="target" type="sphere" size=".02" material="effector" />
<site name="target" size=".005" />
</body>
</worldbody>
<actuator>
<motor name="shoulder" joint="shoulder" />
<motor name="wrist" joint="wrist" />
</actuator>
</mujoco>

115
motrix_envs/src/motrix_envs/basic/reacher/reacher_np.py Normal file
View File

@@ -0,0 +1,115 @@
# 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.reacher.cfg import ReacherEnvCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-reacher", "np")
class Reacher2DEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: ReacherEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._target_size = cfg.target_size
self._finger = self._model.get_link("finger")
self._joint_limits = self._model.joint_limits
self._target_body = self._model.get_body("target")
self._target_xyz = np.zeros((num_envs, 3), dtype=np.float32)
self._init_obs_space()
self._init_action_space()
def _init_obs_space(self):
num_obs = self._model.num_dof_pos + 2 + self._model.num_dof_vel
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
low, high = self._model.actuator_ctrl_limits
self._action_space = gym.spaces.Box(low, high, (self._model.num_actuators,), dtype=np.float32)
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
def apply_action(self, actions, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
qvel = data.dof_vel
finger_xy = self._finger.get_pose(data)[:, :2]
to_target = self._target_xyz[:, :2] - finger_xy
return np.concatenate([qpos, to_target, qvel], axis=-1)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
finger_xy = self._finger.get_pose(data)[:, :2]
dist = np.linalg.norm(self._target_xyz[:, :2] - finger_xy, axis=-1)
rwd = reward.tolerance(
dist, bounds=(0.0, self._target_size), margin=self._target_size, value_at_margin=0.0, sigmoid="linear"
)
terminated = np.isnan(obs).any(axis=-1)
rwd[terminated] = 0.0
state.info["target_info"] = {"positions": self._target_xyz.copy(), "size": self._target_size}
state.info["Reward"] = {"distance": dist, "tolerance": rwd.copy()}
return state.replace(obs=obs, reward=rwd, terminated=terminated)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
"""Reset environment with randomized target position in xy plane (z=0)."""
data.reset(self._model)
num_reset = data.shape[0]
dof_pos = np.zeros((num_reset, self._model.num_dof_pos))
dof_pos[:, 0] = np.random.uniform(-np.pi, np.pi, size=(num_reset,))
dof_pos[:, 1] = np.random.uniform(-np.pi, np.pi, size=(num_reset,))
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
target_x = np.random.uniform(-0.15, 0.15, size=(num_reset,))
target_y = np.random.uniform(0.15, 0.15, size=(num_reset,))
target_dof_pos = np.stack([target_x, target_y], axis=-1)
self._target_body.set_dof_pos(data, target_dof_pos)
self._model.forward_kinematic(data)
target_pose = self._target_body.get_pose(data)
self._target_xyz = target_pose.copy()
self._target_xyz[:, 2] = 0.0
obs = self._get_obs(data)
rewards = {"distance": np.zeros((num_reset,)), "tolerance": np.zeros((num_reset,))}
info = {"Reward": rewards, "target_info": {"positions": self._target_xyz.copy(), "size": self._target_size}}
return obs, info

17
motrix_envs/src/motrix_envs/basic/walker/walker.xml
View File

@@ -1,6 +1,4 @@
<mujoco model="planar walker"> <mujoco model="planar walker">
<include file="../../common/visual.xml" />
<include file="../../common/skybox.xml" />
<include file="../../common/materials.xml" /> <include file="../../common/materials.xml" />
<option timestep="0.005"> <option timestep="0.005">
@@ -21,9 +19,20 @@
</default> </default>
</default> </default>
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody> <worldbody>
<light name="light" dir="0 -1 -1" directional="true"/> <light name="light" dir="0 1 -1" directional="true" />
<geom name="floor" type="plane" conaffinity="1" pos="248 0 0" size="250 .8 .2" material="grid" zaxis="0 0 1" /> <geom name="floor" type="plane" conaffinity="1" size="0 0 0.01"
material="motphys-ground"
zaxis="0 0 1" />
<body name="torso" pos="0 0 1.3" childclass="walker"> <body name="torso" pos="0 0 1.3" childclass="walker">
<joint name="rootz" axis="0 0 1" type="slide" limited="false" armature="0" damping="0" /> <joint name="rootz" axis="0 0 1" type="slide" limited="false" armature="0" damping="0" />
<joint name="rootx" axis="1 0 0" type="slide" limited="false" armature="0" damping="0" /> <joint name="rootx" axis="1 0 0" type="slide" limited="false" armature="0" damping="0" />

3
motrix_envs/src/motrix_envs/common/materials.xml
View File

@@ -5,7 +5,8 @@ for example receiving a positive reward.
--> -->
<mujoco> <mujoco>
<asset> <asset>
<texture name="grid" type="2d" builtin="checker" mark="edge" rgb1="0.4314 0.5294 0.6431" rgb2="0.8157 0.8549 0.9059" markrgb="0.8 0.8 0.8" width="300" height="300"/> <texture name="grid" type="2d" builtin="checker" mark="edge" rgb1="0.4314 0.5294 0.6431"
rgb2="0.8157 0.8549 0.9059" markrgb="0.8 0.8 0.8" width="300" height="300" />
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2" /> <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2" />
<material name="self" rgba=".7 .5 .3 1" /> <material name="self" rgba=".7 .5 .3 1" />
<material name="self_default" rgba=".7 .5 .3 1" /> <material name="self_default" rgba=".7 .5 .3 1" />

3
motrix_envs/src/motrix_envs/common/motphys-ground.png Normal file
View File

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

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

@@ -13,4 +13,4 @@
# limitations under the License. # limitations under the License.
# ============================================================================== # ==============================================================================
from . import go1 # noqa: F401 register envs from . import anymal_c, go1 # noqa: F401 register envs

18
motrix_envs/src/motrix_envs/locomotion/anymal_c/__init__.py Normal file
View File

@@ -0,0 +1,18 @@
# 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 anymal_c_np, cfg # noqa: F401
from .anymal_c_np import AnymalCEnv # noqa: F401
from .cfg import AnymalCEnvCfg # noqa: F401

654
motrix_envs/src/motrix_envs/locomotion/anymal_c/anymal_c_np.py Normal file
View File

@@ -0,0 +1,654 @@
# 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.math.quaternion import Quaternion
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import AnymalCEnvCfg
@registry.env("anymal_c_navigation_flat", "np")
class AnymalCEnv(NpEnv):
_cfg: AnymalCEnvCfg
def __init__(self, cfg: AnymalCEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self._body = self._model.get_body(cfg.asset.body_name)
self._init_contact_geometry()
# Get target marker body
self._target_marker_body = self._model.get_body("target_marker")
self._action_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(12,), dtype=np.float32)
# Observation space: linvel(3) + gyro(3) + gravity(3) + joint_pos(12) + joint_vel(12) + last_actions(12) +
# commands(3) + position_error(2) + heading_error(1) + distance(1) + reached_flag(1) + stop_ready_flag(1) = 54
self._observation_space = gym.spaces.Box(low=-np.inf, high=np.inf, shape=(54,), dtype=np.float32)
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._num_action = self._model.num_actuators
self._init_dof_pos = self._model.compute_init_dof_pos()
self._init_dof_vel = np.zeros(
(self._model.num_dof_vel,),
dtype=np.float32,
)
self._init_buffer()
def _init_buffer(self):
cfg = self._cfg
self.default_angles = np.zeros(self._num_action, dtype=np.float32)
# PD parameters controlled by kp and kv in XML
# Normalization coefficients
self.commands_scale = np.array(
[cfg.normalization.lin_vel, cfg.normalization.lin_vel, cfg.normalization.ang_vel], dtype=np.float32
)
# Set default joint angles
for i in range(self._model.num_actuators):
for name, angle in cfg.init_state.default_joint_angles.items():
if name in self._model.actuator_names[i]:
self.default_angles[i] = angle
self._init_dof_pos[-self._num_action :] = self.default_angles
def _init_contact_geometry(self):
"""Initialize geometry indices required for contact detection"""
cfg = self._cfg
self.ground_index = self._model.get_geom_index(cfg.asset.ground_name)
# Initialize contact detection matrix
self._init_termination_contact()
self._init_foot_contact()
def _init_termination_contact(self):
"""Initialize termination contact detection"""
cfg = self._cfg
# Find base geometries
base_indices = []
for base_name in cfg.asset.terminate_after_contacts_on:
try:
base_idx = self._model.get_geom_index(base_name)
if base_idx is not None:
base_indices.append(base_idx)
else:
print(f"Warning: Geom '{base_name}' not found in model")
except Exception as e:
print(f"Warning: Error finding base geom '{base_name}': {e}")
# Create base-ground contact detection matrix
if base_indices:
self.termination_contact = np.array([[idx, self.ground_index] for idx in base_indices], dtype=np.uint32)
self.num_termination_check = self.termination_contact.shape[0]
else:
# Use empty array
self.termination_contact = np.zeros((0, 2), dtype=np.uint32)
self.num_termination_check = 0
print("Warning: No base contacts configured for termination")
def _init_foot_contact(self):
"""Initialize foot contact detection"""
cfg = self._cfg
foot_indices = []
for foot_name in cfg.asset.foot_names:
try:
foot_idx = self._model.get_geom_index(foot_name)
if foot_idx is not None:
foot_indices.append(foot_idx)
else:
print(f"Warning: Foot geom '{foot_name}' not found in model")
except Exception as e:
print(f"Warning: Error finding foot geom '{foot_name}': {e}")
# Create foot-ground contact detection matrix
if foot_indices:
self.foot_contact_check = np.array([[idx, self.ground_index] for idx in foot_indices], dtype=np.uint32)
self.num_foot_check = self.foot_contact_check.shape[0]
else:
self.foot_contact_check = np.zeros((0, 2), dtype=np.uint32)
self.num_foot_check = 0
print("Warning: No foot contacts configured")
def get_dof_pos(self, data: mtx.SceneData):
return self._body.get_joint_dof_pos(data)
def get_dof_vel(self, data: mtx.SceneData):
return self._body.get_joint_dof_vel(data)
def _extract_root_state(self, data):
"""
Extract root state from self._body
"""
pose = self._body.get_pose(data)
root_pos = pose[:, :3]
root_quat = pose[:, 3:7]
# Get velocity from sensor
root_linvel = self._model.get_sensor_value(self._cfg.sensor.base_linvel, data)
return root_pos, root_quat, root_linvel
@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):
# Save current action for incremental control
if "current_action" not in state.info:
state.info["current_actions"] = np.zeros_like(actions)
state.info["last_actions"] = state.info["current_actions"]
state.info["current_actions"] = actions
# Position control mode: directly input target angles
actions_scaled = actions * self._cfg.control_config.action_scale
state.data.actuator_ctrls = self.default_angles + actions_scaled
return state
def update_state(self, state: NpEnvState):
data = state.data
# Get root state
root_pos, root_quat, root_vel = self._extract_root_state(data)
# Joint states (leg joints)
joint_pos = self.get_dof_pos(data) # [num_envs, 12]
joint_vel = self.get_dof_vel(data) # [num_envs, 12]
joint_pos_rel = joint_pos - self.default_angles
# Get sensor data
base_lin_vel = root_vel[:, :3]
gyro = self._model.get_sensor_value(self._cfg.sensor.base_gyro, data)
projected_gravity = self._compute_projected_gravity(root_quat)
# Get commands - convert to relative velocity commands
pose_commands = state.info["pose_commands"]
robot_position = root_pos[:, :2]
robot_heading = Quaternion.get_yaw(root_quat)
target_position = pose_commands[:, :2]
target_heading = pose_commands[:, 2]
# Calculate desired velocity (based on position error)
position_error = target_position - robot_position
distance_to_target = np.linalg.norm(position_error, axis=1)
position_threshold = 0.3
reached_position = distance_to_target < position_threshold
desired_vel_xy = np.clip(position_error * 1.0, -1.0, 1.0) # Simple P controller
desired_vel_xy = np.where(reached_position[:, np.newaxis], 0.0, desired_vel_xy) # Velocity is 0 after reaching
# Calculate desired angular velocity (based on heading error)
heading_diff = target_heading - robot_heading
heading_diff = np.where(heading_diff > np.pi, heading_diff - 2 * np.pi, heading_diff)
heading_diff = np.where(heading_diff < -np.pi, heading_diff + 2 * np.pi, heading_diff)
heading_threshold = np.deg2rad(15)
reached_heading = np.abs(heading_diff) < heading_threshold
reached_all = np.logical_and(reached_position, reached_heading)
# Angular velocity command calculation + deadband
desired_yaw_rate = np.clip(heading_diff * 1.0, -1.0, 1.0)
deadband_yaw = np.deg2rad(8)
desired_yaw_rate = np.where(np.abs(heading_diff) < deadband_yaw, 0.0, desired_yaw_rate)
# Reset to zero after reaching
desired_yaw_rate = np.where(reached_all, 0.0, desired_yaw_rate)
desired_vel_xy = np.where(reached_all[:, np.newaxis], 0.0, desired_vel_xy)
state.info["desired_vel_xy"] = desired_vel_xy
# Combine into velocity commands
velocity_commands = np.concatenate([desired_vel_xy, desired_yaw_rate[:, np.newaxis]], axis=-1)
# Normalize observations
noisy_linvel = base_lin_vel * self._cfg.normalization.lin_vel
noisy_gyro = gyro * self._cfg.normalization.ang_vel
noisy_joint_angle = joint_pos_rel * self._cfg.normalization.dof_pos
noisy_joint_vel = joint_vel * self._cfg.normalization.dof_vel
command_normalized = velocity_commands * self.commands_scale
last_actions = state.info["current_actions"]
# Calculate task-related observations
position_error_normalized = position_error / 5.0 # Normalize to reasonable range
heading_error_normalized = heading_diff / np.pi # Normalize to [-1, 1]
distance_normalized = np.clip(distance_to_target / 5.0, 0, 1) # Normalize distance
reached_flag = reached_all.astype(np.float32) # Whether target is reached
# Calculate if zero_ang standard is met: reached and angular velocity close to zero
stop_ready = np.logical_and(reached_all, np.abs(gyro[:, 2]) < 5e-2)
stop_ready_flag = stop_ready.astype(np.float32)
obs = np.concatenate(
[
noisy_linvel, # 3
noisy_gyro, # 3
projected_gravity, # 3
noisy_joint_angle, # 12
noisy_joint_vel, # 12
last_actions, # 12
command_normalized, # 3
position_error_normalized, # 2 - Position error vector to target
heading_error_normalized[:, np.newaxis], # 1 - Heading error
distance_normalized[:, np.newaxis], # 1 - Distance to target
reached_flag[:, np.newaxis], # 1 - Whether reached
stop_ready_flag[:, np.newaxis], # 1 - Whether stop standard is met
],
axis=-1,
)
assert obs.shape == (data.shape[0], 54)
# Update target position marker
self._update_target_marker(data, pose_commands)
# Update arrow visualization (no physical effect)
base_lin_vel_xy = base_lin_vel[:, :2]
self._update_heading_arrows(data, root_pos, desired_vel_xy, base_lin_vel_xy)
# Calculate reward
reward = self._compute_reward(data, state.info, velocity_commands)
# Calculate termination conditions
terminated_state = self._compute_terminated(state)
terminated = terminated_state.terminated
state.obs = obs
state.reward = reward
state.terminated = terminated
return state
def _update_heading_arrows(
self, data: mtx.SceneData, robot_pos: np.ndarray, desired_vel_xy: np.ndarray, base_lin_vel_xy: np.ndarray
):
"""
Update arrow positions (using DOF to control freejoint, no physical effect)
robot_pos: [num_envs, 3] - Robot position
desired_vel_xy: [num_envs, 2] - Desired linear velocity (ground coordinates)
base_lin_vel_xy: [num_envs, 2] - Actual linear velocity (ground coordinates)
"""
arrow_height = 0.76 # Arrow height (base=0.56 + 0.2)
cur_yaw = np.where(
np.linalg.norm(base_lin_vel_xy, axis=1) > 1e-3,
np.arctan2(base_lin_vel_xy[:, 1], base_lin_vel_xy[:, 0]),
0.0,
)
robot_arrow_pos = robot_pos.copy()
robot_arrow_pos[:, 2] = arrow_height
robot_arrow_quat = Quaternion.from_euler(0, 0, cur_yaw)
mocap = self._model.get_body("robot_heading_arrow").mocap
mocap.set_pose(data, np.concatenate([robot_arrow_pos, robot_arrow_quat], axis=1))
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
)
desired_arrow_quat = Quaternion.from_euler(0, 0, des_yaw)
mocap = self._model.get_body("desired_heading_arrow").mocap
mocap.set_pose(data, np.concatenate([robot_arrow_pos, desired_arrow_quat], axis=1))
def _compute_reward(self, data: mtx.SceneData, info: dict, velocity_commands: np.ndarray) -> np.ndarray:
"""
Velocity tracking reward mechanism
velocity_commands: [num_envs, 3] - (vx, vy, vyaw)
"""
# Calculate termination condition penalties
termination_penalty = np.zeros(self._num_envs, dtype=np.float32)
# Check if DOF velocity exceeds limit
dof_vel = self.get_dof_vel(data)
vel_max = np.abs(dof_vel).max(axis=1)
vel_overflow = vel_max > self._cfg.max_dof_vel
vel_extreme = (np.isnan(dof_vel).any(axis=1)) | (np.isinf(dof_vel).any(axis=1)) | (vel_max > 1e6)
termination_penalty = np.where(vel_overflow | vel_extreme, -20.0, termination_penalty)
# Robot base contacts ground penalty
cquerys = self._model.get_contact_query(data)
termination_check = cquerys.is_colliding(self.termination_contact)
termination_check = termination_check.reshape((self._num_envs, self.num_termination_check))
base_contact = termination_check.any(axis=1)
termination_penalty = np.where(base_contact, -20.0, termination_penalty)
# Side flip penalty
pose = self._body.get_pose(data)
root_quat = pose[:, 3:7]
proj_g = self._compute_projected_gravity(root_quat)
gxy = np.linalg.norm(proj_g[:, :2], axis=1)
gz = proj_g[:, 2]
tilt_angle = np.arctan2(gxy, np.abs(gz))
side_flip_mask = tilt_angle > np.deg2rad(75)
termination_penalty = np.where(side_flip_mask, -20.0, termination_penalty)
# 1. Linear velocity tracking reward
base_lin_vel = self._model.get_sensor_value(self._cfg.sensor.base_linvel, data)
lin_vel_error = np.sum(np.square(velocity_commands[:, :2] - base_lin_vel[:, :2]), axis=1)
tracking_lin_vel = np.exp(-lin_vel_error / 0.25) # tracking_sigma = 0.25
# 2. Angular velocity tracking reward / heading error penalty (mixed strategy)
gyro = self._model.get_sensor_value(self._cfg.sensor.base_gyro, data)
ang_vel_error = np.square(velocity_commands[:, 2] - gyro[:, 2])
tracking_ang_vel = np.exp(-ang_vel_error / 0.25)
# Get robot position and heading for arrival determination
robot_position = pose[:, :2]
robot_heading = Quaternion.get_yaw(root_quat)
target_position = info["pose_commands"][:, :2]
target_heading = info["pose_commands"][:, 2]
position_error = target_position - robot_position
distance_to_target = np.linalg.norm(position_error, axis=1)
heading_diff = target_heading - robot_heading
heading_diff = np.where(heading_diff > np.pi, heading_diff - 2 * np.pi, heading_diff)
heading_diff = np.where(heading_diff < -np.pi, heading_diff + 2 * np.pi, heading_diff)
position_threshold = 0.3
reached_position = distance_to_target < position_threshold
heading_threshold = np.deg2rad(15)
reached_heading = np.abs(heading_diff) < heading_threshold
reached_all = np.logical_and(reached_position, reached_heading)
# One-time reward for first time reaching position
info["ever_reached"] = info.get("ever_reached", np.zeros(self._num_envs, dtype=bool))
first_time_reach = np.logical_and(reached_all, ~info["ever_reached"])
info["ever_reached"] = np.logical_or(info["ever_reached"], reached_all)
arrival_bonus = np.where(first_time_reach, 10.0, 0.0)
# Distance approach reward: incentivize getting closer to target
# Use historical minimum distance to calculate progress
if "min_distance" not in info:
info["min_distance"] = distance_to_target.copy()
distance_improvement = info["min_distance"] - distance_to_target
info["min_distance"] = np.minimum(info["min_distance"], distance_to_target)
approach_reward = np.clip(distance_improvement * 4.0, -1.0, 1.0) # Reward 5 points for every 1 meter closer
# 3. Orientation stability reward (penalize deviation from normal standing posture)
# When standing normally, projected_gravity ≈ [0, 0, -1]
projected_gravity = self._compute_projected_gravity(root_quat)
orientation_penalty = (
np.square(projected_gravity[:, 0])
+ np.square(projected_gravity[:, 1])
+ np.square(projected_gravity[:, 2] + 1.0)
)
# Arrival and stop determination (reward bonus)
speed_xy = np.linalg.norm(base_lin_vel[:, :2], axis=1)
zero_ang_mask = np.abs(gyro[:, 2]) < 0.05 # Relax to 0.05 rad/s ≈ 2.86°/s
zero_ang_bonus = np.where(np.logical_and(reached_all, zero_ang_mask), 6.0, 0.0)
stop_base = 2 * (0.8 * np.exp(-((speed_xy / 0.2) ** 2)) + 1.2 * np.exp(-((np.abs(gyro[:, 2]) / 0.1) ** 4)))
stop_bonus = np.where(reached_all, stop_base + zero_ang_bonus, 0.0)
# 4. Z-axis linear velocity penalty
lin_vel_z_penalty = np.square(base_lin_vel[:, 2])
# 5. XY-axis angular velocity penalty
ang_vel_xy_penalty = np.sum(np.square(gyro[:, :2]), axis=1)
# 6. Torque penalty
torque_penalty = np.sum(np.square(data.actuator_ctrls), axis=1)
# 7. Joint velocity penalty
joint_vel = self.get_dof_vel(data)
dof_vel_penalty = np.sum(np.square(joint_vel), axis=1)
# 8. Action change penalty
action_diff = info["current_actions"] - info["last_actions"]
action_rate_penalty = np.sum(np.square(action_diff), axis=1)
# Combined reward
# After reaching: stop all positive rewards, only keep stop reward and penalties
reward = np.where(
reached_all,
# After reaching: only stop reward and penalties
(
stop_bonus
+ arrival_bonus
- 2.0 * lin_vel_z_penalty
- 0.05 * ang_vel_xy_penalty
- 0.0 * orientation_penalty
- 0.00001 * torque_penalty
- 0.0 * dof_vel_penalty
- 0.001 * action_rate_penalty
+ termination_penalty # Termination condition penalty
),
# Not reached: normal rewards
(
1.5 * tracking_lin_vel # Increase linear velocity tracking weight
+ 0.3 * tracking_ang_vel # Decrease angular velocity weight
+ approach_reward # Approach reward
- 2.0 * lin_vel_z_penalty
- 0.05 * ang_vel_xy_penalty
- 0.0 * orientation_penalty
- 0.00001 * torque_penalty
- 0.0 * dof_vel_penalty
- 0.001 * action_rate_penalty
+ termination_penalty # Termination condition penalty
),
)
return reward
def _update_target_marker(self, data: mtx.SceneData, pose_commands: np.ndarray):
"""
Update position and orientation of target marker
"""
num_envs = data.shape[0]
arrow_pos = pose_commands.copy()
arrow_pos[:, 2] = 0.05
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])
mocap = self._model.get_body("target_marker").mocap
mocap.set_pose(data, np.concatenate([arrow_pos, arrow_quat], axis=1))
def _compute_terminated(self, state: NpEnvState) -> NpEnvState:
data = state.data
terminated = np.zeros(self._num_envs, dtype=bool)
# Check if DOF velocity exceeds limit (prevent inf/numerical divergence)
dof_vel = self.get_dof_vel(data)
vel_max = np.abs(dof_vel).max(axis=1)
vel_overflow = vel_max > self._cfg.max_dof_vel
# Extreme velocity/NaN/Inf protection
vel_extreme = (np.isnan(dof_vel).any(axis=1)) | (np.isinf(dof_vel).any(axis=1)) | (vel_max > 1e6)
terminated = np.logical_or(terminated, vel_overflow)
terminated = np.logical_or(terminated, vel_extreme)
# Robot base contacts ground termination
cquerys = self._model.get_contact_query(data)
termination_check = cquerys.is_colliding(self.termination_contact)
termination_check = termination_check.reshape((self._num_envs, self.num_termination_check))
base_contact = termination_check.any(axis=1)
terminated = np.logical_or(terminated, base_contact)
# Side flip termination: tilt angle exceeds 75°
pose = self._body.get_pose(data)
root_quat = pose[:, 3:7]
proj_g = self._compute_projected_gravity(root_quat)
gxy = np.linalg.norm(proj_g[:, :2], axis=1)
gz = proj_g[:, 2]
tilt_angle = np.arctan2(gxy, np.abs(gz))
side_flip_mask = tilt_angle > np.deg2rad(75)
terminated = np.logical_or(terminated, side_flip_mask)
return state.replace(terminated=terminated)
def reset(self, data: mtx.SceneData, done: np.ndarray = None) -> tuple[np.ndarray, dict]:
cfg: AnymalCEnvCfg = self._cfg
num_envs = data.shape[0]
# First generate robot initial position (in world coordinates)
pos_range = cfg.init_state.pos_randomization_range
robot_init_x = np.random.uniform(
pos_range[0],
pos_range[2], # x_min, x_max
num_envs,
)
robot_init_y = np.random.uniform(
pos_range[1],
pos_range[3], # y_min, y_max
num_envs,
)
robot_init_pos = np.stack([robot_init_x, robot_init_y], axis=1) # [num_envs, 2]
# Generate target position: offset relative to robot initial position
# pose_command_range now represents offset range relative to robot
target_offset = np.random.uniform(
low=cfg.commands.pose_command_range[:2], high=cfg.commands.pose_command_range[3:5], size=(num_envs, 2)
)
target_positions = robot_init_pos + target_offset # Target position in world coordinates
# Generate target heading (absolute heading, random in horizontal direction)
target_headings = np.random.uniform(
low=cfg.commands.pose_command_range[2], high=cfg.commands.pose_command_range[5], size=(num_envs, 1)
)
pose_commands = np.concatenate([target_positions, target_headings], axis=1)
# Set initial state - avoid adding noise to quaternion
init_dof_pos = np.tile(self._init_dof_pos, (*data.shape, 1))
init_dof_vel = np.tile(self._init_dof_vel, (*data.shape, 1))
# Create noise - do not add noise to quaternion
noise_pos = np.zeros((*data.shape, self._num_dof_pos), dtype=np.float32)
# Base position (DOF 0-2): use the generated random initial position
noise_pos[:, 0] = robot_init_x - cfg.init_state.pos[0] # Offset from default position
noise_pos[:, 1] = robot_init_y - cfg.init_state.pos[1]
# No noise on Z axis, maintain fixed height to avoid falling feeling
# All velocities set to 0, ensure completely stationary
noise_vel = np.zeros((*data.shape, self._num_dof_vel), dtype=np.float32)
dof_pos = init_dof_pos + noise_pos
dof_vel = init_dof_vel + noise_vel
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
# Update target position marker
self._update_target_marker(data, pose_commands)
# Get root state
root_pos, root_quat, root_vel = self._extract_root_state(data)
# Joint states (leg joints)
joint_pos = self.get_dof_pos(data)
joint_vel = self.get_dof_vel(data)
joint_pos_rel = joint_pos - self.default_angles
# Get sensor data
base_lin_vel = root_vel[:, :3]
gyro = self._model.get_sensor_value(self._cfg.sensor.base_gyro, data)
projected_gravity = self._compute_projected_gravity(root_quat)
# Calculate velocity commands (consistent with update_state)
robot_position = root_pos[:, :2]
robot_heading = Quaternion.get_yaw(root_quat)
target_position = pose_commands[:, :2]
target_heading = pose_commands[:, 2]
position_error = target_position - robot_position
distance_to_target = np.linalg.norm(position_error, axis=1)
# Position threshold: considered reached within 0.1 meters
position_threshold = 0.1
reached_position = distance_to_target < position_threshold
desired_vel_xy = np.clip(position_error * 1.0, -1.0, 1.0)
desired_vel_xy = np.where(reached_position[:, np.newaxis], 0.0, desired_vel_xy) # Velocity is 0 after reaching
# Actual linear velocity XY
base_lin_vel_xy = base_lin_vel[:, :2]
# Update arrow visualization (no physical effect)
self._update_heading_arrows(data, root_pos, desired_vel_xy, base_lin_vel_xy)
heading_diff = target_heading - robot_heading
heading_diff = np.where(heading_diff > np.pi, heading_diff - 2 * np.pi, heading_diff)
heading_diff = np.where(heading_diff < -np.pi, heading_diff + 2 * np.pi, heading_diff)
# Heading threshold: considered reached within 15 degrees
heading_threshold = np.deg2rad(15)
reached_heading = np.abs(heading_diff) < heading_threshold
desired_yaw_rate = np.clip(heading_diff * 1.0, -1.0, 1.0)
reached_all = np.logical_and(reached_position, reached_heading)
desired_yaw_rate = np.where(reached_all, 0.0, desired_yaw_rate) # Velocity is 0 after reaching
desired_vel_xy = np.where(reached_all[:, np.newaxis], 0.0, desired_vel_xy) # Velocity is 0 after reaching
# Ensure desired_yaw_rate is 1D array
if desired_yaw_rate.ndim > 1:
desired_yaw_rate = desired_yaw_rate.flatten()
velocity_commands = np.concatenate([desired_vel_xy, desired_yaw_rate[:, np.newaxis]], axis=-1)
# Normalize observations (consistent with update_state)
noisy_linvel = base_lin_vel * self._cfg.normalization.lin_vel
noisy_gyro = gyro * self._cfg.normalization.ang_vel
noisy_joint_angle = joint_pos_rel * self._cfg.normalization.dof_pos
noisy_joint_vel = joint_vel * self._cfg.normalization.dof_vel
command_normalized = velocity_commands * self.commands_scale
last_actions = np.zeros((num_envs, self._num_action), dtype=np.float32)
# Calculate task-related observations (consistent with update_state)
position_error_normalized = position_error / 5.0
heading_error_normalized = heading_diff / np.pi
distance_normalized = np.clip(distance_to_target / 5.0, 0, 1)
reached_flag = reached_all.astype(np.float32)
# Calculate if zero_ang standard is met
stop_ready = np.logical_and(reached_all, np.abs(gyro[:, 2]) < 5e-2)
stop_ready_flag = stop_ready.astype(np.float32)
obs = np.concatenate(
[
noisy_linvel, # 3
noisy_gyro, # 3
projected_gravity, # 3
noisy_joint_angle, # 12
noisy_joint_vel, # 12
last_actions, # 12
command_normalized, # 3
position_error_normalized, # 2
heading_error_normalized[:, np.newaxis], # 1
distance_normalized[:, np.newaxis], # 1
reached_flag[:, np.newaxis], # 1
stop_ready_flag[:, np.newaxis], # 1
],
axis=-1,
)
assert obs.shape == (num_envs, 54)
info = {
"pose_commands": pose_commands,
"last_actions": np.zeros((num_envs, self._num_action), dtype=np.float32),
"current_actions": np.zeros((num_envs, self._num_action), dtype=np.float32),
"ever_reached": np.zeros(num_envs, dtype=bool),
"min_distance": distance_to_target.copy(), # Initialize minimum distance
}
return obs, info
def _compute_projected_gravity(self, quat: np.ndarray) -> np.ndarray:
gravity = np.array([0.0, 0.0, -1.0], dtype=np.float32)
return Quaternion.rotate_vector(quat, gravity)

128
motrix_envs/src/motrix_envs/locomotion/anymal_c/cfg.py Normal file
View File

@@ -0,0 +1,128 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass, field
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/xmls/scene.xml"
@dataclass
class NoiseConfig:
level: float = 1.0
scale_joint_angle: float = 0.03
scale_joint_vel: float = 1.5
scale_gyro: float = 0.2
scale_gravity: float = 0.05
scale_linvel: float = 0.1
@dataclass
class ControlConfig:
# stiffness[N*m/rad] uses kp parameter from XML, recorded for reference only
# damping[N*m*s/rad] uses kv parameter from XML, recorded for reference only
action_scale = 0.06 # action scale
@dataclass
class InitState:
# the initial position of the robot in the world frame
pos = [0.0, 0.0, 0.5] # Z-axis height matches the initial height of base in XML
# position randomization range [x_min, y_min, x_max, y_max]
pos_randomization_range = [-10.0, -10.0, 10.0, 10.0] # randomly distributed over 20m x 20m range on ground
# the default angles for all joints. key = joint name, value = target angle [rad]
default_joint_angles = {
"LF_HAA": 0.0, # [rad]
"RF_HAA": 0.0, # [rad]
"LH_HAA": 0.0, # [rad]
"RH_HAA": 0.0, # [rad]
"LF_HFE": 0.4, # [rad]
"RF_HFE": 0.4, # [rad]
"LH_HFE": -0.4, # [rad]
"RH_HFE": -0.4, # [rad]
"LF_KFE": -0.8, # [rad]
"RF_KFE": -0.8, # [rad]
"LH_KFE": 0.8, # [rad]
"RH_KFE": 0.8, # [rad]
}
@dataclass
class Commands:
# offset range of target position relative to robot initial position
# [dx_min, dy_min, yaw_min, dx_max, dy_max, yaw_max]
# dx/dy: offset relative to robot initial position (meters)
# yaw: target absolute orientation (radians), random horizontal direction
pose_command_range = [-5.0, -5.0, -3.14, 5.0, 5.0, 3.14]
@dataclass
class Normalization:
lin_vel = 2.0
ang_vel = 0.25
dof_pos = 1.0
dof_vel = 0.05
@dataclass
class Asset:
body_name = "base"
foot_names = ["LF_FOOT", "RF_FOOT", "LH_FOOT", "RH_FOOT"]
terminate_after_contacts_on = ["base"]
ground_name = "ground"
@dataclass
class Sensor:
base_linvel = "base_linvel"
base_gyro = "base_gyro"
@dataclass
class RewardConfig:
scales: dict[str, float] = field(
default_factory=lambda: {
"termination": -400.0,
"position_tracking": 0.5,
"fine_position_tracking": 0.5,
"orientation": -0.2,
}
)
@registry.envcfg("anymal_c_navigation_flat")
@dataclass
class AnymalCEnvCfg(EnvCfg):
model_file: str = model_file
reset_noise_scale: float = 0.01
max_episode_seconds: float = 7.0
sim_dt: float = 0.01
ctrl_dt: float = 0.01
reset_yaw_scale: float = 0.1
max_dof_vel: float = 100.0 # maximum joint velocity threshold, greater tolerance during early training
noise_config: NoiseConfig = field(default_factory=NoiseConfig)
control_config: ControlConfig = field(default_factory=ControlConfig)
reward_config: RewardConfig = field(default_factory=RewardConfig)
init_state: InitState = field(default_factory=InitState)
commands: Commands = field(default_factory=Commands)
normalization: Normalization = field(default_factory=Normalization)
asset: Asset = field(default_factory=Asset)
sensor: Sensor = field(default_factory=Sensor)

392
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/anymal_c.xml Normal file
View File

@@ -0,0 +1,392 @@
<mujoco model="anymal_c">
<compiler angle="radian" autolimits="true" />
<option cone="elliptic" impratio="100" iterations="60"/>
<default>
<material reflectance="0.5" metallic="0.0" roughness="0.7"/>
<default class="anymal_c">
<joint damping="1" frictionloss="0.1" />
<default class="visual">
<geom type="mesh" contype="0" conaffinity="0" group="2" />
<default class="visual_zflip">
<geom quat="0 0 0 1" />
</default>
</default>
<default class="collision">
<geom group="3" type="cylinder" contype="0" conaffinity="0"/>
<default class="foot">
<geom type="sphere" size="0.03" pos="0 0 0.02325" priority="1" solimp="0.015 1 0.03" contype="0" conaffinity="3"
condim="6"
friction="0.8 0.02 0.01" />
</default>
</default>
<default class="affine">
<position kp="200" kv="1" ctrlrange="-6.28 6.28" forcerange="-140 140" />
</default>
</default>
</default>
<asset>
<material name="black_plastic" rgba="0.007 0.007 0.007 1" />
<material name="green" rgba="0.052 0.178 0.033 1" />
<material name="red" rgba="0.178 0.006 0.005 1" />
<material name="yellow" rgba="0.178 0.119 0.006 1" />
<material name="lwl" rgba="0.8 0.8 0.8 1" />
<material name="base" texture="base" />
<material name="top_shell" texture="top_shell" />
<material name="bottom_shell" texture="bottom_shell" />
<material name="hip_l" texture="hip_l" />
<material name="thigh" texture="thigh" />
<material name="shank_l" texture="shank_l" metallic="1.0"/>
<material name="foot" texture="foot" />
<material name="hip_r" texture="hip_r" />
<material name="shank" texture="shank_r" metallic="1.0"/>
<material name="hatch" texture="hatch" />
<material name="remote" texture="remote" />
<material name="handle" texture="handle" />
<material name="face" texture="face" />
<material name="depth_camera" texture="depth_camera" />
<material name="wide_angle_camera" texture="wide_angle_camera" />
<material name="battery" texture="battery" />
<material name="lidar_cage" texture="lidar_cage" />
<material name="lidar" texture="lidar" />
<material name="drive" texture="drive" />
<texture type="2d" name="base" file="assets/base.png" />
<texture type="2d" name="top_shell" file="assets/top_shell.png" />
<texture type="2d" name="bottom_shell" file="assets/bottom_shell.png" />
<texture type="2d" name="remote" file="assets/remote.png" />
<texture type="2d" name="hatch" file="assets/hatch.png" />
<texture type="2d" name="handle" file="assets/handle.png" />
<texture type="2d" name="face" file="assets/face.png" />
<texture type="2d" name="battery" file="assets/battery.png" />
<texture type="2d" name="lidar_cage" file="assets/lidar_cage.png" />
<texture type="2d" name="depth_camera" file="assets/depth_camera.png" />
<texture type="2d" name="wide_angle_camera" file="assets/wide_angle_camera.png" />
<texture type="2d" name="lidar" file="assets/lidar.png" />
<texture type="2d" name="drive" file="assets/drive.png" />
<texture type="2d" name="hip_l" file="assets/hip_l.png" />
<texture type="2d" name="shank_l" file="assets/shank_l.png" />
<texture type="2d" name="foot" file="assets/foot.png" />
<texture type="2d" name="thigh" file="assets/thigh.png" />
<texture type="2d" name="shank_r" file="assets/shank_r.png" />
<texture type="2d" name="hip_r" file="assets/hip_r.png" />
<mesh file="assets/base_0.obj" />
<mesh file="assets/base_1.obj" />
<mesh file="assets/base_2.obj" />
<mesh file="assets/base_3.obj" />
<mesh file="assets/base_4.obj" />
<mesh file="assets/base_5.obj" />
<mesh file="assets/top_shell.obj" />
<mesh file="assets/bottom_shell.obj" />
<mesh file="assets/remote.obj" />
<mesh file="assets/handle.obj" />
<mesh file="assets/face.obj" />
<mesh file="assets/wide_angle_camera.obj" />
<mesh file="assets/depth_camera.obj" />
<mesh file="assets/battery.obj" />
<mesh file="assets/lidar_cage.obj" />
<mesh file="assets/lidar.obj" />
<mesh file="assets/drive.obj" />
<mesh file="assets/hip_l.obj" />
<mesh file="assets/thigh.obj" />
<mesh file="assets/shank_l.obj" />
<mesh file="assets/foot.obj" />
<mesh file="assets/hip_r.obj" />
<mesh file="assets/hatch.obj" />
<mesh file="assets/shank_r.obj" />
<!-- Target arrow mesh -->
<mesh name="arrow_cone" file="assets/cone.stl" scale="0.003 0.003 0.003" />
<mesh name="arrow" file="assets/arrow.obj"/>
<!-- Target arrow material -->
<material name="target_green" rgba="0 1 0 0.9" />
</asset>
<worldbody>
<!-- Target position marker: 3D arrow (cylinder + cone) -->
<body name="target_marker" pos="0 0 0.05" mocap="true">
<!-- Arrow head: cone, base attaches to cylinder end face, tip points forward -->
<!-- Cylinder end at x=0.3, cone base also near x=0.3 -->
<!-- Cone default orientation: tip points to +Z, need to rotate -90° around Y axis to make tip point to +X -->
<geom name="arrow_head" type="mesh" mesh="arrow"
pos="0.0 0.0 0.09" quat="1 0 0 0"
material="target_green" contype="0" conaffinity="0" />
</body>
<body name="base" pos="0 0 0.56" quat="1 0 0 0" childclass="anymal_c">
<freejoint />
<inertial mass="19.2035" pos="0.0025 0 0.0502071" quat="0.5 0.5 0.5 0.5"
diaginertia="0.639559 0.624031 0.217374" />
<!-- IMU sensor position -->
<site name="imu_site" pos="0 0 0" size="0.01" />
<geom mesh="base_0" material="green" class="visual" />
<geom mesh="base_1" material="yellow" class="visual" />
<geom mesh="base_2" material="red" class="visual" />
<geom mesh="base_3" material="black_plastic" class="visual" />
<geom mesh="base_4" material="lwl" class="visual" />
<geom mesh="base_5" material="base" class="visual" />
<geom name="base" class="collision" size="0.29 0.07 0.09" type="box" />
<geom class="collision" size="0.09 0.0725" pos="0.2175 0.07 0" quat="1 0 1 0" />
<geom class="collision" size="0.09 0.0725" pos="-0.2175 0.07 0" quat="1 0 1 0" />
<geom class="collision" size="0.09 0.0725" pos="0.2175 -0.07 0" quat="1 0 1 0" />
<geom class="collision" size="0.09 0.0725" pos="-0.2175 -0.07 0" quat="1 0 1 0" />
<geom material="top_shell" mesh="top_shell" class="visual" />
<geom material="bottom_shell" mesh="bottom_shell" class="visual" />
<geom material="remote" mesh="remote" class="visual" />
<geom material="handle" mesh="handle" class="visual" />
<geom pos="0.4145 0 0" material="face" mesh="face" class="visual" />
<geom class="collision" size="0.055 0.07 0.09" pos="0.4695 0 0" type="box" />
<geom class="collision" size="0.09 0.055" pos="0.4695 0.07 0" quat="1 0 1 0" />
<geom class="collision" size="0.09 0.055" pos="0.4695 -0.07 0" quat="1 0 1 0" />
<geom pos="0.46165 0 -0.0292" quat="0.965926 0 0.258819 0" material="depth_camera"
mesh="depth_camera"
class="visual" />
<geom pos="0.513 0 0.01497" material="wide_angle_camera" mesh="wide_angle_camera"
class="visual" />
<geom pos="-0.4145 0 0" material="face" mesh="face" class="visual_zflip" />
<geom class="collision" size="0.055 0.07 0.09" pos="-0.4695 0 0" quat="0 0 0 1" type="box" />
<geom class="collision" size="0.09 0.055" pos="-0.4695 -0.07 0" quat="0 -1 0 1" />
<geom class="collision" size="0.09 0.055" pos="-0.4695 0.07 0" quat="0 -1 0 1" />
<geom pos="-0.46165 0 -0.0292" quat="0 -0.258819 0 0.965926" material="depth_camera"
mesh="depth_camera"
class="visual" />
<geom pos="-0.513 0 0.01497" material="wide_angle_camera" mesh="wide_angle_camera"
class="visual_zflip" />
<geom material="battery" mesh="battery" class="visual" />
<geom pos="0 0.07646 0.02905" quat="0.683013 -0.183013 0.183013 0.683013"
material="depth_camera"
mesh="depth_camera" class="visual" />
<geom pos="0 -0.07646 0.02905" quat="0.683013 0.183013 0.183013 -0.683013"
material="depth_camera"
mesh="depth_camera" class="visual" />
<geom pos="-0.364 0 0.0735" material="lidar_cage" mesh="lidar_cage" class="visual" />
<geom class="collision" size="0.07 0.06" pos="-0.364 0 0.1335" />
<geom pos="-0.364 0 0.1422" quat="1 0 0 -1" material="lidar" mesh="lidar" class="visual" />
<geom pos="0.2999 0.104 0" quat="0.258819 0.965926 0 0" material="drive" mesh="drive"
class="visual" />
<geom pos="0.2999 -0.104 0" quat="0.258819 -0.965926 0 0" material="drive" mesh="drive"
class="visual" />
<geom pos="-0.2999 0.104 0" quat="0 0 0.965926 -0.258819" material="drive" mesh="drive"
class="visual" />
<geom pos="-0.2999 -0.104 0" quat="0 0 -0.965926 -0.258819" material="drive" mesh="drive"
class="visual" />
<geom pos="0.116 0 0.073" material="hatch" mesh="hatch" class="visual" />
<body name="LF_HIP" pos="0.2999 0.104 0" quat="0.258819 0.965926 0 0">
<inertial mass="2.781" pos="0.0566606 -0.015294 -0.00829784"
quat="-0.127978 0.709783 -0.135278 0.679359"
diaginertia="0.00585729 0.00491868 0.00329081" />
<joint name="LF_HAA" axis="1 0 0" range="-0.72 0.49" />
<geom quat="0.258819 -0.965926 0 0" material="hip_l" mesh="hip_l" class="visual" />
<geom pos="0.0599 -0.0725816 -0.041905" quat="0.183013 -0.683013 0.683013 0.183013"
material="drive"
mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0599 -0.0119598 -0.006905"
quat="-0.353553 -0.612372 0.612372 -0.353553" />
<body name="LF_THIGH" pos="0.0599 -0.0725816 -0.041905"
quat="0.183013 -0.683013 0.683013 0.183013">
<inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
quat="0.993166 -0.00515309 -0.0806592 0.0841972"
diaginertia="0.03025 0.0298943 0.00418465" />
<joint name="LF_HFE" axis="1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
<geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 -0.5 -0.5" />
<geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 -1"
type="box" />
<geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
<body name="LF_SHANK" pos="0.1003 0 -0.285">
<inertial mass="0.58842" pos="0.005462 -0.0612528 -0.0806598"
quat="0.992934 -0.115904 -0.00105487 -0.0254421"
diaginertia="0.0101637 0.00923838 0.00111927" />
<joint name="LF_KFE" axis="1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="shank_l" mesh="shank_l" class="visual" />
<geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 -0.5 -0.5" />
<geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 -0.057499 0"
quat="0.5 0.5 -0.5 -0.5"
type="box" />
<geom pos="0.01305 -0.08795 -0.33797" quat="0.382683 0 0 -0.92388" material="foot"
mesh="foot"
class="visual" />
<geom class="collision" size="0.0175 0.141252" pos="0.01305 -0.08795 -0.168985"
quat="1 0 0 -1" />
<geom name="LF_FOOT" class="foot" pos="0.01305 -0.08795 -0.31547" quat="1 0 0 -1" />
</body>
</body>
</body>
<body name="RF_HIP" pos="0.2999 -0.104 0" quat="0.258819 -0.965926 0 0">
<inertial mass="2.781" pos="0.0567633 0.015294 -0.00829784"
quat="0.13524 0.679072 0.127985 0.710065"
diaginertia="0.00585928 0.0049205 0.00329064" />
<joint name="RF_HAA" axis="1 0 0" range="-0.49 0.72" />
<geom quat="0.258819 0.965926 0 0" material="hip_r" mesh="hip_r" class="visual" />
<geom pos="0.0599 0.0725816 -0.041905" quat="0.183013 0.683013 0.683013 -0.183013"
material="drive" mesh="drive"
class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0599 0.0119598 -0.006905"
quat="-0.353553 0.612372 0.612372 0.353553" />
<body name="RF_THIGH" pos="0.0599 0.0725816 -0.041905"
quat="0.183013 0.683013 0.683013 -0.183013">
<inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
quat="0.992775 -0.00512735 -0.0806685 0.0886811"
diaginertia="0.0302511 0.0298933 0.0041845" />
<joint name="RF_HFE" axis="-1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
<geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 0.5 0.5" />
<geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 1"
type="box" />
<geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
<body name="RF_SHANK" pos="0.1003 0 -0.285">
<inertial mass="0.58842" pos="0.005462 0.0612528 -0.0806598"
quat="0.992934 0.115904 -0.00105487 0.0254421"
diaginertia="0.0101637 0.00923838 0.00111927" />
<joint name="RF_KFE" axis="-1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 1" material="shank" mesh="shank_r" class="visual" />
<geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 0.5 0.5" />
<geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 0.057499 0"
quat="0.5 0.5 0.5 0.5"
type="box" />
<geom pos="0.01305 0.08795 -0.33797" quat="0.382683 0 0 0.92388" material="foot"
mesh="foot" class="visual" />
<geom class="collision" size="0.0175 0.141252" pos="0.01305 0.08795 -0.168985"
quat="1 0 0 1" />
<geom name="RF_FOOT" class="foot" pos="0.01305 0.08795 -0.31547" quat="1 0 0 1" />
</body>
</body>
</body>
<body name="LH_HIP" pos="-0.2999 0.104 0" quat="0 0 0.965926 -0.258819">
<inertial mass="2.781" pos="0.0567633 0.015294 -0.00829784"
quat="0.13524 0.679072 0.127985 0.710065"
diaginertia="0.00585928 0.0049205 0.00329064" />
<joint name="LH_HAA" axis="-1 0 0" range="-0.72 0.49" />
<geom quat="-0.258819 -0.965926 0 0" material="hip_r" mesh="hip_r" class="visual" />
<geom pos="0.0599 0.0725816 -0.041905" quat="0.183013 0.683013 0.683013 -0.183013"
material="drive" mesh="drive"
class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0599 0.0119598 -0.006905"
quat="-0.353553 0.612372 0.612372 0.353553" />
<body name="LH_THIGH" pos="0.0599 0.0725816 -0.041905"
quat="0.183013 0.683013 0.683013 -0.183013">
<inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
quat="0.992775 -0.00512735 -0.0806685 0.0886811"
diaginertia="0.0302511 0.0298933 0.0041845" />
<joint name="LH_HFE" axis="1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
<geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 -0.5 -0.5" />
<geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 -1"
type="box" />
<geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
<body name="LH_SHANK" pos="0.1003 0 -0.285">
<inertial mass="0.58842" pos="0.005462 0.0612528 -0.0806598"
quat="0.992934 0.115904 -0.00105487 0.0254421"
diaginertia="0.0101637 0.00923838 0.00111927" />
<joint name="LH_KFE" axis="1 0 0" range="-9.42478 9.42478" />
<geom quat="-1 0 0 -1" material="shank" mesh="shank_r" class="visual" />
<geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 -0.5 -0.5" />
<geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 0.057499 0"
quat="0.5 0.5 -0.5 -0.5"
type="box" />
<geom pos="0.01305 0.08795 -0.33797" quat="-0.382683 0 0 -0.92388" material="foot"
mesh="foot"
class="visual" />
<geom class="collision" size="0.0175 0.141252" pos="0.01305 0.08795 -0.168985"
quat="1 0 0 -1" />
<geom name="LH_FOOT" class="foot" pos="0.01305 0.08795 -0.31547" quat="1 0 0 -1" />
</body>
</body>
</body>
<body name="RH_HIP" pos="-0.2999 -0.104 0" quat="0 0 -0.965926 -0.258819">
<inertial mass="2.781" pos="0.0566606 -0.015294 -0.00829784"
quat="-0.127978 0.709783 -0.135278 0.679359"
diaginertia="0.00585729 0.00491868 0.00329081" />
<joint name="RH_HAA" axis="-1 0 0" range="-0.49 0.72" />
<geom quat="-0.258819 0.965926 0 0" material="hip_l" mesh="hip_l" class="visual" />
<geom pos="0.0599 -0.0725816 -0.041905" quat="-0.183013 0.683013 -0.683013 -0.183013"
material="drive"
mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0599 -0.0119598 -0.006905"
quat="0.353553 0.612372 -0.612372 0.353553" />
<body name="RH_THIGH" pos="0.0599 -0.0725816 -0.041905"
quat="-0.183013 0.683013 -0.683013 -0.183013">
<inertial mass="3.071" pos="0.0308147 4.64995e-05 -0.245696"
quat="0.993166 -0.00515309 -0.0806592 0.0841972"
diaginertia="0.03025 0.0298943 0.00418465" />
<joint name="RH_HFE" axis="-1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="thigh" mesh="thigh" class="visual" />
<geom class="collision" size="0.065 0.04" pos="0.04 0 0" quat="0.5 0.5 0.5 0.5" />
<geom class="collision" size="0.0375 0.03 0.141314" pos="0.03 0 -0.141314" quat="1 0 0 1"
type="box" />
<geom pos="0.1003 0 -0.285" material="drive" mesh="drive" class="visual" />
<geom class="collision" size="0.05 0.07" pos="0.0303 0 -0.285" quat="1 0 1 0" />
<body name="RH_SHANK" pos="0.1003 0 -0.285">
<inertial mass="0.58842" pos="0.005462 -0.0612528 -0.0806598"
quat="0.992934 -0.115904 -0.00105487 -0.0254421"
diaginertia="0.0101637 0.00923838 0.00111927" />
<joint name="RH_KFE" axis="-1 0 0" range="-9.42478 9.42478" />
<geom quat="1 0 0 -1" material="shank_l" mesh="shank_l" class="visual" />
<geom class="collision" size="0.06 0.02" pos="0.02 0 0" quat="0.5 0.5 0.5 0.5" />
<geom class="collision" size="0.057499 0.03375 0.019" pos="0.02 -0.057499 0"
quat="0.5 0.5 0.5 0.5"
type="box" />
<geom pos="0.01305 -0.08795 -0.33797" quat="0.382683 0 0 -0.92388" material="foot"
mesh="foot"
class="visual" />
<geom class="collision" size="0.0175 0.141252" pos="0.01305 -0.08795 -0.168985"
quat="1 0 0 1" />
<geom name="RH_FOOT" class="foot" pos="0.01305 -0.08795 -0.31547" quat="1 0 0 1" />
</body>
</body>
</body>
</body>
<!-- Current heading arrow (green) - use freejoint to control position, no physical effect -->
<body name="robot_heading_arrow" pos="0 0 0.76" mocap="true">
<geom name="robot_arrow_head" type="mesh" mesh="arrow"
pos="0.25 0.0 0.08" quat="1 0 0 0"
rgba="0 1 0 0.8" contype="0" conaffinity="0" />
</body>
<!-- Desired heading arrow (blue) - use freejoint to control position, no physical effect -->
<body name="desired_heading_arrow" pos="0 0 0.76" mocap="true">
<geom name="desired_arrow_head" type="mesh" mesh="arrow"
pos="0.25 0.0 0.08" quat="1 0 0 0"
rgba="0 0 1 0.8" contype="0" conaffinity="0" />
</body>
</worldbody>
<contact>
<exclude body1="base" body2="LF_THIGH" />
<exclude body1="base" body2="RF_THIGH" />
<exclude body1="base" body2="LH_THIGH" />
<exclude body1="base" body2="RH_THIGH" />
</contact>
<sensor>
<framelinvel name="base_linvel" objtype="body" objname="base" />
<gyro name="base_gyro" site="imu_site" />
</sensor>
<actuator>
<position class="affine" joint="LF_HAA" name="LF_HAA" />
<position class="affine" joint="LF_HFE" name="LF_HFE" />
<position class="affine" joint="LF_KFE" name="LF_KFE" />
<position class="affine" joint="RF_HAA" name="RF_HAA" />
<position class="affine" joint="RF_HFE" name="RF_HFE" />
<position class="affine" joint="RF_KFE" name="RF_KFE" />
<position class="affine" joint="LH_HAA" name="LH_HAA" />
<position class="affine" joint="LH_HFE" name="LH_HFE" />
<position class="affine" joint="LH_KFE" name="LH_KFE" />
<position class="affine" joint="RH_HAA" name="RH_HAA" />
<position class="affine" joint="RH_HFE" name="RH_HFE" />
<position class="affine" joint="RH_KFE" name="RH_KFE" />
</actuator>
</mujoco>

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/arrow.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base.png Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_0.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_1.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_2.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_3.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_4.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/base_5.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/battery.obj Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/battery.png Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/bottom_shell.obj Normal file
View File

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

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