have a can go model, but is not good
This commit is contained in:
parent
81a8acc681
commit
d6cc7bbb53
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
64
README.md
64
README.md
|
|
@ -1,22 +1,24 @@
|
||||||
# TwoFootBot_isaaclab
|
# TwoFootBot-isaaclab
|
||||||
|
|
||||||
An installable downstream Isaac Lab task package with a standard uv `src` layout.
|
An Isaac Lab 3.0 project containing the migrated TwoFootBot locomotion tasks and robot description.
|
||||||
|
|
||||||
No tasks are registered yet. Add task packages under `src/TwoFootBot_isaaclab/tasks` and register them with Gymnasium.
|
|
||||||
|
|
||||||
|
|
||||||
## Installation
|
## Installation
|
||||||
|
|
||||||
Install [uv](https://docs.astral.sh/uv/getting-started/installation/), then create the project environment. The default
|
Install [uv](https://docs.astral.sh/uv/getting-started/installation/), then create the project environment with the
|
||||||
environment uses the Newton backend and does not install Isaac Sim:
|
Isaac Sim and RSL-RL extras used for training:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv sync
|
uv sync --locked --extra isaacsim --extra rsl-rl
|
||||||
```
|
```
|
||||||
|
|
||||||
This project's `pyproject.toml` uses editable relative paths to an Isaac Lab source checkout. Update
|
This project's `pyproject.toml` uses editable relative paths to an Isaac Lab source checkout. Update
|
||||||
`[tool.uv.sources]` if either directory moves.
|
`[tool.uv.sources]` if either directory moves.
|
||||||
|
|
||||||
|
The first dependency resolution creates `uv.lock` and may inspect Git dependencies declared by other optional Isaac Lab
|
||||||
|
extras, even if those extras are not selected for installation. Once the lock file exists, `--locked` reuses it and
|
||||||
|
installs only the selected extras.
|
||||||
|
|
||||||
The project forwards every optional extra declared by the Isaac Lab package used to create it, including physics,
|
The project forwards every optional extra declared by the Isaac Lab package used to create it, including physics,
|
||||||
rendering, visualizer, RL, teleoperation, and development features. Inspect `pyproject.toml` for the complete list and
|
rendering, visualizer, RL, teleoperation, and development features. Inspect `pyproject.toml` for the complete list and
|
||||||
pass each required extra to `uv run`:
|
pass each required extra to `uv run`:
|
||||||
|
|
@ -38,16 +40,58 @@ environment.
|
||||||
|
|
||||||
## Run the tasks
|
## Run the tasks
|
||||||
|
|
||||||
After registering a task, list it with:
|
List the migrated tasks from the project root:
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv run isaaclab list_envs --show_presets
|
uv run isaaclab list_envs --show_presets
|
||||||
```
|
```
|
||||||
|
|
||||||
|
The MLP training tasks are `Template-Twofootbot-mlp-v0` and `Template-Twofootbot-mlp-v1`.
|
||||||
|
Use Isaac Lab's unified trainer:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
uv run --extra isaacsim --extra rsl-rl isaaclab train --rl_library rsl_rl \
|
||||||
|
--task Template-Twofootbot-mlp-v1 --headless
|
||||||
|
```
|
||||||
|
|
||||||
|
The robot's URDF and ROS package are in `bot/2foot`.
|
||||||
|
|
||||||
|
## Play all checkpoints in a run
|
||||||
|
|
||||||
|
From the project root, provide a training run directory to play each `.pt` checkpoint in step order. If the directory is
|
||||||
|
omitted, the script prompts for it:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
uv run python playall.py logs/rsl_rl/cartpole_direct/<RUN_DIRECTORY>
|
||||||
|
# or
|
||||||
|
uv run python playall.py
|
||||||
|
```
|
||||||
|
|
||||||
|
The task is read from the run directory's `run.json`; each checkpoint is played with 96 environments and a 500-step
|
||||||
|
video. Videos are saved in the run's `videos/play` directory using the checkpoint name. Existing videos are skipped,
|
||||||
|
so rerunning the script only plays checkpoints that do not yet have a video. The script reports an error if `run.json`
|
||||||
|
is missing, invalid, or does not contain a non-empty `task`.
|
||||||
|
|
||||||
|
## Resume an existing run
|
||||||
|
|
||||||
|
The original project logs are intentionally not copied. Resume directly from its checkpoint while running from this
|
||||||
|
project root:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
CUDA_VISIBLE_DEVICES=0 uv run --extra isaacsim --extra rsl-rl isaaclab train --rl_library rsl_rl \
|
||||||
|
--task Template-Twofootbot-mlp-v1 \
|
||||||
|
--max_iterations 100000 \
|
||||||
|
--headless \
|
||||||
|
--checkpoint ../TwoFootBot/logs/rsl_rl/cartpole_direct/2026-10-27_14-26-38/model_28000.pt
|
||||||
|
```
|
||||||
|
|
||||||
|
Isaac Lab 3.0 takes the checkpoint path directly; the old `--resume` and `--load_run` options are no longer used.
|
||||||
|
|
||||||
## Project structure
|
## Project structure
|
||||||
|
|
||||||
Add task packages under `src/TwoFootBot_isaaclab/tasks`. Import each task package from that directory so its Gymnasium
|
Migrated task configurations and Gymnasium registrations are under
|
||||||
registrations load through the project's `isaaclab.tasks` entry point.
|
`src/TwoFootBot_isaaclab/tasks/manager_based/twofootbot`. Importing the project's `isaaclab.tasks` entry point registers
|
||||||
|
the preserved `Template-Twofootbot-mlp-v0` and `Template-Twofootbot-mlp-v1` task IDs.
|
||||||
|
|
||||||
## Project assets
|
## Project assets
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,14 @@
|
||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
|
||||||
|
project(2foot)
|
||||||
|
|
||||||
|
find_package(catkin REQUIRED)
|
||||||
|
|
||||||
|
catkin_package()
|
||||||
|
|
||||||
|
find_package(roslaunch)
|
||||||
|
|
||||||
|
foreach(dir config launch meshes urdf)
|
||||||
|
install(DIRECTORY ${dir}/
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
|
||||||
|
endforeach(dir)
|
||||||
|
|
@ -0,0 +1 @@
|
||||||
|
controller_joint_names: ['', 'jlf0_link', 'jlf1_link', 'jlf2_link', 'jlf3_link', 'jrf0_link', 'jrf1_link', 'jrf2_link', 'jrf3_link', ]
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
<launch>
|
||||||
|
<arg
|
||||||
|
name="model" />
|
||||||
|
<param
|
||||||
|
name="robot_description"
|
||||||
|
textfile="$(find 2foot)/urdf/2foot.urdf" />
|
||||||
|
<node
|
||||||
|
name="joint_state_publisher_gui"
|
||||||
|
pkg="joint_state_publisher_gui"
|
||||||
|
type="joint_state_publisher_gui" />
|
||||||
|
<node
|
||||||
|
name="robot_state_publisher"
|
||||||
|
pkg="robot_state_publisher"
|
||||||
|
type="robot_state_publisher" />
|
||||||
|
<node
|
||||||
|
name="rviz"
|
||||||
|
pkg="rviz"
|
||||||
|
type="rviz"
|
||||||
|
args="-d $(find 2foot)/urdf.rviz" />
|
||||||
|
</launch>
|
||||||
|
|
@ -0,0 +1,20 @@
|
||||||
|
<launch>
|
||||||
|
<include
|
||||||
|
file="$(find gazebo_ros)/launch/empty_world.launch" />
|
||||||
|
<node
|
||||||
|
name="tf_footprint_base"
|
||||||
|
pkg="tf"
|
||||||
|
type="static_transform_publisher"
|
||||||
|
args="0 0 0 0 0 0 base_link base_footprint 40" />
|
||||||
|
<node
|
||||||
|
name="spawn_model"
|
||||||
|
pkg="gazebo_ros"
|
||||||
|
type="spawn_model"
|
||||||
|
args="-file $(find 2foot)/urdf/2foot.urdf -urdf -model 2foot"
|
||||||
|
output="screen" />
|
||||||
|
<node
|
||||||
|
name="fake_joint_calibration"
|
||||||
|
pkg="rostopic"
|
||||||
|
type="rostopic"
|
||||||
|
args="pub /calibrated std_msgs/Bool true" />
|
||||||
|
</launch>
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,21 @@
|
||||||
|
<package format="2">
|
||||||
|
<name>2foot</name>
|
||||||
|
<version>1.0.0</version>
|
||||||
|
<description>
|
||||||
|
<p>URDF Description package for 2foot</p>
|
||||||
|
<p>This package contains configuration data, 3D models and launch files
|
||||||
|
for 2foot robot</p>
|
||||||
|
</description>
|
||||||
|
<author>TODO</author>
|
||||||
|
<maintainer email="TODO@email.com" />
|
||||||
|
<license>BSD</license>
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
<depend>roslaunch</depend>
|
||||||
|
<depend>robot_state_publisher</depend>
|
||||||
|
<depend>rviz</depend>
|
||||||
|
<depend>joint_state_publisher_gui</depend>
|
||||||
|
<depend>gazebo</depend>
|
||||||
|
<export>
|
||||||
|
<architecture_independent />
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||||
|
head_link,0.00637927718522201,8.36838553548436E-05,0.0502679117290854,0,0,0,0.104147639434427,0.000119888626287878,4.26649795023379E-08,-3.14056079328627E-06,9.2091610344693E-05,1.01375294153974E-07,3.18810914232645E-05,0,0,0,0,0,0,package://2foot/meshes/head_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/head_link.STL,,主体-2;bateria 9v-1;Step Up-1;3D_PCB1_2026-08-10.step-1/Board~RkW7.step-1;Freenove ESP32-S3-WROOM-CAM-1;3D_PCB1_2026-08-10.step-1/H15~HDR-TH_20P-P2.54-V-F~HDR-F-2.54_1x20~VMZC.step-1/HDR-TH_20P-P2.54-V-F~HDR-F-2.54_1x20~Z7qR.step-1;3D_PCB1_2026-08-10.step-1/H1~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~UsHF.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;3D_PCB1_2026-08-10.step-1/H8~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~Shzr.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;3D_PCB1_2026-08-10.step-1/H2~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~Ckmi.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;3D_PCB1_2026-08-10.step-1/H4~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~7m2w.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;3D_PCB1_2026-08-10.step-1/H6~HDR-TH_20P-P2.54-V-F~HDR-F-2.54_1x20~FFBa.step-1/HDR-TH_20P-P2.54-V-F~HDR-F-2.54_1x20~Z7qR.step-1;3D_PCB1_2026-08-10.step-1/H3~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~2T1y.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;3D_PCB1_2026-08-10.step-1/H5~HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~T7xH.step-1/HDR-TH_20P-P2.54-V-M-1~HDR-M-2.54_1x20~m73r.step-1;舵机总体-5/Servo-1;舵机总体-4/Servo-1,head,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||||
|
lf0_link,2.5034674019167E-06,-0.00399152370097755,-0.0136055723537577,0,0,0,0.0155246301341258,1.99164881392775E-06,-4.82887905395487E-10,-2.07253296776602E-10,2.02900387840545E-06,2.49004321417992E-07,1.54078395860252E-06,0,0,0,0,0,0,package://2foot/meshes/lf0_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/lf0_link.STL,,右腿胯骨-18;舵机总体-8/Servo-1,lf0,lf0_hige,jlf0_link,revolute,-0.0031441,0.025688,-0.006475,0,0,0,head_link,0,0,-1,0,0,0,0,,,,,,,,
|
||||||
|
lf1_link,2.91696425805399E-06,0.004390376864982,-0.0296712953157869,0,0,0,0.013323922418148,2.72143018103993E-06,4.82887905395412E-10,2.07253296776833E-10,2.92090153101514E-06,-3.25175555348237E-08,9.23918900070304E-07,0,0,0,0,0,0,package://2foot/meshes/lf1_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/lf1_link.STL,,右大腿-3;舵机总体-10/Servo-1,lf1,lf1_hige,jlf1_link,revolute,9.7027E-05,0.011975,-0.012388,0,0,0,lf0_link,0,-1,0,0,0,0,0,,,,,,,,
|
||||||
|
lf2_link,2.43737372657149E-06,0.00363613025354126,-0.0425732788173739,0,0,0,0.0159456077850123,5.35424510746978E-06,4.82887905395677E-10,2.07253296776739E-10,5.71315402728463E-06,-2.13454759600996E-08,1.09035404123551E-06,0,0,0,0,0,0,package://2foot/meshes/lf2_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/lf2_link.STL,,右小腿-2;舵机总体-12/Servo-1,lf2,lf2_hige,jlf2_link,revolute,9.7027E-05,-0.008475,-0.040938,0,0,0,lf1_link,0,1,0,0,0,0,0,,,,,,,,
|
||||||
|
lf3_link,-4.82253126321552E-16,-0.000451394188820231,-0.0198245674059442,0,0,0,0.0192906631601767,3.18209633375994E-06,3.34527248954873E-21,1.76056444695768E-21,6.55022061625284E-06,2.53604908486043E-07,7.24649308736933E-06,0,0,0,0,0,0,package://2foot/meshes/lf3_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/lf3_link.STL,,玉足-4,lf3,lf3_hige,jlf3_link,revolute,9.7027E-05,-0.006475,-0.060937,0,0,0,lf2_link,0,-1,0,0,0,0,0,,,,,,,,
|
||||||
|
rf0_link,-2.50346740203813E-06,0.00399152370097733,-0.0136055723537577,0,0,0,0.0155246301341258,1.99164881392775E-06,-4.82887905395413E-10,2.07253296776268E-10,2.02900387840545E-06,-2.49004321417992E-07,1.54078395860252E-06,0,0,0,0,0,0,package://2foot/meshes/rf0_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/rf0_link.STL,,右腿胯骨-16;舵机总体-9/Servo-1,rf0,rf0_hige,jrf0_link,revolute,-0.0033382,-0.025687,-0.006475,0,0,0,head_link,0,0,-1,0,0,0,0,,,,,,,,
|
||||||
|
rf1_link,-2.91696425855359E-06,-0.00439037686498245,-0.0296712953157865,0,0,0,0.0133239224181479,2.72143018103992E-06,4.82887905395081E-10,-2.07253296776895E-10,2.92090153101514E-06,3.25175555348265E-08,9.23918900070297E-07,0,0,0,0,0,0,package://2foot/meshes/rf1_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/rf1_link.STL,,右大腿-2;舵机总体-11/Servo-1,rf1,rf1_hige,jrf1_link,revolute,-9.7027E-05,-0.011975,-0.012388,0,0,0,rf0_link,0,1,0,0,0,0,0,,,,,,,,
|
||||||
|
rf2_link,-2.43737372681782E-06,-0.00363613025354104,-0.0425732788173739,0,0,0,0.0159456077850122,5.35424510746977E-06,4.82887905394926E-10,-2.0725329677683E-10,5.71315402728462E-06,2.13454759600991E-08,1.0903540412355E-06,0,0,0,0,0,0,package://2foot/meshes/rf2_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/rf2_link.STL,,右小腿-1;舵机总体-13/Servo-1,rf2,lf2_hige,jrf2_link,revolute,-9.7027E-05,0.008475,-0.040937,0,0,0,rf1_link,0,1,0,0,0,0,0,,,,,,,,
|
||||||
|
rf3_link,8.18789480661053E-16,0.000451394188820897,-0.0198245674059443,0,0,0,0.0192906631601768,3.18209633375994E-06,8.58350838304875E-21,2.39867798130625E-21,6.55022061625285E-06,-2.53604908486041E-07,7.24649308736931E-06,0,0,0,0,0,0,package://2foot/meshes/rf3_link.STL,1,1,1,1,0,0,0,0,0,0,package://2foot/meshes/rf3_link.STL,,玉足-3,rf3,rf3_hige,jrf3_link,revolute,-9.7027E-05,0.006475,-0.060938,0,0,0,rf2_link,0,1,0,0,0,0,0,,,,,,,,
|
||||||
|
|
|
@ -0,0 +1,565 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||||
|
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
|
<robot
|
||||||
|
name="2foot">
|
||||||
|
<link
|
||||||
|
name="head_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.00637927718522201 8.36838553548436E-05 0.0502679117290854"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.104147639434427" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000119888626287878"
|
||||||
|
ixy="4.26649795023379E-08"
|
||||||
|
ixz="-3.14056079328627E-06"
|
||||||
|
iyy="9.2091610344693E-05"
|
||||||
|
iyz="1.01375294153974E-07"
|
||||||
|
izz="3.18810914232645E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/head_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/head_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<link
|
||||||
|
name="lf0_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.5034674019167E-06 -0.00399152370097755 -0.0136055723537577"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0155246301341258" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.99164881392775E-06"
|
||||||
|
ixy="-4.82887905395487E-10"
|
||||||
|
ixz="-2.07253296776602E-10"
|
||||||
|
iyy="2.02900387840545E-06"
|
||||||
|
iyz="2.49004321417992E-07"
|
||||||
|
izz="1.54078395860252E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf0_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-0.0031441 0.025688 -0.006475"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="head_link" />
|
||||||
|
<child
|
||||||
|
link="lf0_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 -1" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf1_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.91696425805399E-06 0.004390376864982 -0.0296712953157869"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.013323922418148" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.72143018103993E-06"
|
||||||
|
ixy="4.82887905395412E-10"
|
||||||
|
ixz="2.07253296776833E-10"
|
||||||
|
iyy="2.92090153101514E-06"
|
||||||
|
iyz="-3.25175555348237E-08"
|
||||||
|
izz="9.23918900070304E-07" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf1_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 0.011975 -0.012388"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf0_link" />
|
||||||
|
<child
|
||||||
|
link="lf1_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf2_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.43737372657149E-06 0.00363613025354126 -0.0425732788173739"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0159456077850123" />
|
||||||
|
<inertia
|
||||||
|
ixx="5.35424510746978E-06"
|
||||||
|
ixy="4.82887905395677E-10"
|
||||||
|
ixz="2.07253296776739E-10"
|
||||||
|
iyy="5.71315402728463E-06"
|
||||||
|
iyz="-2.13454759600996E-08"
|
||||||
|
izz="1.09035404123551E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf2_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 -0.008475 -0.040938"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf1_link" />
|
||||||
|
<child
|
||||||
|
link="lf2_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf3_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-4.82253126321552E-16 -0.000451394188820231 -0.0198245674059442"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0192906631601767" />
|
||||||
|
<inertia
|
||||||
|
ixx="3.18209633375994E-06"
|
||||||
|
ixy="3.34527248954873E-21"
|
||||||
|
ixz="1.76056444695768E-21"
|
||||||
|
iyy="6.55022061625284E-06"
|
||||||
|
iyz="2.53604908486043E-07"
|
||||||
|
izz="7.24649308736933E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf3_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 -0.006475 -0.060937"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf2_link" />
|
||||||
|
<child
|
||||||
|
link="lf3_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf0_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.50346740203813E-06 0.00399152370097733 -0.0136055723537577"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0155246301341258" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.99164881392775E-06"
|
||||||
|
ixy="-4.82887905395413E-10"
|
||||||
|
ixz="2.07253296776268E-10"
|
||||||
|
iyy="2.02900387840545E-06"
|
||||||
|
iyz="-2.49004321417992E-07"
|
||||||
|
izz="1.54078395860252E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf0_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-0.0033382 -0.025687 -0.006475"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="head_link" />
|
||||||
|
<child
|
||||||
|
link="rf0_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 -1" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf1_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.91696425855359E-06 -0.00439037686498245 -0.0296712953157865"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0133239224181479" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.72143018103992E-06"
|
||||||
|
ixy="4.82887905395081E-10"
|
||||||
|
ixz="-2.07253296776895E-10"
|
||||||
|
iyy="2.92090153101514E-06"
|
||||||
|
iyz="3.25175555348265E-08"
|
||||||
|
izz="9.23918900070297E-07" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf1_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 -0.011975 -0.012388"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf0_link" />
|
||||||
|
<child
|
||||||
|
link="rf1_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf2_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.43737372681782E-06 -0.00363613025354104 -0.0425732788173739"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0159456077850122" />
|
||||||
|
<inertia
|
||||||
|
ixx="5.35424510746977E-06"
|
||||||
|
ixy="4.82887905394926E-10"
|
||||||
|
ixz="-2.0725329677683E-10"
|
||||||
|
iyy="5.71315402728462E-06"
|
||||||
|
iyz="2.13454759600991E-08"
|
||||||
|
izz="1.0903540412355E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf2_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 0.008475 -0.040937"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf1_link" />
|
||||||
|
<child
|
||||||
|
link="rf2_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf3_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="8.18789480661053E-16 0.000451394188820897 -0.0198245674059443"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0192906631601768" />
|
||||||
|
<inertia
|
||||||
|
ixx="3.18209633375994E-06"
|
||||||
|
ixy="8.58350838304875E-21"
|
||||||
|
ixz="2.39867798130625E-21"
|
||||||
|
iyy="6.55022061625285E-06"
|
||||||
|
iyz="-2.53604908486041E-07"
|
||||||
|
izz="7.24649308736931E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf3_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 0.006475 -0.060938"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf2_link" />
|
||||||
|
<child
|
||||||
|
link="rf3_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="-1.0"
|
||||||
|
upper="1.0"
|
||||||
|
effort="10.0"
|
||||||
|
velocity="5.0" />
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
|
|
@ -0,0 +1,565 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||||
|
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578
|
||||||
|
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||||
|
<robot
|
||||||
|
name="2foot">
|
||||||
|
<link
|
||||||
|
name="head_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="0.00637927718522201 8.36838553548436E-05 0.0502679117290854"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.104147639434427" />
|
||||||
|
<inertia
|
||||||
|
ixx="0.000119888626287878"
|
||||||
|
ixy="4.26649795023379E-08"
|
||||||
|
ixz="-3.14056079328627E-06"
|
||||||
|
iyy="9.2091610344693E-05"
|
||||||
|
iyz="1.01375294153974E-07"
|
||||||
|
izz="3.18810914232645E-05" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/head_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/head_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<link
|
||||||
|
name="lf0_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.5034674019167E-06 -0.00399152370097755 -0.0136055723537577"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0155246301341258" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.99164881392775E-06"
|
||||||
|
ixy="-4.82887905395487E-10"
|
||||||
|
ixz="-2.07253296776602E-10"
|
||||||
|
iyy="2.02900387840545E-06"
|
||||||
|
iyz="2.49004321417992E-07"
|
||||||
|
izz="1.54078395860252E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf0_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-0.0031441 0.025688 -0.006475"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="head_link" />
|
||||||
|
<child
|
||||||
|
link="lf0_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 -1" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf1_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.91696425805399E-06 0.004390376864982 -0.0296712953157869"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.013323922418148" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.72143018103993E-06"
|
||||||
|
ixy="4.82887905395412E-10"
|
||||||
|
ixz="2.07253296776833E-10"
|
||||||
|
iyy="2.92090153101514E-06"
|
||||||
|
iyz="-3.25175555348237E-08"
|
||||||
|
izz="9.23918900070304E-07" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf1_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 0.011975 -0.012388"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf0_link" />
|
||||||
|
<child
|
||||||
|
link="lf1_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf2_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="2.43737372657149E-06 0.00363613025354126 -0.0425732788173739"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0159456077850123" />
|
||||||
|
<inertia
|
||||||
|
ixx="5.35424510746978E-06"
|
||||||
|
ixy="4.82887905395677E-10"
|
||||||
|
ixz="2.07253296776739E-10"
|
||||||
|
iyy="5.71315402728463E-06"
|
||||||
|
iyz="-2.13454759600996E-08"
|
||||||
|
izz="1.09035404123551E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf2_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 -0.008475 -0.040938"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf1_link" />
|
||||||
|
<child
|
||||||
|
link="lf2_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="lf3_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-4.82253126321552E-16 -0.000451394188820231 -0.0198245674059442"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0192906631601767" />
|
||||||
|
<inertia
|
||||||
|
ixx="3.18209633375994E-06"
|
||||||
|
ixy="3.34527248954873E-21"
|
||||||
|
ixz="1.76056444695768E-21"
|
||||||
|
iyy="6.55022061625284E-06"
|
||||||
|
iyz="2.53604908486043E-07"
|
||||||
|
izz="7.24649308736933E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/lf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jlf3_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="9.7027E-05 -0.006475 -0.060937"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="lf2_link" />
|
||||||
|
<child
|
||||||
|
link="lf3_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 -1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf0_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.50346740203813E-06 0.00399152370097733 -0.0136055723537577"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0155246301341258" />
|
||||||
|
<inertia
|
||||||
|
ixx="1.99164881392775E-06"
|
||||||
|
ixy="-4.82887905395413E-10"
|
||||||
|
ixz="2.07253296776268E-10"
|
||||||
|
iyy="2.02900387840545E-06"
|
||||||
|
iyz="-2.49004321417992E-07"
|
||||||
|
izz="1.54078395860252E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf0_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf0_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-0.0033382 -0.025687 -0.006475"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="head_link" />
|
||||||
|
<child
|
||||||
|
link="rf0_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 0 -1" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf1_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.91696425855359E-06 -0.00439037686498245 -0.0296712953157865"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0133239224181479" />
|
||||||
|
<inertia
|
||||||
|
ixx="2.72143018103992E-06"
|
||||||
|
ixy="4.82887905395081E-10"
|
||||||
|
ixz="-2.07253296776895E-10"
|
||||||
|
iyy="2.92090153101514E-06"
|
||||||
|
iyz="3.25175555348265E-08"
|
||||||
|
izz="9.23918900070297E-07" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf1_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf1_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 -0.011975 -0.012388"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf0_link" />
|
||||||
|
<child
|
||||||
|
link="rf1_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf2_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="-2.43737372681782E-06 -0.00363613025354104 -0.0425732788173739"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0159456077850122" />
|
||||||
|
<inertia
|
||||||
|
ixx="5.35424510746977E-06"
|
||||||
|
ixy="4.82887905394926E-10"
|
||||||
|
ixz="-2.0725329677683E-10"
|
||||||
|
iyy="5.71315402728462E-06"
|
||||||
|
iyz="2.13454759600991E-08"
|
||||||
|
izz="1.0903540412355E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf2_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf2_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 0.008475 -0.040937"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf1_link" />
|
||||||
|
<child
|
||||||
|
link="rf2_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
<link
|
||||||
|
name="rf3_link">
|
||||||
|
<inertial>
|
||||||
|
<origin
|
||||||
|
xyz="8.18789480661053E-16 0.000451394188820897 -0.0198245674059443"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<mass
|
||||||
|
value="0.0192906631601768" />
|
||||||
|
<inertia
|
||||||
|
ixx="3.18209633375994E-06"
|
||||||
|
ixy="8.58350838304875E-21"
|
||||||
|
ixz="2.39867798130625E-21"
|
||||||
|
iyy="6.55022061625285E-06"
|
||||||
|
iyz="-2.53604908486041E-07"
|
||||||
|
izz="7.24649308736931E-06" />
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
<material
|
||||||
|
name="">
|
||||||
|
<color
|
||||||
|
rgba="1 1 1 1" />
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin
|
||||||
|
xyz="0 0 0"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<mesh
|
||||||
|
filename="package://2foot/meshes/rf3_link.STL" />
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
<contact>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||||
|
</contact>
|
||||||
|
</link>
|
||||||
|
<joint
|
||||||
|
name="jrf3_link"
|
||||||
|
type="revolute">
|
||||||
|
<origin
|
||||||
|
xyz="-9.7027E-05 0.006475 -0.060938"
|
||||||
|
rpy="0 0 0" />
|
||||||
|
<parent
|
||||||
|
link="rf2_link" />
|
||||||
|
<child
|
||||||
|
link="rf3_link" />
|
||||||
|
<axis
|
||||||
|
xyz="0 1 0" />
|
||||||
|
<limit
|
||||||
|
lower="0"
|
||||||
|
upper="0"
|
||||||
|
effort="0"
|
||||||
|
velocity="0" />
|
||||||
|
</joint>
|
||||||
|
</robot>
|
||||||
|
|
@ -0,0 +1,29 @@
|
||||||
|
asset_path: bot/2foot/urdf/2foot.urdf
|
||||||
|
usd_dir: bot/
|
||||||
|
usd_file_name: 2foot/2foot.usda
|
||||||
|
force_usd_conversion: true
|
||||||
|
make_instanceable: true
|
||||||
|
fix_base: false
|
||||||
|
root_link_name: null
|
||||||
|
link_density: 0.0
|
||||||
|
merge_fixed_joints: false
|
||||||
|
convert_mimic_joints_to_normal_joints: false
|
||||||
|
joint_drive:
|
||||||
|
drive_type: force
|
||||||
|
target_type: position
|
||||||
|
gains:
|
||||||
|
stiffness: 100.0
|
||||||
|
damping: 1.0
|
||||||
|
collision_from_visuals: false
|
||||||
|
collision_type: Convex Hull
|
||||||
|
self_collision: false
|
||||||
|
replace_cylinders_with_capsules: false
|
||||||
|
merge_mesh: false
|
||||||
|
ros_package_paths: []
|
||||||
|
robot_type: Default
|
||||||
|
run_asset_transformer: true
|
||||||
|
run_multi_physics_conversion: true
|
||||||
|
debug_mode: false
|
||||||
|
##
|
||||||
|
# Generated by UrdfConverter on 2026-09-08 at 18:58:47.
|
||||||
|
##
|
||||||
|
|
@ -0,0 +1,131 @@
|
||||||
|
"""Play every RSL-RL checkpoint in a training run directory."""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import json
|
||||||
|
import os
|
||||||
|
import re
|
||||||
|
import subprocess
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
PROJECT_ROOT = Path(__file__).resolve().parent
|
||||||
|
|
||||||
|
|
||||||
|
def checkpoint_sort_key(checkpoint: Path) -> tuple[int, int | str]:
|
||||||
|
match = re.fullmatch(r"model_(\d+)", checkpoint.stem)
|
||||||
|
if match:
|
||||||
|
return (0, int(match.group(1)))
|
||||||
|
return (1, checkpoint.name)
|
||||||
|
|
||||||
|
|
||||||
|
def play_checkpoint(checkpoint: Path, video_folder: Path, task_name: str) -> None:
|
||||||
|
video_folder.mkdir(parents=True, exist_ok=True)
|
||||||
|
expected_video = video_folder / f"{checkpoint.stem}.mp4"
|
||||||
|
if expected_video.exists():
|
||||||
|
if expected_video.is_file():
|
||||||
|
print(f"Skipping {checkpoint.name}; video already exists: {expected_video}")
|
||||||
|
return
|
||||||
|
raise FileExistsError(f"Expected video path exists but is not a file: {expected_video}")
|
||||||
|
|
||||||
|
existing_videos = {
|
||||||
|
path: (path.stat().st_mtime_ns, path.stat().st_size)
|
||||||
|
for path in video_folder.glob("*.mp4")
|
||||||
|
}
|
||||||
|
|
||||||
|
command = [
|
||||||
|
"uv",
|
||||||
|
"run",
|
||||||
|
"--extra",
|
||||||
|
"video",
|
||||||
|
"isaaclab",
|
||||||
|
"play",
|
||||||
|
"--rl_library",
|
||||||
|
"rsl_rl",
|
||||||
|
"--task",
|
||||||
|
task_name,
|
||||||
|
"--num_envs",
|
||||||
|
"96",
|
||||||
|
"--checkpoint",
|
||||||
|
str(checkpoint),
|
||||||
|
"--video",
|
||||||
|
"--video_length",
|
||||||
|
"500",
|
||||||
|
]
|
||||||
|
environment = os.environ.copy()
|
||||||
|
environment["CUDA_VISIBLE_DEVICES"] = "1"
|
||||||
|
|
||||||
|
print(f"\n=== Playing {checkpoint.name} to {task_name} ===", flush=True)
|
||||||
|
subprocess.run(command, cwd=PROJECT_ROOT, env=environment, check=True)
|
||||||
|
|
||||||
|
recorded_videos = sorted(
|
||||||
|
path
|
||||||
|
for path in video_folder.glob("*.mp4")
|
||||||
|
if path not in existing_videos
|
||||||
|
or (path.stat().st_mtime_ns, path.stat().st_size) != existing_videos[path]
|
||||||
|
)
|
||||||
|
if not recorded_videos:
|
||||||
|
raise RuntimeError(f"No video was recorded for {checkpoint}.")
|
||||||
|
|
||||||
|
for video in recorded_videos:
|
||||||
|
suffix = f"-{video.stem}" if len(recorded_videos) > 1 else ""
|
||||||
|
destination = video_folder / f"{checkpoint.stem}{suffix}.mp4"
|
||||||
|
if destination.exists():
|
||||||
|
raise FileExistsError(f"Refusing to overwrite existing video: {destination}")
|
||||||
|
video.rename(destination)
|
||||||
|
print(f"Saved video: {destination}")
|
||||||
|
|
||||||
|
|
||||||
|
def main() -> None:
|
||||||
|
parser = argparse.ArgumentParser(
|
||||||
|
description="Play all .pt checkpoints directly inside a training log directory."
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"log_dir",
|
||||||
|
nargs="?",
|
||||||
|
help="Training run directory containing checkpoint .pt files.",
|
||||||
|
)
|
||||||
|
args = parser.parse_args()
|
||||||
|
|
||||||
|
log_dir_input = args.log_dir or input("Training log directory: ").strip()
|
||||||
|
log_dir = Path(log_dir_input).expanduser().resolve()
|
||||||
|
if not log_dir.is_dir():
|
||||||
|
parser.error(f"Training log directory does not exist: {log_dir}")
|
||||||
|
|
||||||
|
run_file = log_dir / "run.json"
|
||||||
|
try:
|
||||||
|
with run_file.open(encoding="utf-8") as file:
|
||||||
|
run_metadata = json.load(file)
|
||||||
|
except FileNotFoundError:
|
||||||
|
parser.error(f"Training metadata file not found: {run_file}")
|
||||||
|
except json.JSONDecodeError as error:
|
||||||
|
parser.error(f"Invalid JSON in {run_file}: {error}")
|
||||||
|
except OSError as error:
|
||||||
|
parser.error(f"Could not read {run_file}: {error}")
|
||||||
|
|
||||||
|
if not isinstance(run_metadata, dict):
|
||||||
|
parser.error(f"Expected a JSON object in {run_file}")
|
||||||
|
task_name = run_metadata.get("task")
|
||||||
|
if not isinstance(task_name, str) or not task_name.strip():
|
||||||
|
parser.error(f"Expected a non-empty 'task' string in {run_file}")
|
||||||
|
task_name = task_name.strip()
|
||||||
|
|
||||||
|
checkpoints = sorted(log_dir.glob("*.pt"), key=checkpoint_sort_key)
|
||||||
|
if not checkpoints:
|
||||||
|
parser.error(f"No .pt checkpoint files found directly inside: {log_dir}")
|
||||||
|
|
||||||
|
video_folder = log_dir / "videos" / "play"
|
||||||
|
pending_checkpoints = [
|
||||||
|
checkpoint
|
||||||
|
for checkpoint in checkpoints
|
||||||
|
if not (video_folder / f"{checkpoint.stem}.mp4").is_file()
|
||||||
|
]
|
||||||
|
skipped_count = len(checkpoints) - len(pending_checkpoints)
|
||||||
|
print(
|
||||||
|
f"Found {len(checkpoints)} checkpoint(s) for task {task_name} in {log_dir}; "
|
||||||
|
f"{skipped_count} already have videos, {len(pending_checkpoints)} to play."
|
||||||
|
)
|
||||||
|
for checkpoint in pending_checkpoints:
|
||||||
|
play_checkpoint(checkpoint, video_folder, task_name)
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
@ -0,0 +1,86 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
"""Script to an environment with random action agent."""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import contextlib
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import gymnasium as gym
|
||||||
|
import torch
|
||||||
|
|
||||||
|
import isaaclab_tasks # noqa: F401
|
||||||
|
|
||||||
|
with contextlib.suppress(ImportError):
|
||||||
|
import isaaclab_tasks_experimental # noqa: F401
|
||||||
|
from isaaclab_tasks.utils import (
|
||||||
|
add_launcher_args,
|
||||||
|
launch_simulation,
|
||||||
|
resolve_task_config,
|
||||||
|
setup_preset_cli,
|
||||||
|
)
|
||||||
|
|
||||||
|
# add argparse arguments
|
||||||
|
parser = argparse.ArgumentParser(description="Random agent for Isaac Lab environments.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations."
|
||||||
|
)
|
||||||
|
parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.")
|
||||||
|
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
|
||||||
|
# append AppLauncher cli args
|
||||||
|
add_launcher_args(parser)
|
||||||
|
# simple agents should open Kit visualizer by default
|
||||||
|
parser.set_defaults(visualizer=["kit"])
|
||||||
|
args_cli, hydra_args = setup_preset_cli(parser)
|
||||||
|
sys.argv = [sys.argv[0]] + hydra_args
|
||||||
|
|
||||||
|
import TwoFootBot_isaaclab.tasks # noqa: F401
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
"""Random actions agent with Isaac Lab environment."""
|
||||||
|
|
||||||
|
torch.manual_seed(42)
|
||||||
|
|
||||||
|
# parse configuration via Hydra (supports preset selection, e.g. env.sim.physics=newton_mjwarp)
|
||||||
|
env_cfg, _ = resolve_task_config(args_cli.task, "")
|
||||||
|
|
||||||
|
with launch_simulation(env_cfg, args_cli):
|
||||||
|
# override with CLI arguments
|
||||||
|
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
|
||||||
|
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device
|
||||||
|
if args_cli.disable_fabric:
|
||||||
|
env_cfg.sim.use_fabric = False
|
||||||
|
|
||||||
|
# create environment
|
||||||
|
env = gym.make(args_cli.task, cfg=env_cfg)
|
||||||
|
|
||||||
|
# print info (this is vectorized environment)
|
||||||
|
print(f"[INFO]: Gym observation space: {env.observation_space}")
|
||||||
|
print(f"[INFO]: Gym action space: {env.action_space}")
|
||||||
|
# reset environment
|
||||||
|
env.reset()
|
||||||
|
# simulate environment
|
||||||
|
sim = env.unwrapped.sim
|
||||||
|
while True:
|
||||||
|
if sim.visualizers:
|
||||||
|
# visualizer mode: run until the visualizer window is closed
|
||||||
|
if not any(v.is_running() and not v.is_closed for v in sim.visualizers):
|
||||||
|
break
|
||||||
|
# run everything in inference mode
|
||||||
|
with torch.inference_mode():
|
||||||
|
# sample actions from -1 to 1
|
||||||
|
actions = 2 * torch.rand(env.action_space.shape, device=env.unwrapped.device) - 1
|
||||||
|
# apply actions
|
||||||
|
env.step(actions)
|
||||||
|
|
||||||
|
# close the simulator
|
||||||
|
env.close()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
# run the main function
|
||||||
|
main()
|
||||||
|
|
@ -0,0 +1,93 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import random
|
||||||
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
|
from isaaclab_rl.rsl_rl import RslRlBaseRunnerCfg
|
||||||
|
|
||||||
|
|
||||||
|
def add_rsl_rl_args(parser: argparse.ArgumentParser):
|
||||||
|
"""Add RSL-RL arguments to the parser.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
parser: The parser to add the arguments to.
|
||||||
|
"""
|
||||||
|
# create a new argument group
|
||||||
|
arg_group = parser.add_argument_group("rsl_rl", description="Arguments for RSL-RL agent.")
|
||||||
|
# -- experiment arguments
|
||||||
|
arg_group.add_argument(
|
||||||
|
"--experiment_name", type=str, default=None, help="Name of the experiment folder where logs will be stored."
|
||||||
|
)
|
||||||
|
arg_group.add_argument("--run_name", type=str, default=None, help="Run name suffix to the log directory.")
|
||||||
|
# -- load arguments
|
||||||
|
arg_group.add_argument("--resume", action="store_true", default=False, help="Whether to resume from a checkpoint.")
|
||||||
|
arg_group.add_argument("--load_run", type=str, default=None, help="Name of the run folder to resume from.")
|
||||||
|
arg_group.add_argument("--checkpoint", type=str, default=None, help="Checkpoint file to resume from.")
|
||||||
|
# -- logger arguments
|
||||||
|
arg_group.add_argument(
|
||||||
|
"--logger", type=str, default=None, choices={"wandb", "tensorboard", "neptune"}, help="Logger module to use."
|
||||||
|
)
|
||||||
|
arg_group.add_argument(
|
||||||
|
"--log_project_name", type=str, default=None, help="Name of the logging project when using wandb or neptune."
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def parse_rsl_rl_cfg(task_name: str, args_cli: argparse.Namespace) -> RslRlBaseRunnerCfg:
|
||||||
|
"""Parse configuration for RSL-RL agent based on inputs.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
task_name: The name of the environment.
|
||||||
|
args_cli: The command line arguments.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
The parsed configuration for RSL-RL agent based on inputs.
|
||||||
|
"""
|
||||||
|
from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry
|
||||||
|
|
||||||
|
# load the default configuration
|
||||||
|
rslrl_cfg: RslRlBaseRunnerCfg = load_cfg_from_registry(task_name, "rsl_rl_cfg_entry_point")
|
||||||
|
rslrl_cfg = update_rsl_rl_cfg(rslrl_cfg, args_cli)
|
||||||
|
return rslrl_cfg
|
||||||
|
|
||||||
|
|
||||||
|
def update_rsl_rl_cfg(agent_cfg: RslRlBaseRunnerCfg, args_cli: argparse.Namespace):
|
||||||
|
"""Update configuration for RSL-RL agent based on inputs.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
agent_cfg: The configuration for RSL-RL agent.
|
||||||
|
args_cli: The command line arguments.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
The updated configuration for RSL-RL agent based on inputs.
|
||||||
|
"""
|
||||||
|
# override the default configuration with CLI arguments
|
||||||
|
if hasattr(args_cli, "seed") and args_cli.seed is not None:
|
||||||
|
# randomly sample a seed if seed = -1
|
||||||
|
if args_cli.seed == -1:
|
||||||
|
args_cli.seed = random.randint(0, 10000)
|
||||||
|
agent_cfg.seed = args_cli.seed
|
||||||
|
if args_cli.resume is not None:
|
||||||
|
agent_cfg.resume = args_cli.resume
|
||||||
|
if args_cli.load_run is not None:
|
||||||
|
agent_cfg.load_run = args_cli.load_run
|
||||||
|
if args_cli.checkpoint is not None:
|
||||||
|
agent_cfg.load_checkpoint = args_cli.checkpoint
|
||||||
|
if args_cli.experiment_name is not None:
|
||||||
|
agent_cfg.experiment_name = args_cli.experiment_name
|
||||||
|
if args_cli.run_name is not None:
|
||||||
|
agent_cfg.run_name = args_cli.run_name
|
||||||
|
if args_cli.logger is not None:
|
||||||
|
agent_cfg.logger = args_cli.logger
|
||||||
|
# set the project name for wandb and neptune
|
||||||
|
if agent_cfg.logger in {"wandb", "neptune"} and args_cli.log_project_name:
|
||||||
|
agent_cfg.wandb_project = args_cli.log_project_name
|
||||||
|
agent_cfg.neptune_project = args_cli.log_project_name
|
||||||
|
|
||||||
|
return agent_cfg
|
||||||
|
|
@ -0,0 +1,273 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
"""Script to play a checkpoint if an RL agent from RSL-RL."""
|
||||||
|
|
||||||
|
import warnings
|
||||||
|
|
||||||
|
warnings.warn(
|
||||||
|
"scripts/reinforcement_learning/rsl_rl/play.py is deprecated. Use "
|
||||||
|
"`./isaaclab.sh play --rl_library rsl_rl --task <TASK>` instead. "
|
||||||
|
"Example: `./isaaclab.sh play --rl_library rsl_rl --task Isaac-Cartpole-v0`.",
|
||||||
|
DeprecationWarning,
|
||||||
|
stacklevel=1,
|
||||||
|
)
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import contextlib
|
||||||
|
import importlib.metadata as metadata
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
import gymnasium as gym
|
||||||
|
import torch
|
||||||
|
from packaging import version
|
||||||
|
from rsl_rl.runners import DistillationRunner, OnPolicyRunner
|
||||||
|
|
||||||
|
from isaaclab.envs import DirectMARLEnvCfg, DirectRLEnvCfg, ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.utils.assets import retrieve_file_path
|
||||||
|
from isaaclab.utils.dict import print_dict
|
||||||
|
from isaaclab.utils.seed import configure_seed
|
||||||
|
from isaaclab.utils.string import list_intersection, string_to_callable
|
||||||
|
|
||||||
|
from isaaclab_rl.rsl_rl import (
|
||||||
|
RslRlBaseRunnerCfg,
|
||||||
|
RslRlVecEnvWrapper,
|
||||||
|
export_policy_as_jit,
|
||||||
|
export_policy_as_onnx,
|
||||||
|
handle_deprecated_rsl_rl_cfg,
|
||||||
|
)
|
||||||
|
from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint
|
||||||
|
|
||||||
|
import isaaclab_tasks # noqa: F401
|
||||||
|
from isaaclab_tasks.utils import (
|
||||||
|
add_launcher_args,
|
||||||
|
get_checkpoint_path,
|
||||||
|
launch_simulation,
|
||||||
|
setup_preset_cli,
|
||||||
|
)
|
||||||
|
from isaaclab_tasks.utils.hydra import hydra_task_config
|
||||||
|
|
||||||
|
# local imports
|
||||||
|
import cli_args # isort: skip
|
||||||
|
|
||||||
|
import TwoFootBot_isaaclab.tasks # noqa: F401
|
||||||
|
with contextlib.suppress(ImportError):
|
||||||
|
import isaaclab_tasks_experimental # noqa: F401
|
||||||
|
|
||||||
|
# -- argparse ----------------------------------------------------------------
|
||||||
|
parser = argparse.ArgumentParser(description="Train an RL agent with RSL-RL.")
|
||||||
|
parser.add_argument("--video", action="store_true", default=True, help="Record videos during training.")
|
||||||
|
parser.add_argument("--video_length", type=int, default=500, help="Length of the recorded video (in steps).")
|
||||||
|
parser.add_argument(
|
||||||
|
"--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations."
|
||||||
|
)
|
||||||
|
parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.")
|
||||||
|
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--agent", type=str, default="rsl_rl_cfg_entry_point", help="Name of the RL agent configuration entry point."
|
||||||
|
)
|
||||||
|
parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment")
|
||||||
|
parser.add_argument(
|
||||||
|
"--use_pretrained_checkpoint",
|
||||||
|
action="store_true",
|
||||||
|
help="Use the pre-trained checkpoint from Nucleus.",
|
||||||
|
)
|
||||||
|
parser.add_argument("--real-time", action="store_true", default=False, help="Run in real-time, if possible.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--print_actions",
|
||||||
|
action="store_true",
|
||||||
|
default=False,
|
||||||
|
help="Print the raw MLP action output before it is clipped and sent to the environment.",
|
||||||
|
)
|
||||||
|
parser.add_argument("--external_callback", default=None, help="Fully qualified path to an externally defined callback.")
|
||||||
|
cli_args.add_rsl_rl_args(parser)
|
||||||
|
add_launcher_args(parser)
|
||||||
|
args_cli, remaining_args = setup_preset_cli(parser)
|
||||||
|
|
||||||
|
if args_cli.video:
|
||||||
|
args_cli.enable_cameras = True
|
||||||
|
|
||||||
|
|
||||||
|
# Call an external callback if requested. This gives opportunity to external code to register the environments
|
||||||
|
# The function is expected to return a list of arguments that were not consumed by the callback.
|
||||||
|
remaining_args_env_registration = None
|
||||||
|
if args_cli.external_callback:
|
||||||
|
external_callback_function = string_to_callable(args_cli.external_callback, separator=".")
|
||||||
|
remaining_args_env_registration = external_callback_function()
|
||||||
|
|
||||||
|
# clear out sys.argv for Hydra
|
||||||
|
# The remaining arguments are the arguments that were not consumed by both this scripts
|
||||||
|
# argparser and (optionally) the external callback function. Both sides of this
|
||||||
|
# intersection are pre-fold (the callback reads the user's original sys.argv), so
|
||||||
|
# preset tokens like ``physics=NAME`` compare correctly here. Fold runs after.
|
||||||
|
remaining_args = list_intersection(remaining_args, remaining_args_env_registration)
|
||||||
|
sys.argv = [sys.argv[0]] + remaining_args
|
||||||
|
|
||||||
|
# Check for installed RSL-RL version
|
||||||
|
installed_version = metadata.version("rsl-rl-lib")
|
||||||
|
|
||||||
|
|
||||||
|
@hydra_task_config(args_cli.task, args_cli.agent)
|
||||||
|
def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg):
|
||||||
|
"""Play with RSL-RL agent."""
|
||||||
|
with launch_simulation(env_cfg, args_cli):
|
||||||
|
# grab task name for checkpoint path
|
||||||
|
task_name = args_cli.task.split(":")[-1]
|
||||||
|
train_task_name = task_name.replace("-Play", "")
|
||||||
|
|
||||||
|
# override configurations with non-hydra CLI arguments
|
||||||
|
agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli)
|
||||||
|
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
|
||||||
|
|
||||||
|
# handle deprecated configurations
|
||||||
|
agent_cfg = handle_deprecated_rsl_rl_cfg(agent_cfg, installed_version)
|
||||||
|
|
||||||
|
# set the environment seed
|
||||||
|
# note: certain randomizations occur in the environment initialization so we set the seed here
|
||||||
|
env_cfg.seed = agent_cfg.seed
|
||||||
|
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device
|
||||||
|
|
||||||
|
# specify directory for logging experiments
|
||||||
|
log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name)
|
||||||
|
log_root_path = os.path.abspath(log_root_path)
|
||||||
|
print(f"[INFO] Loading experiment from directory: {log_root_path}")
|
||||||
|
if args_cli.use_pretrained_checkpoint:
|
||||||
|
resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name)
|
||||||
|
if not resume_path:
|
||||||
|
print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.")
|
||||||
|
return
|
||||||
|
elif args_cli.checkpoint:
|
||||||
|
resume_path = retrieve_file_path(args_cli.checkpoint)
|
||||||
|
else:
|
||||||
|
resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint)
|
||||||
|
|
||||||
|
log_dir = os.path.dirname(resume_path)
|
||||||
|
|
||||||
|
# set the log directory for the environment
|
||||||
|
env_cfg.log_dir = log_dir
|
||||||
|
|
||||||
|
# create isaac environment
|
||||||
|
env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None)
|
||||||
|
|
||||||
|
# convert to single-agent instance if required by the RL algorithm
|
||||||
|
if isinstance(env.unwrapped.cfg, DirectMARLEnvCfg):
|
||||||
|
from isaaclab.envs import multi_agent_to_single_agent
|
||||||
|
|
||||||
|
env = multi_agent_to_single_agent(env)
|
||||||
|
|
||||||
|
# wrap for video recording
|
||||||
|
if args_cli.video:
|
||||||
|
video_kwargs = {
|
||||||
|
"video_folder": os.path.join(log_dir, "videos", "play"),
|
||||||
|
"step_trigger": lambda step: step == 0,
|
||||||
|
"video_length": args_cli.video_length,
|
||||||
|
"disable_logger": True,
|
||||||
|
}
|
||||||
|
print("[INFO] Recording videos during training.")
|
||||||
|
print_dict(video_kwargs, nesting=4)
|
||||||
|
env = gym.wrappers.RecordVideo(env, **video_kwargs)
|
||||||
|
|
||||||
|
# wrap around environment for rsl-rl
|
||||||
|
env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
|
||||||
|
|
||||||
|
print(f"[INFO]: Loading model checkpoint from: {resume_path}")
|
||||||
|
# load previously trained model
|
||||||
|
if agent_cfg.class_name == "OnPolicyRunner":
|
||||||
|
runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device)
|
||||||
|
elif agent_cfg.class_name == "DistillationRunner":
|
||||||
|
runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device)
|
||||||
|
else:
|
||||||
|
raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}")
|
||||||
|
# configure_seed must be called after runner construction so that PyTorch deterministic settings
|
||||||
|
# do not interfere with the runner's internal initialization.
|
||||||
|
if args_cli.deterministic:
|
||||||
|
configure_seed(env_cfg.seed, True)
|
||||||
|
runner.load(resume_path)
|
||||||
|
|
||||||
|
# obtain the trained policy for inference
|
||||||
|
policy = runner.get_inference_policy(device=env.unwrapped.device)
|
||||||
|
|
||||||
|
# export the trained policy to JIT and ONNX formats
|
||||||
|
export_model_dir = os.path.join(os.path.dirname(resume_path), "exported")
|
||||||
|
|
||||||
|
if version.parse(installed_version) >= version.parse("4.0.0"):
|
||||||
|
# use the new export functions for rsl-rl >= 4.0.0
|
||||||
|
runner.export_policy_to_jit(path=export_model_dir, filename="policy.pt")
|
||||||
|
runner.export_policy_to_onnx(path=export_model_dir, filename="policy.onnx")
|
||||||
|
policy_nn = None # Not needed for rsl-rl >= 4.0.0
|
||||||
|
else:
|
||||||
|
# extract the neural network for rsl-rl < 4.0.0
|
||||||
|
if version.parse(installed_version) >= version.parse("2.3.0"):
|
||||||
|
policy_nn = runner.alg.policy
|
||||||
|
else:
|
||||||
|
policy_nn = runner.alg.actor_critic
|
||||||
|
|
||||||
|
# extract the normalizer
|
||||||
|
if hasattr(policy_nn, "actor_obs_normalizer"):
|
||||||
|
normalizer = policy_nn.actor_obs_normalizer
|
||||||
|
elif hasattr(policy_nn, "student_obs_normalizer"):
|
||||||
|
normalizer = policy_nn.student_obs_normalizer
|
||||||
|
else:
|
||||||
|
normalizer = None
|
||||||
|
|
||||||
|
# export to JIT and ONNX
|
||||||
|
export_policy_as_jit(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.pt")
|
||||||
|
export_policy_as_onnx(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.onnx")
|
||||||
|
|
||||||
|
dt = env.unwrapped.step_dt
|
||||||
|
|
||||||
|
# reset environment
|
||||||
|
obs = env.get_observations()
|
||||||
|
timestep = 0
|
||||||
|
# simulate environment
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
start_time = time.time()
|
||||||
|
# run everything in inference mode
|
||||||
|
with torch.inference_mode():
|
||||||
|
# agent stepping
|
||||||
|
actions = policy(obs)
|
||||||
|
if args_cli.print_actions:
|
||||||
|
print(
|
||||||
|
f"[MLP ACTION] step={timestep + 1}, "
|
||||||
|
f"shape={tuple(actions.shape)}, "
|
||||||
|
f"device={actions.device}, "
|
||||||
|
f"values={actions.detach().cpu().tolist()}"
|
||||||
|
)
|
||||||
|
# env stepping
|
||||||
|
obs, _, dones, _ = env.step(actions)
|
||||||
|
if args_cli.print_actions:
|
||||||
|
joint_pos_action = env.unwrapped.action_manager.get_term("joint_pos")
|
||||||
|
joint_pos_target = joint_pos_action.processed_actions
|
||||||
|
print(
|
||||||
|
f"[JOINT TARGET RAD] step={timestep + 1}, "
|
||||||
|
f"shape={tuple(joint_pos_target.shape)}, "
|
||||||
|
f"device={joint_pos_target.device}, "
|
||||||
|
f"values={joint_pos_target.detach().cpu().tolist()}"
|
||||||
|
)
|
||||||
|
# reset recurrent states for episodes that have terminated
|
||||||
|
if version.parse(installed_version) >= version.parse("4.0.0"):
|
||||||
|
policy.reset(dones)
|
||||||
|
else:
|
||||||
|
policy_nn.reset(dones)
|
||||||
|
if args_cli.video:
|
||||||
|
timestep += 1
|
||||||
|
if timestep == args_cli.video_length:
|
||||||
|
break
|
||||||
|
|
||||||
|
sleep_time = dt - (time.time() - start_time)
|
||||||
|
if args_cli.real_time and sleep_time > 0:
|
||||||
|
time.sleep(sleep_time)
|
||||||
|
|
||||||
|
# close the simulator
|
||||||
|
env.close()
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
@ -0,0 +1,234 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
"""Script to play a checkpoint of an RL agent from RSL-RL."""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import contextlib
|
||||||
|
import importlib.metadata as metadata
|
||||||
|
import os
|
||||||
|
import sys
|
||||||
|
import time
|
||||||
|
|
||||||
|
import gymnasium as gym
|
||||||
|
import torch
|
||||||
|
from packaging import version
|
||||||
|
from rsl_rl.runners import DistillationRunner, OnPolicyRunner
|
||||||
|
|
||||||
|
from isaaclab.envs import DirectMARLEnvCfg, DirectRLEnvCfg, ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.utils.assets import retrieve_file_path
|
||||||
|
from isaaclab.utils.dict import print_dict
|
||||||
|
from isaaclab.utils.string import list_intersection, string_to_callable
|
||||||
|
|
||||||
|
from isaaclab_rl.rsl_rl import (
|
||||||
|
RslRlBaseRunnerCfg,
|
||||||
|
RslRlVecEnvWrapper,
|
||||||
|
export_policy_as_jit,
|
||||||
|
export_policy_as_onnx,
|
||||||
|
handle_deprecated_rsl_rl_cfg,
|
||||||
|
)
|
||||||
|
from isaaclab_rl.utils.pretrained_checkpoint import get_published_pretrained_checkpoint
|
||||||
|
|
||||||
|
import isaaclab_tasks # noqa: F401
|
||||||
|
from isaaclab_tasks.utils import (
|
||||||
|
add_launcher_args,
|
||||||
|
get_checkpoint_path,
|
||||||
|
launch_simulation,
|
||||||
|
setup_preset_cli,
|
||||||
|
)
|
||||||
|
from isaaclab_tasks.utils.hydra import hydra_task_config
|
||||||
|
|
||||||
|
# local imports
|
||||||
|
import cli_args # isort: skip
|
||||||
|
|
||||||
|
import TwoFootBot_isaaclab.tasks # noqa: F401
|
||||||
|
with contextlib.suppress(ImportError):
|
||||||
|
import isaaclab_tasks_experimental # noqa: F401
|
||||||
|
|
||||||
|
# -- argparse ----------------------------------------------------------------
|
||||||
|
parser = argparse.ArgumentParser(description="Play a checkpoint of an RL agent from RSL-RL.")
|
||||||
|
parser.add_argument("--video", action="store_true", default=False, help="Record videos during play.")
|
||||||
|
parser.add_argument("--video_length", type=int, default=500, help="Length of the recorded video (in steps).")
|
||||||
|
parser.add_argument(
|
||||||
|
"--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations."
|
||||||
|
)
|
||||||
|
parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.")
|
||||||
|
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--agent", type=str, default="rsl_rl_cfg_entry_point", help="Name of the RL agent configuration entry point."
|
||||||
|
)
|
||||||
|
parser.add_argument("--seed", type=int, default=None, help="Seed used for the environment")
|
||||||
|
parser.add_argument(
|
||||||
|
"--use_pretrained_checkpoint",
|
||||||
|
action="store_true",
|
||||||
|
help="Use the pre-trained checkpoint from Nucleus.",
|
||||||
|
)
|
||||||
|
parser.add_argument("--real-time", action="store_true", default=False, help="Run in real-time, if possible.")
|
||||||
|
parser.add_argument("--external_callback", default=None, help="Fully qualified path to an externally defined callback.")
|
||||||
|
cli_args.add_rsl_rl_args(parser)
|
||||||
|
add_launcher_args(parser)
|
||||||
|
args_cli, remaining_args = setup_preset_cli(parser)
|
||||||
|
|
||||||
|
if args_cli.video:
|
||||||
|
args_cli.enable_cameras = True
|
||||||
|
|
||||||
|
|
||||||
|
# Call an external callback if requested. This gives opportunity to external code to register the environments
|
||||||
|
# The function is expected to return a list of arguments that were not consumed by the callback.
|
||||||
|
remaining_args_env_registration = None
|
||||||
|
if args_cli.external_callback:
|
||||||
|
external_callback_function = string_to_callable(args_cli.external_callback, separator=".")
|
||||||
|
remaining_args_env_registration = external_callback_function()
|
||||||
|
|
||||||
|
# clear out sys.argv for Hydra
|
||||||
|
# The remaining arguments are the arguments that were not consumed by both this scripts
|
||||||
|
# argparser and (optionally) the external callback function.
|
||||||
|
remaining_args = list_intersection(remaining_args, remaining_args_env_registration)
|
||||||
|
sys.argv = [sys.argv[0]] + remaining_args
|
||||||
|
|
||||||
|
# Check for installed RSL-RL version
|
||||||
|
installed_version = metadata.version("rsl-rl-lib")
|
||||||
|
|
||||||
|
|
||||||
|
@hydra_task_config(args_cli.task, args_cli.agent)
|
||||||
|
def main(env_cfg: ManagerBasedRLEnvCfg | DirectRLEnvCfg | DirectMARLEnvCfg, agent_cfg: RslRlBaseRunnerCfg):
|
||||||
|
"""Play with RSL-RL agent."""
|
||||||
|
with launch_simulation(env_cfg, args_cli):
|
||||||
|
# grab task name for checkpoint path
|
||||||
|
task_name = args_cli.task.split(":")[-1]
|
||||||
|
train_task_name = task_name.replace("-Play", "")
|
||||||
|
|
||||||
|
# override configurations with non-hydra CLI arguments
|
||||||
|
agent_cfg = cli_args.update_rsl_rl_cfg(agent_cfg, args_cli)
|
||||||
|
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
|
||||||
|
|
||||||
|
# handle deprecated configurations
|
||||||
|
agent_cfg = handle_deprecated_rsl_rl_cfg(agent_cfg, installed_version)
|
||||||
|
|
||||||
|
# set the environment seed
|
||||||
|
# note: certain randomizations occur in the environment initialization so we set the seed here
|
||||||
|
env_cfg.seed = agent_cfg.seed
|
||||||
|
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device
|
||||||
|
|
||||||
|
# specify directory for logging experiments
|
||||||
|
log_root_path = os.path.join("logs", "rsl_rl", agent_cfg.experiment_name)
|
||||||
|
log_root_path = os.path.abspath(log_root_path)
|
||||||
|
print(f"[INFO] Loading experiment from directory: {log_root_path}")
|
||||||
|
if args_cli.use_pretrained_checkpoint:
|
||||||
|
resume_path = get_published_pretrained_checkpoint("rsl_rl", train_task_name)
|
||||||
|
if not resume_path:
|
||||||
|
print("[INFO] Unfortunately a pre-trained checkpoint is currently unavailable for this task.")
|
||||||
|
return
|
||||||
|
elif args_cli.checkpoint:
|
||||||
|
resume_path = retrieve_file_path(args_cli.checkpoint)
|
||||||
|
else:
|
||||||
|
resume_path = get_checkpoint_path(log_root_path, agent_cfg.load_run, agent_cfg.load_checkpoint)
|
||||||
|
|
||||||
|
log_dir = os.path.dirname(resume_path)
|
||||||
|
|
||||||
|
# set the log directory for the environment
|
||||||
|
env_cfg.log_dir = log_dir
|
||||||
|
|
||||||
|
# create isaac environment
|
||||||
|
env = gym.make(args_cli.task, cfg=env_cfg, render_mode="rgb_array" if args_cli.video else None)
|
||||||
|
|
||||||
|
# convert to single-agent instance if required by the RL algorithm
|
||||||
|
if isinstance(env.unwrapped.cfg, DirectMARLEnvCfg):
|
||||||
|
from isaaclab.envs import multi_agent_to_single_agent
|
||||||
|
|
||||||
|
env = multi_agent_to_single_agent(env)
|
||||||
|
|
||||||
|
# wrap for video recording
|
||||||
|
if args_cli.video:
|
||||||
|
video_kwargs = {
|
||||||
|
"video_folder": os.path.join(log_dir, "videos", "play"),
|
||||||
|
"step_trigger": lambda step: step == 0,
|
||||||
|
"video_length": args_cli.video_length,
|
||||||
|
"disable_logger": True,
|
||||||
|
}
|
||||||
|
print("[INFO] Recording videos during play.")
|
||||||
|
print_dict(video_kwargs, nesting=4)
|
||||||
|
env = gym.wrappers.RecordVideo(env, **video_kwargs)
|
||||||
|
|
||||||
|
# wrap around environment for rsl-rl
|
||||||
|
env = RslRlVecEnvWrapper(env, clip_actions=agent_cfg.clip_actions)
|
||||||
|
|
||||||
|
print(f"[INFO]: Loading model checkpoint from: {resume_path}")
|
||||||
|
# load previously trained model
|
||||||
|
if agent_cfg.class_name == "OnPolicyRunner":
|
||||||
|
runner = OnPolicyRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device)
|
||||||
|
elif agent_cfg.class_name == "DistillationRunner":
|
||||||
|
runner = DistillationRunner(env, agent_cfg.to_dict(), log_dir=None, device=agent_cfg.device)
|
||||||
|
else:
|
||||||
|
raise ValueError(f"Unsupported runner class: {agent_cfg.class_name}")
|
||||||
|
runner.load(resume_path)
|
||||||
|
|
||||||
|
# obtain the trained policy for inference
|
||||||
|
policy = runner.get_inference_policy(device=env.unwrapped.device)
|
||||||
|
|
||||||
|
# export the trained policy to JIT and ONNX formats
|
||||||
|
export_model_dir = os.path.join(os.path.dirname(resume_path), "exported")
|
||||||
|
|
||||||
|
if version.parse(installed_version) >= version.parse("4.0.0"):
|
||||||
|
# use the new export functions for rsl-rl >= 4.0.0
|
||||||
|
runner.export_policy_to_jit(path=export_model_dir, filename="policy.pt")
|
||||||
|
runner.export_policy_to_onnx(path=export_model_dir, filename="policy.onnx")
|
||||||
|
policy_nn = None # Not needed for rsl-rl >= 4.0.0
|
||||||
|
else:
|
||||||
|
# extract the neural network for rsl-rl < 4.0.0
|
||||||
|
if version.parse(installed_version) >= version.parse("2.3.0"):
|
||||||
|
policy_nn = runner.alg.policy
|
||||||
|
else:
|
||||||
|
policy_nn = runner.alg.actor_critic
|
||||||
|
|
||||||
|
# extract the normalizer
|
||||||
|
if hasattr(policy_nn, "actor_obs_normalizer"):
|
||||||
|
normalizer = policy_nn.actor_obs_normalizer
|
||||||
|
elif hasattr(policy_nn, "student_obs_normalizer"):
|
||||||
|
normalizer = policy_nn.student_obs_normalizer
|
||||||
|
else:
|
||||||
|
normalizer = None
|
||||||
|
|
||||||
|
# export to JIT and ONNX
|
||||||
|
export_policy_as_jit(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.pt")
|
||||||
|
export_policy_as_onnx(policy_nn, normalizer=normalizer, path=export_model_dir, filename="policy.onnx")
|
||||||
|
|
||||||
|
dt = env.unwrapped.step_dt
|
||||||
|
|
||||||
|
# reset environment
|
||||||
|
obs = env.get_observations()
|
||||||
|
timestep = 0
|
||||||
|
# simulate environment
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
start_time = time.time()
|
||||||
|
# run everything in inference mode
|
||||||
|
with torch.inference_mode():
|
||||||
|
# agent stepping
|
||||||
|
actions = policy(obs)
|
||||||
|
# env stepping
|
||||||
|
obs, _, dones, _ = env.step(actions)
|
||||||
|
# reset recurrent states for episodes that have terminated
|
||||||
|
if version.parse(installed_version) >= version.parse("4.0.0"):
|
||||||
|
policy.reset(dones)
|
||||||
|
else:
|
||||||
|
policy_nn.reset(dones)
|
||||||
|
if args_cli.video:
|
||||||
|
timestep += 1
|
||||||
|
if timestep == args_cli.video_length:
|
||||||
|
break
|
||||||
|
|
||||||
|
sleep_time = dt - (time.time() - start_time)
|
||||||
|
if args_cli.real_time and sleep_time > 0:
|
||||||
|
time.sleep(sleep_time)
|
||||||
|
|
||||||
|
# close the simulator
|
||||||
|
env.close()
|
||||||
|
except KeyboardInterrupt:
|
||||||
|
pass
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
main()
|
||||||
|
|
@ -0,0 +1,86 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
"""Script to run an environment with zero action agent."""
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import contextlib
|
||||||
|
import sys
|
||||||
|
|
||||||
|
import gymnasium as gym
|
||||||
|
import torch
|
||||||
|
|
||||||
|
import isaaclab_tasks # noqa: F401
|
||||||
|
|
||||||
|
with contextlib.suppress(ImportError):
|
||||||
|
import isaaclab_tasks_experimental # noqa: F401
|
||||||
|
from isaaclab_tasks.utils import (
|
||||||
|
add_launcher_args,
|
||||||
|
launch_simulation,
|
||||||
|
resolve_task_config,
|
||||||
|
setup_preset_cli,
|
||||||
|
)
|
||||||
|
|
||||||
|
# add argparse arguments
|
||||||
|
parser = argparse.ArgumentParser(description="Zero agent for Isaac Lab environments.")
|
||||||
|
parser.add_argument(
|
||||||
|
"--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations."
|
||||||
|
)
|
||||||
|
parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.")
|
||||||
|
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
|
||||||
|
# append AppLauncher cli args
|
||||||
|
add_launcher_args(parser)
|
||||||
|
# simple agents should open Kit visualizer by default
|
||||||
|
parser.set_defaults(visualizer=["kit"])
|
||||||
|
args_cli, hydra_args = setup_preset_cli(parser)
|
||||||
|
sys.argv = [sys.argv[0]] + hydra_args
|
||||||
|
|
||||||
|
import TwoFootBot_isaaclab.tasks # noqa: F401
|
||||||
|
MAX_STEPS = 100
|
||||||
|
|
||||||
|
|
||||||
|
def main():
|
||||||
|
"""Zero actions agent with Isaac Lab environment."""
|
||||||
|
|
||||||
|
torch.manual_seed(42)
|
||||||
|
|
||||||
|
# parse configuration via Hydra (supports preset selection, e.g. env.sim.physics=newton_mjwarp)
|
||||||
|
env_cfg, _ = resolve_task_config(args_cli.task, "")
|
||||||
|
|
||||||
|
with launch_simulation(env_cfg, args_cli):
|
||||||
|
# override with CLI arguments
|
||||||
|
env_cfg.scene.num_envs = args_cli.num_envs if args_cli.num_envs is not None else env_cfg.scene.num_envs
|
||||||
|
env_cfg.sim.device = args_cli.device if args_cli.device is not None else env_cfg.sim.device
|
||||||
|
if args_cli.disable_fabric:
|
||||||
|
env_cfg.sim.use_fabric = False
|
||||||
|
|
||||||
|
# create environment
|
||||||
|
env = gym.make(args_cli.task, cfg=env_cfg)
|
||||||
|
|
||||||
|
# print info (this is vectorized environment)
|
||||||
|
print(f"[INFO]: Gym observation space: {env.observation_space}")
|
||||||
|
print(f"[INFO]: Gym action space: {env.action_space}")
|
||||||
|
# reset environment
|
||||||
|
env.reset()
|
||||||
|
# simulate environment
|
||||||
|
# keep running while any visualizer is open, otherwise fall back to MAX_STEPS
|
||||||
|
sim = env.unwrapped.sim
|
||||||
|
actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device)
|
||||||
|
while True:
|
||||||
|
if sim.visualizers:
|
||||||
|
# visualizer mode: run until the visualizer window is closed
|
||||||
|
if not any(v.is_running() and not v.is_closed for v in sim.visualizers):
|
||||||
|
break
|
||||||
|
# run everything in inference mode
|
||||||
|
with torch.inference_mode():
|
||||||
|
# apply actions
|
||||||
|
env.step(actions)
|
||||||
|
# close the simulator
|
||||||
|
env.close()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
# run the main function
|
||||||
|
main()
|
||||||
|
|
@ -1,10 +1,17 @@
|
||||||
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
# All rights reserved.
|
# All rights reserved.
|
||||||
#
|
#
|
||||||
# SPDX-License-Identifier: BSD-3-Clause
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
"""Task registrations for the project."""
|
"""Package containing task implementations for the extension."""
|
||||||
|
|
||||||
|
##
|
||||||
|
# Register Gym environments.
|
||||||
|
##
|
||||||
|
|
||||||
from isaaclab_tasks.utils import import_packages
|
from isaaclab_tasks.utils import import_packages
|
||||||
|
|
||||||
import_packages(__name__, ["utils", ".mdp"])
|
# The blacklist is used to prevent importing configs from sub-packages
|
||||||
|
_BLACKLIST_PKGS = ["utils", ".mdp"]
|
||||||
|
# Import all configs in this package
|
||||||
|
import_packages(__name__, _BLACKLIST_PKGS)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,6 @@
|
||||||
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import gymnasium as gym # noqa: F401
|
||||||
|
|
@ -0,0 +1,391 @@
|
||||||
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import math
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
import torch
|
||||||
|
import isaaclab.sim as sim_utils
|
||||||
|
from isaaclab.actuators import ImplicitActuatorCfg
|
||||||
|
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab.managers import ObservationGroupCfg as ObsGroup
|
||||||
|
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab_physx.physics import PhysxCfg
|
||||||
|
from isaaclab.scene import InteractiveSceneCfg
|
||||||
|
from isaaclab.sensors import ContactSensorCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
from isaaclab.utils.noise import UniformNoiseCfg
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as velocity_mdp
|
||||||
|
|
||||||
|
from . import mdp
|
||||||
|
|
||||||
|
##
|
||||||
|
# Pre-defined configs
|
||||||
|
##
|
||||||
|
|
||||||
|
JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。
|
||||||
|
"jlf0_link",
|
||||||
|
"jlf1_link",
|
||||||
|
"jlf2_link",
|
||||||
|
"jlf3_link",
|
||||||
|
"jrf0_link",
|
||||||
|
"jrf1_link",
|
||||||
|
"jrf2_link",
|
||||||
|
"jrf3_link",
|
||||||
|
]
|
||||||
|
BOT_PACKAGE_PATH = Path(__file__).resolve().parents[5] / "bot/2foot" # 获取机器人 ROS 包目录。
|
||||||
|
URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。
|
||||||
|
|
||||||
|
def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs):
|
||||||
|
"""Spawn the URDF and enable contact reporting on every rigid link."""
|
||||||
|
from pxr import Usd, UsdPhysics
|
||||||
|
from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf
|
||||||
|
from isaaclab.sim.utils import safe_set_attribute_on_usd_prim
|
||||||
|
|
||||||
|
spawn_cfg = cfg.replace(activate_contact_sensors=False)
|
||||||
|
prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs)
|
||||||
|
for child_prim in Usd.PrimRange(prim):
|
||||||
|
if not child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||||
|
continue
|
||||||
|
applied_schemas = child_prim.GetAppliedSchemas()
|
||||||
|
if "PhysxRigidBodyAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
|
||||||
|
if "PhysxContactReportAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxContactReportAPI")
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxContactReport:threshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
return prim
|
||||||
|
|
||||||
|
TWOFOOTBOT_URDF_CFG = ArticulationCfg(
|
||||||
|
spawn=sim_utils.UrdfFileCfg(
|
||||||
|
func=spawn_twofootbot_from_urdf,
|
||||||
|
asset_path=str(URDF_PATH),
|
||||||
|
fix_base=False,
|
||||||
|
activate_contact_sensors=False,
|
||||||
|
ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}],
|
||||||
|
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
|
||||||
|
drive_type="force",
|
||||||
|
target_type="position",
|
||||||
|
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(stiffness=17.0, damping=0.3),
|
||||||
|
),
|
||||||
|
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||||
|
disable_gravity=False, # 不启用重力。
|
||||||
|
max_linear_velocity=100.0,
|
||||||
|
max_angular_velocity=100.0,
|
||||||
|
max_depenetration_velocity=5.0,
|
||||||
|
),
|
||||||
|
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||||
|
enabled_self_collisions=True, # # 启用自碰撞检测。
|
||||||
|
solver_position_iteration_count=8,
|
||||||
|
solver_velocity_iteration_count=1,
|
||||||
|
),
|
||||||
|
),
|
||||||
|
init_state=ArticulationCfg.InitialStateCfg( # 初始状态配置。单位: 弧度
|
||||||
|
pos=(0.0, 0.0, 0.18),
|
||||||
|
joint_pos={"jlf0_link": 0.0, "jlf1_link": 0.0, "jlf2_link": 0.0, "jlf3_link": 0.0,
|
||||||
|
"jrf0_link": 0.0, "jrf1_link": 0.0, "jrf2_link": 0.0, "jrf3_link": 0.0},
|
||||||
|
joint_vel={".*": 0.0},
|
||||||
|
),
|
||||||
|
actuators={ # 动力配置。
|
||||||
|
"legs": ImplicitActuatorCfg(
|
||||||
|
joint_names_expr=JOINT_NAMES, # 控制所有腿的关节。
|
||||||
|
effort_limit_sim=0.5, # 力矩限制。单位:牛顿米。
|
||||||
|
velocity_limit_sim=5, # 速度限制 (rad/s)
|
||||||
|
stiffness=5.0, # 刚度。单位:牛顿/米。
|
||||||
|
damping=0.3, # 阻尼。单位:牛顿秒/米。
|
||||||
|
)
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
DEMO_ROOT = Path(__file__).resolve().parents[5]
|
||||||
|
URDF_PATH = DEMO_ROOT / "bot" / "2foot" / "urdf" / "2foot.urdf"
|
||||||
|
BOT_PACKAGE_PATH = DEMO_ROOT / "bot" / "2foot"
|
||||||
|
JOINT_NAMES = [
|
||||||
|
"jlf0_link",
|
||||||
|
"jlf1_link",
|
||||||
|
"jlf2_link",
|
||||||
|
"jlf3_link",
|
||||||
|
"jrf0_link",
|
||||||
|
"jrf1_link",
|
||||||
|
"jrf2_link",
|
||||||
|
"jrf3_link",
|
||||||
|
]
|
||||||
|
IMU_OFFSET_B = (0.0, 0.0, 0.091)
|
||||||
|
|
||||||
|
|
||||||
|
def imu_lin_vel(env, asset_cfg: SceneEntityCfg, offset_b: tuple[float, float, float]) -> torch.Tensor:
|
||||||
|
"""Return the linear velocity at the IMU position, expressed in the body frame."""
|
||||||
|
asset = env.scene[asset_cfg.name]
|
||||||
|
offset = asset.data.root_ang_vel_b.new_tensor(offset_b).expand_as(asset.data.root_ang_vel_b)
|
||||||
|
return asset.data.root_lin_vel_b + torch.linalg.cross(asset.data.root_ang_vel_b, offset, dim=1)
|
||||||
|
|
||||||
|
TWOFOOTBOT_CFG = ArticulationCfg(
|
||||||
|
spawn=sim_utils.UrdfFileCfg(
|
||||||
|
asset_path=str(URDF_PATH),
|
||||||
|
fix_base=False,
|
||||||
|
activate_contact_sensors=True,
|
||||||
|
ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}],
|
||||||
|
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
|
||||||
|
drive_type="force",
|
||||||
|
target_type="position",
|
||||||
|
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(stiffness=17.0, damping=0.3),
|
||||||
|
),
|
||||||
|
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||||
|
disable_gravity=False,
|
||||||
|
max_linear_velocity=100.0,
|
||||||
|
max_angular_velocity=100.0,
|
||||||
|
max_depenetration_velocity=5.0,
|
||||||
|
),
|
||||||
|
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||||
|
enabled_self_collisions=True,
|
||||||
|
solver_position_iteration_count=8,
|
||||||
|
solver_velocity_iteration_count=1,
|
||||||
|
),
|
||||||
|
),
|
||||||
|
init_state=ArticulationCfg.InitialStateCfg(
|
||||||
|
pos=(0.0, 0.0, 0.22),
|
||||||
|
joint_pos={joint_name: 0.0 for joint_name in JOINT_NAMES},
|
||||||
|
joint_vel={".*": 0.0},
|
||||||
|
),
|
||||||
|
actuators={
|
||||||
|
"legs": ImplicitActuatorCfg(
|
||||||
|
joint_names_expr=JOINT_NAMES,
|
||||||
|
effort_limit_sim=0.5,
|
||||||
|
velocity_limit_sim=5.0,
|
||||||
|
stiffness=5.0,
|
||||||
|
damping=0.3,
|
||||||
|
)
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Scene definition
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TwofootbotdemoSceneCfg(InteractiveSceneCfg):
|
||||||
|
|
||||||
|
"""
|
||||||
|
双足机器人演示场景配置类,继承自InteractiveSceneCfg。
|
||||||
|
用于配置双足机器人演示场景中的各种元素,包括地面、机器人、光源等。
|
||||||
|
"""
|
||||||
|
# 地面配置:设置地面的大小和位置
|
||||||
|
ground = AssetBaseCfg(
|
||||||
|
prim_path="/World/ground", # 地面在场景中的路径
|
||||||
|
spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)), # 地面大小为100x100单位
|
||||||
|
)
|
||||||
|
|
||||||
|
# 机器人配置:使用URDF配置文件,并设置其在场景中的路径
|
||||||
|
robot: ArticulationCfg = TWOFOOTBOT_URDF_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
contact_forces = None # 接触力设置为None,表示不计算接触力
|
||||||
|
|
||||||
|
# 穹顶光源配置:设置场景中的全局光照
|
||||||
|
dome_light = AssetBaseCfg(
|
||||||
|
prim_path="/World/DomeLight", # 光源在场景中的路径
|
||||||
|
spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0), # 设置光源颜色和强度
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# MDP settings
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ActionsCfg:
|
||||||
|
"""Action specifications for the MDP."""
|
||||||
|
|
||||||
|
joint_pos = velocity_mdp.JointPositionActionCfg(
|
||||||
|
asset_name="robot", joint_names=JOINT_NAMES, scale=0.5, use_default_offset=True
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CommandsCfg:
|
||||||
|
"""The arrow points along +X at a fixed walking speed."""
|
||||||
|
|
||||||
|
base_velocity = velocity_mdp.UniformVelocityCommandCfg(
|
||||||
|
asset_name="robot",
|
||||||
|
resampling_time_range=(0.0, 3.0),
|
||||||
|
# 相对航向环境参数,用于控制或影响相对航向相关的环境配置
|
||||||
|
# 该参数的默认值为0.0,表示在不设置特殊航向环境时的基准值
|
||||||
|
rel_standing_envs=0.2,
|
||||||
|
# 相对航向环境参数,默认值为0.0
|
||||||
|
rel_heading_envs=0.0,
|
||||||
|
# 航向命令标志,默认为False
|
||||||
|
heading_command=False,
|
||||||
|
debug_vis=True,
|
||||||
|
ranges=velocity_mdp.UniformVelocityCommandCfg.Ranges(
|
||||||
|
lin_vel_x=(0.2, 0.2),
|
||||||
|
lin_vel_y=(0.0, 0.0),
|
||||||
|
ang_vel_z=(-0.1, 0.1),
|
||||||
|
heading=(-math.pi, math.pi),
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ObservationsCfg:
|
||||||
|
"""Observation specifications for the MDP."""
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PolicyCfg(ObsGroup):
|
||||||
|
"""Observations for policy group."""
|
||||||
|
|
||||||
|
# observation terms (order preserved)
|
||||||
|
|
||||||
|
# 创建观测项,用于定义环境中的各种观测信息
|
||||||
|
# 每个观测项都包含一个函数和一个可选的噪声配置
|
||||||
|
# 基线线性速度观测项,添加均匀噪声,噪声范围在-0.1到0.1之间
|
||||||
|
# base_lin_vel = ObsTerm(
|
||||||
|
# func=imu_lin_vel,
|
||||||
|
# params={"asset_cfg": SceneEntityCfg("robot"), "offset_b": IMU_OFFSET_B},
|
||||||
|
# noise=UniformNoiseCfg(n_min=-0.1, n_max=0.1),
|
||||||
|
# )
|
||||||
|
# 基线角速度观测项,添加均匀噪声,噪声范围在-0.2到0.2之间
|
||||||
|
base_ang_vel = ObsTerm(func=velocity_mdp.base_ang_vel, noise=UniformNoiseCfg(n_min=-0.2, n_max=0.2))
|
||||||
|
# 投影重力观测项,添加噪声
|
||||||
|
projected_gravity = ObsTerm(func=velocity_mdp.projected_gravity, noise=UniformNoiseCfg(n_min=-0.3, n_max=0.3))
|
||||||
|
# 速度命令观测项,使用特定的命令名称"base_velocity"
|
||||||
|
velocity_commands = ObsTerm(
|
||||||
|
func=velocity_mdp.generated_commands, params={"command_name": "base_velocity"}
|
||||||
|
)
|
||||||
|
# 相对关节位置观测项,使用场景中的机器人实体
|
||||||
|
#joint_pos_rel = ObsTerm(func=velocity_mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("robot")})
|
||||||
|
# 相对关节速度观测项,使用场景中的机器人实体
|
||||||
|
#joint_vel_rel = ObsTerm(func=velocity_mdp.joint_vel_rel, params={"asset_cfg": SceneEntityCfg("robot")})
|
||||||
|
# 上一个动作观测项
|
||||||
|
actions = ObsTerm(func=velocity_mdp.last_action)
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
self.enable_corruption = False
|
||||||
|
self.concatenate_terms = True
|
||||||
|
|
||||||
|
# observation groups
|
||||||
|
policy: PolicyCfg = PolicyCfg()
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class EventCfg:
|
||||||
|
"""Configuration for events."""
|
||||||
|
|
||||||
|
# reset
|
||||||
|
#定义一个基础重置事件项,用于重置机器人的初始状态
|
||||||
|
reset_base = EventTerm(
|
||||||
|
# 指定重置函数为velocity_mdp模块中的reset_root_state_uniform函数
|
||||||
|
func=velocity_mdp.reset_root_state_uniform,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"pose_range": {"x": (-0.2, 0.2), "y": (-0.05, 0.05), "yaw": (-0.1, 0.1)},
|
||||||
|
"velocity_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.05, 0.05)},
|
||||||
|
},
|
||||||
|
)
|
||||||
|
# 定义一个重置关节的事件项
|
||||||
|
reset_joints = EventTerm(
|
||||||
|
# 指定重置关节的函数,使用velocity_mdp模块中的reset_joints_by_scale方法
|
||||||
|
func=velocity_mdp.reset_joints_by_scale,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"position_range": (0.9, 1.1),
|
||||||
|
"velocity_range": (0.0, 0.0),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RewardsCfg:
|
||||||
|
"""Reward terms for the MDP."""
|
||||||
|
|
||||||
|
# (1) Constant running reward
|
||||||
|
# 定义跟踪角速度z方向的指数奖励项配置
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_exp, # 奖励函数,用于跟踪角速度z方向的指数
|
||||||
|
weight=2.0, # 奖励项权重,控制该奖励项的重要性
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5}, # 命令名称和标准差参数
|
||||||
|
)
|
||||||
|
# 轨迹线性速度奖励项
|
||||||
|
track_lin_vel = RewTerm( # 定义一个奖励项,用于跟踪线性速度
|
||||||
|
func=velocity_mdp.track_lin_vel_xy_exp, # 使用velocity_mdp模块中的track_lin_vel_xy_exp函数作为奖励计算方法
|
||||||
|
weight=5.0,
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5},
|
||||||
|
)
|
||||||
|
# 创建奖励项,用于控制机器人的各种行为
|
||||||
|
upright = RewTerm(func=velocity_mdp.flat_orientation_l2, weight=-1e-4) # 控制机器人保持直立姿态,权重为-1.0
|
||||||
|
vertical_velocity = RewTerm(func=velocity_mdp.lin_vel_z_l2, weight=-0.1) # 控制机器人垂直方向速度,权重为-1.0
|
||||||
|
angular_velocity = RewTerm(func=velocity_mdp.ang_vel_xy_l2, weight=-0.05) # 控制机器人角速度,权重为-0.05
|
||||||
|
action_rate = RewTerm(func=velocity_mdp.action_rate_l2, weight=-0.001) # 控制机器人动作变化率,权重为-0.01
|
||||||
|
joint_acceleration = RewTerm(func=velocity_mdp.joint_acc_l2, weight=-2.5e-7) # 控制机器人关节加速度,权重为-2.5e-7
|
||||||
|
joint_limits = RewTerm(func=velocity_mdp.joint_pos_limits, weight=-1.0) # 确保机器人关节位置在限制范围内,权重为-1.0
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TerminationsCfg:
|
||||||
|
|
||||||
|
# 设置超时终止条件,当仿真运行时间超过设定值时终止
|
||||||
|
time_out = DoneTerm(func=mdp.time_out, time_out=True)
|
||||||
|
# low_speed = DoneTerm(
|
||||||
|
# func=mdp.root_low_speed_for_duration,
|
||||||
|
# params={
|
||||||
|
# "asset_cfg": SceneEntityCfg("robot"),
|
||||||
|
# "threshold": 0.1,
|
||||||
|
# "duration": 1.0,
|
||||||
|
# },
|
||||||
|
# )
|
||||||
|
# 设置姿态终止条件,当机器人姿态超过限制角度时终止
|
||||||
|
fallen_orientation = DoneTerm(
|
||||||
|
func=mdp.bad_orientation,
|
||||||
|
params={"limit_angle": 0.3}, # 限制角度设为0.7弧度
|
||||||
|
)
|
||||||
|
# 设置高度终止条件,当机器人高度低于最小值时终止
|
||||||
|
fallen_height = DoneTerm(
|
||||||
|
func=mdp.root_height_below_minimum,
|
||||||
|
params={"minimum_height": 0.10}, # 最小高度设为0.1米
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Environment configuration
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TwofootbotdemoEnvCfg(ManagerBasedRLEnvCfg):
|
||||||
|
scene: TwofootbotdemoSceneCfg = TwofootbotdemoSceneCfg(num_envs=4096*5, env_spacing=0.5)
|
||||||
|
# Basic settings
|
||||||
|
observations: ObservationsCfg = ObservationsCfg()
|
||||||
|
actions: ActionsCfg = ActionsCfg()
|
||||||
|
commands: CommandsCfg = CommandsCfg()
|
||||||
|
events: EventCfg = EventCfg()
|
||||||
|
# MDP settings
|
||||||
|
rewards: RewardsCfg = RewardsCfg()
|
||||||
|
terminations: TerminationsCfg = TerminationsCfg()
|
||||||
|
|
||||||
|
# Post initialization
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
# general settings
|
||||||
|
self.decimation = 4
|
||||||
|
self.episode_length_s = 80.0 #time_out
|
||||||
|
# viewer settings
|
||||||
|
self.viewer.eye = (5.0, -5.0, 3.0)
|
||||||
|
self.viewer.lookat = (0.0, 0.0, 0.0)
|
||||||
|
# simulation settings
|
||||||
|
self.sim.physics = PhysxCfg(gpu_max_rigid_patch_count=100 * 2**15)
|
||||||
|
self.sim.dt = 1 / 200
|
||||||
|
self.sim.render_interval = self.decimation
|
||||||
|
self.sim.visualizer_cfgs = []
|
||||||
|
|
@ -0,0 +1,138 @@
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab_tasks.utils import preset
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab_physx.physics import PhysxCfg
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv0 import TwofootbotdemoEnvCfg
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv0 import EventCfg, TerminationsCfg, RewardsCfg
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class Lv1EventCfg(EventCfg):
|
||||||
|
# 创建一个物理材质事件项
|
||||||
|
physics_material = EventTerm(
|
||||||
|
# 指定函数为随机化刚体材质的函数
|
||||||
|
func=mdp.randomize_rigid_body_material,
|
||||||
|
# 设置事件模式为启动时执行
|
||||||
|
mode="startup",
|
||||||
|
# 设置事件参数
|
||||||
|
params={
|
||||||
|
# 配置场景实体,指定为机器人,并匹配所有身体部位
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||||
|
# 设置静态摩擦力的范围为固定值0.8
|
||||||
|
"static_friction_range": (0.8, 0.8),
|
||||||
|
# 设置动态摩擦力的范围为固定值0.6
|
||||||
|
"dynamic_friction_range": (0.6, 0.6),
|
||||||
|
# 设置恢复系数的范围为固定值0.0(完全非弹性碰撞)
|
||||||
|
"restitution_range": (0.0, 0.0),
|
||||||
|
# 设置材质分桶数量为64
|
||||||
|
"num_buckets": 64,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 定义一个基础质量添加事件项
|
||||||
|
add_base_mass = EventTerm(
|
||||||
|
# 设置事件项的执行函数为随机化刚体质量
|
||||||
|
func=mdp.randomize_rigid_body_mass,
|
||||||
|
# 设置事件项的执行模式为"startup",表示在启动时执行
|
||||||
|
mode="startup",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
"mass_distribution_params": (1 / 1.25, 1.25),
|
||||||
|
"operation": "scale",
|
||||||
|
"distribution": "log_uniform",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 预设基座质心随机化配置
|
||||||
|
base_com = preset( # 使用preset函数创建预设配置
|
||||||
|
default=EventTerm( # 默认事件项配置
|
||||||
|
func=mdp.randomize_rigid_body_com, # 执行的函数:随机化刚体质心
|
||||||
|
mode="startup", # 事件模式:启动时执行
|
||||||
|
params={ # 函数参数配置
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"), # 场景实体配置:机器人基座
|
||||||
|
"com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.05, 0.05)},
|
||||||
|
},
|
||||||
|
),
|
||||||
|
newton_mjwarp=None,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# # 定义一个重置机器人关节的事件项
|
||||||
|
# reset_robot_joints = EventTerm(
|
||||||
|
# # 指定事件处理函数为mdp.reset_joints_by_scale
|
||||||
|
# func=mdp.reset_joints_by_scale,
|
||||||
|
# # 设置事件模式为"reset"
|
||||||
|
# mode="reset",
|
||||||
|
# # 设置事件参数
|
||||||
|
# params={
|
||||||
|
# # 设置位置范围为(0.5, 1.5)
|
||||||
|
# "position_range": (0.5, 1.5),
|
||||||
|
# # 设置速度范围为(0.0, 0.0)
|
||||||
|
# "velocity_range": (0.0, 0.0),
|
||||||
|
# },
|
||||||
|
# )
|
||||||
|
|
||||||
|
# interval
|
||||||
|
# #创建一个名为push_robot的EventTerm对象,用于执行机器人推动操作
|
||||||
|
# push_robot = EventTerm(
|
||||||
|
# func=mdp.push_by_setting_velocity, # 指定执行函数为mdp中的push_by_setting_velocity
|
||||||
|
# mode="interval", # 设置执行模式为间隔模式
|
||||||
|
# interval_range_s=(10.0, 10.0), # 设置执行间隔时间范围为
|
||||||
|
# params={"velocity_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05)}}, # 设置推动速度范围,x和y方向的速度范围均为-0.5到0.5
|
||||||
|
# )
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TerminationsCfg(TerminationsCfg):
|
||||||
|
# 设置姿态终止条件,当机器人姿态超过限制角度时终止
|
||||||
|
fallen_orientation = DoneTerm(
|
||||||
|
func=mdp.bad_orientation,
|
||||||
|
params={"limit_angle": 0.7}, # 限制角度设为X弧度
|
||||||
|
)
|
||||||
|
# 设置高度终止条件,当机器人高度低于最小值时终止
|
||||||
|
fallen_height = DoneTerm(
|
||||||
|
func=mdp.root_height_below_minimum,
|
||||||
|
params={"minimum_height": 0.08}, # 最小高度设为X米
|
||||||
|
)
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RewardsCfg(RewardsCfg):
|
||||||
|
# 定义跟踪角速度z方向的指数奖励项配置
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_exp, # 奖励函数,用于跟踪角速度z方向的指数
|
||||||
|
weight=2.0, # 奖励项权重,控制该奖励项的重要性
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5}, # 命令名称和标准差参数
|
||||||
|
)
|
||||||
|
# 轨迹线性速度奖励项
|
||||||
|
track_lin_vel = RewTerm( # 定义一个奖励项,用于跟踪线性速度
|
||||||
|
func=mdp.track_lin_vel_xy_exp, # 使用velocity_mdp模块中的track_lin_vel_xy_exp函数作为奖励计算方法
|
||||||
|
weight=4.0,
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5},
|
||||||
|
)
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class Lv1Cfg(TwofootbotdemoEnvCfg):
|
||||||
|
"""Configuration for the first level of the TwoFootBot task."""
|
||||||
|
events: Lv1EventCfg = Lv1EventCfg()
|
||||||
|
terminations: TerminationsCfg = TerminationsCfg()
|
||||||
|
rewards: RewardsCfg = RewardsCfg()
|
||||||
|
|
||||||
|
decimation = 4
|
||||||
|
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
"""Post initialization."""
|
||||||
|
self.episode_length_s = 120.0 #time_out
|
||||||
|
self.viewer.eye = (3.0, -3.0, 0.1)
|
||||||
|
self.viewer.lookat = (0.0, 0.0, 0.0)
|
||||||
|
self.sim.dt = 1 / 200
|
||||||
|
self.sim.physics = PhysxCfg(gpu_max_rigid_patch_count=100 * 2**15)
|
||||||
|
|
@ -0,0 +1,40 @@
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab_tasks.utils import preset
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab_physx.physics import PhysxCfg
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv1 import Lv1Cfg
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv1 import Lv1EventCfg
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class Lv2EventCfg(Lv1EventCfg):
|
||||||
|
# interval
|
||||||
|
#创建一个名为push_robot的EventTerm对象,用于执行机器人推动操作
|
||||||
|
push_robot = EventTerm(
|
||||||
|
func=mdp.push_by_setting_velocity, # 指定执行函数为mdp中的push_by_setting_velocity
|
||||||
|
mode="interval", # 设置执行模式为间隔模式
|
||||||
|
interval_range_s=(10.0, 10.0), # 设置执行间隔时间范围为
|
||||||
|
params={"velocity_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05)}}, # 设置推动速度范围,x和y方向的速度范围均为-0.5到0.5
|
||||||
|
)
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class Lv2Cfg(Lv1Cfg):
|
||||||
|
"""Configuration for the first level of the TwoFootBot task."""
|
||||||
|
events: Lv2EventCfg = Lv2EventCfg()
|
||||||
|
decimation = 4
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
"""Post initialization."""
|
||||||
|
self.episode_length_s = 120.0 #time_out
|
||||||
|
self.viewer.eye = (3.0, -3.0, 0.1)
|
||||||
|
self.viewer.lookat = (0.0, 0.0, 0.0)
|
||||||
|
self.sim.dt = 1 / 200
|
||||||
|
self.sim.physics = PhysxCfg(gpu_max_rigid_patch_count=100 * 2**15)
|
||||||
|
|
@ -0,0 +1,118 @@
|
||||||
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import gymnasium as gym
|
||||||
|
|
||||||
|
from . import agents
|
||||||
|
|
||||||
|
##
|
||||||
|
# Register Gym environments.
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
'''gym.register(
|
||||||
|
id="Template-G1-Twofootbot-mlp-v0",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
gym.register(
|
||||||
|
id="Template-G1-Twofootbot-cnn-v0",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
gym.register(
|
||||||
|
id="Template-G1-Twofootbot-mlp-v0-Play",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg_PLAY",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
gym.register(
|
||||||
|
id="Template-G1-Twofootbot-cnn-v0-Play",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg_PLAY",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)'''
|
||||||
|
|
||||||
|
#CUDA_VISIBLE_DEVICES=1 python ... 可把所有计算放在cuda1,直接是由--device cuda:1只会把torch放在1而sim在0
|
||||||
|
#$ python scripts/rsl_rl/train.py --task Template-Twofootbot-mlp-v0
|
||||||
|
#robot 学会在理想情况下行走 v=0.2m/s
|
||||||
|
gym.register(
|
||||||
|
id="Template-Twofootbot-mlp-v0",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.Lv0:TwofootbotdemoEnvCfg",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# $ python scripts/rsl_rl/train.py \
|
||||||
|
# --task Template-Twofootbot-mlp-v1 \
|
||||||
|
# --headless \
|
||||||
|
# --resume \
|
||||||
|
# --load_run 2026-09-15_13-16-41-V0 \
|
||||||
|
# --checkpoint model_4999.pt --device cuda:0
|
||||||
|
#robot学会在变速情况下行走 v=0.2m/s--0.5m/s
|
||||||
|
gym.register(
|
||||||
|
id="Template-Twofootbot-mlp-v1",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.Lv1:Lv1Cfg",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
gym.register(
|
||||||
|
id="Template-Twofootbot-mlp-v2",
|
||||||
|
entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
disable_env_checker=True,
|
||||||
|
kwargs={
|
||||||
|
"env_cfg_entry_point": f"{__name__}.Lv2:Lv2Cfg",
|
||||||
|
"rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# # Lv2Cfg
|
||||||
|
# #加入随机的重量,之心,外力干扰。
|
||||||
|
# gym.register(
|
||||||
|
# id="Template-Twofootbot-mlp-v2",
|
||||||
|
# entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
# disable_env_checker=True,
|
||||||
|
# kwargs={
|
||||||
|
# "env_cfg_entry_point": f"{__name__}.Lv2:Lv2Cfg",
|
||||||
|
# "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
# },
|
||||||
|
# )
|
||||||
|
|
||||||
|
# # Lv3Cfg
|
||||||
|
# # 尝试去除base_lin_vel这个在机器人上面不太好拿到的信息
|
||||||
|
|
||||||
|
# gym.register(
|
||||||
|
# id="Template-Twofootbot-mlp-v3",
|
||||||
|
# entry_point="isaaclab.envs:ManagerBasedRLEnv",
|
||||||
|
# disable_env_checker=True,
|
||||||
|
# kwargs={
|
||||||
|
# "env_cfg_entry_point": f"{__name__}.Lv3:Lv3Cfg",
|
||||||
|
# "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
# },
|
||||||
|
# )
|
||||||
|
|
@ -0,0 +1,4 @@
|
||||||
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
from isaaclab_rl.rsl_rl import (
|
||||||
|
RslRlCNNModelCfg,
|
||||||
|
RslRlMLPModelCfg,
|
||||||
|
RslRlOnPolicyRunnerCfg,
|
||||||
|
RslRlPpoAlgorithmCfg,
|
||||||
|
RslRlRndCfg,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||||
|
num_steps_per_env = 24
|
||||||
|
max_iterations = 50000
|
||||||
|
save_interval = 1000
|
||||||
|
experiment_name = "cartpole_direct"
|
||||||
|
#obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]}
|
||||||
|
obs_groups = {"actor": ["policy"], "critic": ["policy"], "rnd_state": ["policy"]}
|
||||||
|
#obs_groups = {"actor": ["policy"], "critic": ["policy"]}
|
||||||
|
'''actor = RslRlCNNModelCfg(
|
||||||
|
hidden_dims=[512, 256, 256, 128],
|
||||||
|
activation="relu",
|
||||||
|
obs_normalization=False,
|
||||||
|
distribution_cfg=RslRlCNNModelCfg.GaussianDistributionCfg(init_std=1.0),
|
||||||
|
cnn_cfg=RslRlCNNModelCfg.CNNCfg(
|
||||||
|
output_channels=[16,16,16],
|
||||||
|
kernel_size=[8, 4, 3],
|
||||||
|
stride=[8,4,2],
|
||||||
|
activation="elu",
|
||||||
|
),
|
||||||
|
)'''
|
||||||
|
actor = RslRlMLPModelCfg(
|
||||||
|
hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
|
||||||
|
activation="elu",
|
||||||
|
obs_normalization=True,
|
||||||
|
distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0),
|
||||||
|
)
|
||||||
|
critic = RslRlMLPModelCfg(
|
||||||
|
hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
|
||||||
|
activation="elu",
|
||||||
|
obs_normalization=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
algorithm = RslRlPpoAlgorithmCfg(
|
||||||
|
value_loss_coef=1.0, #价值函数损失系数
|
||||||
|
use_clipped_value_loss=True, #是否使用裁剪值损失
|
||||||
|
clip_param=0.2, #裁剪参数
|
||||||
|
#clip_actions=1.0, #动作裁剪参数
|
||||||
|
entropy_coef=0.005, #熵系数,探索的奖励
|
||||||
|
num_learning_epochs=5, #学习的轮数
|
||||||
|
num_mini_batches=4, #小批量的数量
|
||||||
|
learning_rate=5e-4, #学习率
|
||||||
|
schedule="adaptive", #学习率调度器类型
|
||||||
|
gamma=0.99, #折扣因子
|
||||||
|
lam=0.95, #GAE的lambda参数
|
||||||
|
desired_kl=0.01, #期望的KL散度
|
||||||
|
max_grad_norm=1.0, #梯度裁剪的最大范数
|
||||||
|
# 创建随机强化学习配置对象
|
||||||
|
rnd_cfg=RslRlRndCfg(
|
||||||
|
weight=0.01, # 设置权重参数为0.01
|
||||||
|
reward_normalization=True, # 启用奖励归一化
|
||||||
|
state_normalization=True, # 启用状态归一化
|
||||||
|
learning_rate=1e-3, # 设置学习率为0.001
|
||||||
|
predictor_hidden_dims=[128, 64], # 设置预测器隐藏层维度为[128, 64]
|
||||||
|
target_hidden_dims=[128, 64], # 设置目标网络隐藏层维度为[128, 64]
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
from isaaclab_rl.rsl_rl import (
|
||||||
|
RslRlCNNModelCfg,
|
||||||
|
RslRlMLPModelCfg,
|
||||||
|
RslRlOnPolicyRunnerCfg,
|
||||||
|
RslRlPpoAlgorithmCfg,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PPORunnerCfg(RslRlOnPolicyRunnerCfg):
|
||||||
|
num_steps_per_env = 16
|
||||||
|
max_iterations = 15000
|
||||||
|
save_interval = 200
|
||||||
|
experiment_name = "cartpole_direct"
|
||||||
|
obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]}
|
||||||
|
actor = RslRlCNNModelCfg(
|
||||||
|
hidden_dims=[1024, 512, 512, 256, 256, 128],
|
||||||
|
activation="relu",
|
||||||
|
obs_normalization=False,
|
||||||
|
distribution_cfg=RslRlCNNModelCfg.GaussianDistributionCfg(init_std=1.0),
|
||||||
|
cnn_cfg=RslRlCNNModelCfg.CNNCfg(
|
||||||
|
output_channels=[16,16,16],
|
||||||
|
kernel_size=[8, 4, 3],
|
||||||
|
stride=[8,4,2],
|
||||||
|
activation="relu",
|
||||||
|
),
|
||||||
|
)
|
||||||
|
critic = RslRlMLPModelCfg(
|
||||||
|
hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128],
|
||||||
|
activation="relu",
|
||||||
|
obs_normalization=True,
|
||||||
|
)
|
||||||
|
|
||||||
|
algorithm = RslRlPpoAlgorithmCfg(
|
||||||
|
value_loss_coef=1.0, #价值函数损失系数
|
||||||
|
use_clipped_value_loss=True, #是否使用裁剪值损失
|
||||||
|
clip_param=0.2, #裁剪参数
|
||||||
|
entropy_coef=0.005, #熵系数,探索的奖励
|
||||||
|
num_learning_epochs=5, #学习的轮数
|
||||||
|
num_mini_batches=4, #小批量的数量
|
||||||
|
learning_rate=5.0e-4, #学习率
|
||||||
|
schedule="adaptive", #学习率调度器类型
|
||||||
|
gamma=0.99, #折扣因子
|
||||||
|
lam=0.95, #GAE的lambda参数
|
||||||
|
desired_kl=0.01, #期望的KL散度
|
||||||
|
max_grad_norm=1.0, #梯度裁剪的最大范数
|
||||||
|
)
|
||||||
|
|
@ -0,0 +1,186 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.twofootbot_cnn_env_cfg import (
|
||||||
|
RewardsCfg,
|
||||||
|
TwofootbotcnnEnvCfg,
|
||||||
|
)
|
||||||
|
|
||||||
|
##
|
||||||
|
# Pre-defined configs
|
||||||
|
##
|
||||||
|
from isaaclab_assets import G1_MINIMAL_CFG # isort: skip
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1Rewards(RewardsCfg):
|
||||||
|
|
||||||
|
"""
|
||||||
|
G1机器人的奖励函数配置类,继承自RewardsCfg
|
||||||
|
定义了各种奖励项及其权重,用于训练和评估G1机器人的行为
|
||||||
|
"""
|
||||||
|
# 终止惩罚:当机器人终止时给予的惩罚
|
||||||
|
termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0)
|
||||||
|
# 跟踪线性速度奖励:鼓励机器人跟踪期望的xy方向线性速度(在坐标系中)
|
||||||
|
track_lin_vel_xy_exp = RewTerm(
|
||||||
|
func=mdp.track_lin_vel_xy_yaw_frame_exp,
|
||||||
|
weight=1.0,
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5},
|
||||||
|
)
|
||||||
|
# 跟踪角速度奖励:鼓励机器人跟踪期望的z方向角速度(在世界坐标系中)
|
||||||
|
# 奖励项1:跟踪世界坐标系下的z轴角速度期望值
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5}
|
||||||
|
)
|
||||||
|
# 定义一个奖励项,用于评估双足机器人的脚部空中时间
|
||||||
|
#feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0)
|
||||||
|
feet_air_time = RewTerm(
|
||||||
|
func=mdp.feet_air_time_positive_biped, # 调用的函数,用于计算双足机器人脚部空中时间
|
||||||
|
weight=0.25, # 该奖励项的权重,表示在总奖励中的重要性
|
||||||
|
params={ # 函数所需的参数配置
|
||||||
|
"command_name": "base_velocity", # 命令名称,可能与基础速度控制相关
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置,指定监测接触力的脚部刚体
|
||||||
|
"threshold": 0.4, # 阈值,用于判断脚部是否接触地面
|
||||||
|
},
|
||||||
|
)
|
||||||
|
# 创建一个奖励项(RewTerm),用于计算脚部滑动惩罚
|
||||||
|
feet_slide = RewTerm(
|
||||||
|
# 指定使用的函数为mdp.feet_slide
|
||||||
|
func=mdp.feet_slide,
|
||||||
|
# 设置权重为-0.1,表示脚部滑动会带来负奖励
|
||||||
|
weight=-0.1,
|
||||||
|
# 配置参数,包括传感器和资产的设置
|
||||||
|
params={
|
||||||
|
# 配置接触力传感器,监测所有脚踝滚动链接的接触力
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
# 配置机器人实体,指定监测所有脚踝滚动链接
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 定义一个奖励终端项(Reward Terminal),用于评估关节位置限制的违反程度
|
||||||
|
dof_pos_limits = RewTerm(
|
||||||
|
# 指定评估函数为mdp.joint_pos_limits,该函数用于计算关节位置限制的违反程度
|
||||||
|
func=mdp.joint_pos_limits,
|
||||||
|
# 设置权重为-1.0,表示当关节位置限制被违反时,会给予负奖励
|
||||||
|
weight=-1.0,
|
||||||
|
#params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])},
|
||||||
|
)
|
||||||
|
# 定义髋关节偏差奖励项
|
||||||
|
joint_deviation_hip = RewTerm(
|
||||||
|
func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数
|
||||||
|
weight=-0.1, # 奖励权重,负值表示惩罚偏差
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数,指定机器人髋关节的名称模式
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1RoughEnvCfg(TwofootbotcnnEnvCfg):
|
||||||
|
"""
|
||||||
|
G1机器人在粗糙环境中的配置类,继承自LocomotionVelocityRoughEnvCfg
|
||||||
|
定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等
|
||||||
|
"""
|
||||||
|
rewards: G1Rewards = G1Rewards()
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
# 调用父类的__post_init__方法
|
||||||
|
super().__post_init__()
|
||||||
|
|
||||||
|
|
||||||
|
# 所有髋关节
|
||||||
|
hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"]
|
||||||
|
# ========== 膝关节 ==========
|
||||||
|
knee_joints = ["jlf2_link", "jrf2_link"]
|
||||||
|
|
||||||
|
# ========== 踝关节 ==========
|
||||||
|
ankle_joints = ["jlf3_link", "jrf3_link"]
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度的偏航成功阈值
|
||||||
|
self.commands.base_velocity.vel_yaw_success_threshold = 0.8
|
||||||
|
# 注释:场景中的机器人配置 - 已被注释掉
|
||||||
|
#self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
# 注释:场景中高度扫描器的路径配置 - 已被注释掉
|
||||||
|
#self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link"
|
||||||
|
|
||||||
|
# 初始化事件相关参数
|
||||||
|
self.events.add_base_mass = None
|
||||||
|
#elf.events.base_com = None
|
||||||
|
# 设置外部力矩事件参数,指定作用在机器人躯干链接上
|
||||||
|
self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link"
|
||||||
|
# 重置机器人关节的位置范围参数
|
||||||
|
self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置各种奖励函数的权重
|
||||||
|
self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0
|
||||||
|
self.rewards.undesired_contacts = None # 禁用不期望接触的奖励
|
||||||
|
self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0
|
||||||
|
self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005
|
||||||
|
self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7
|
||||||
|
# 设置自由度加速度L2惩罚的配置参数
|
||||||
|
self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg(
|
||||||
|
"robot", joint_names=hip_joints # 只考虑髋关节和膝关节
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# 设置自由度扭矩L2惩罚的权重和配置参数
|
||||||
|
self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7
|
||||||
|
self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg(
|
||||||
|
"robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度命令的范围
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0(禁止横向移动)
|
||||||
|
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置终止条件的配置参数
|
||||||
|
self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link"
|
||||||
|
# 终止条件基于躯干接触
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1RoughEnvCfg_PLAY(G1RoughEnvCfg):
|
||||||
|
def __post_init__(self):
|
||||||
|
# 调用父类的__post_init__方法
|
||||||
|
super().__post_init__()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置场景参数
|
||||||
|
self.scene.num_envs = 50 # 环境数量
|
||||||
|
self.scene.env_spacing = 2.5 # 环境间距
|
||||||
|
self.episode_length_s = 40.0 # 每集长度(秒)
|
||||||
|
self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制
|
||||||
|
# 如果地形生成器存在,则设置其参数
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数
|
||||||
|
self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度命令范围
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围(固定值1.0)
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围(固定值0.0)
|
||||||
|
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围(-1.0到1.0)
|
||||||
|
self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围(固定值0.0)
|
||||||
|
# 禁用策略观察值的损坏
|
||||||
|
self.observations.policy.enable_corruption = False
|
||||||
|
# 禁用外部力和推力事件
|
||||||
|
self.events.base_external_force_torque = None
|
||||||
|
self.events.push_robot = None
|
||||||
|
|
@ -0,0 +1,186 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
from TwoFootBot_isaaclab.tasks.manager_based.twofootbot.velocity_env_cfg import (
|
||||||
|
RewardsCfg,
|
||||||
|
LocomotionVelocityRoughEnvCfg,
|
||||||
|
)
|
||||||
|
|
||||||
|
##
|
||||||
|
# Pre-defined configs
|
||||||
|
##
|
||||||
|
from isaaclab_assets import G1_MINIMAL_CFG # isort: skip
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1Rewards(RewardsCfg):
|
||||||
|
|
||||||
|
"""
|
||||||
|
G1机器人的奖励函数配置类,继承自RewardsCfg
|
||||||
|
定义了各种奖励项及其权重,用于训练和评估G1机器人的行为
|
||||||
|
"""
|
||||||
|
# 终止惩罚:当机器人终止时给予的惩罚
|
||||||
|
termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0)
|
||||||
|
# 跟踪线性速度奖励:鼓励机器人跟踪期望的xy方向线性速度(在坐标系中)
|
||||||
|
track_lin_vel_xy_exp = RewTerm(
|
||||||
|
func=mdp.track_lin_vel_xy_yaw_frame_exp,
|
||||||
|
weight=1.0,
|
||||||
|
params={"command_name": "base_velocity", "std": 0.5},
|
||||||
|
)
|
||||||
|
# 跟踪角速度奖励:鼓励机器人跟踪期望的z方向角速度(在世界坐标系中)
|
||||||
|
# 奖励项1:跟踪世界坐标系下的z轴角速度期望值
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5}
|
||||||
|
)
|
||||||
|
# 定义一个奖励项,用于评估双足机器人的脚部空中时间
|
||||||
|
#feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0)
|
||||||
|
feet_air_time = RewTerm(
|
||||||
|
func=mdp.feet_air_time_positive_biped, # 调用的函数,用于计算双足机器人脚部空中时间
|
||||||
|
weight=0.25, # 该奖励项的权重,表示在总奖励中的重要性
|
||||||
|
params={ # 函数所需的参数配置
|
||||||
|
"command_name": "base_velocity", # 命令名称,可能与基础速度控制相关
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置,指定监测接触力的脚部刚体
|
||||||
|
"threshold": 0.4, # 阈值,用于判断脚部是否接触地面
|
||||||
|
},
|
||||||
|
)
|
||||||
|
# 创建一个奖励项(RewTerm),用于计算脚部滑动惩罚
|
||||||
|
feet_slide = RewTerm(
|
||||||
|
# 指定使用的函数为mdp.feet_slide
|
||||||
|
func=mdp.feet_slide,
|
||||||
|
# 设置权重为-0.1,表示脚部滑动会带来负奖励
|
||||||
|
weight=-0.1,
|
||||||
|
# 配置参数,包括传感器和资产的设置
|
||||||
|
params={
|
||||||
|
# 配置接触力传感器,监测所有脚踝滚动链接的接触力
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
# 配置机器人实体,指定监测所有脚踝滚动链接
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 定义一个奖励终端项(Reward Terminal),用于评估关节位置限制的违反程度
|
||||||
|
dof_pos_limits = RewTerm(
|
||||||
|
# 指定评估函数为mdp.joint_pos_limits,该函数用于计算关节位置限制的违反程度
|
||||||
|
func=mdp.joint_pos_limits,
|
||||||
|
# 设置权重为-1.0,表示当关节位置限制被违反时,会给予负奖励
|
||||||
|
weight=-1.0,
|
||||||
|
#params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])},
|
||||||
|
)
|
||||||
|
# 定义髋关节偏差奖励项
|
||||||
|
joint_deviation_hip = RewTerm(
|
||||||
|
func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数
|
||||||
|
weight=-0.1, # 奖励权重,负值表示惩罚偏差
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数,指定机器人髋关节的名称模式
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg):
|
||||||
|
"""
|
||||||
|
G1机器人在粗糙环境中的配置类,继承自LocomotionVelocityRoughEnvCfg
|
||||||
|
定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等
|
||||||
|
"""
|
||||||
|
rewards: G1Rewards = G1Rewards()
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
# 调用父类的__post_init__方法
|
||||||
|
super().__post_init__()
|
||||||
|
|
||||||
|
|
||||||
|
# 所有髋关节
|
||||||
|
hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"]
|
||||||
|
# ========== 膝关节 ==========
|
||||||
|
knee_joints = ["jlf2_link", "jrf2_link"]
|
||||||
|
|
||||||
|
# ========== 踝关节 ==========
|
||||||
|
ankle_joints = ["jlf3_link", "jrf3_link"]
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度的偏航成功阈值
|
||||||
|
self.commands.base_velocity.vel_yaw_success_threshold = 0.8
|
||||||
|
# 注释:场景中的机器人配置 - 已被注释掉
|
||||||
|
#self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
# 注释:场景中高度扫描器的路径配置 - 已被注释掉
|
||||||
|
#self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link"
|
||||||
|
|
||||||
|
# 初始化事件相关参数
|
||||||
|
self.events.add_base_mass = None
|
||||||
|
#elf.events.base_com = None
|
||||||
|
# 设置外部力矩事件参数,指定作用在机器人躯干链接上
|
||||||
|
self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link"
|
||||||
|
# 重置机器人关节的位置范围参数
|
||||||
|
self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置各种奖励函数的权重
|
||||||
|
self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0
|
||||||
|
self.rewards.undesired_contacts = None # 禁用不期望接触的奖励
|
||||||
|
self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0
|
||||||
|
self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005
|
||||||
|
self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7
|
||||||
|
# 设置自由度加速度L2惩罚的配置参数
|
||||||
|
self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg(
|
||||||
|
"robot", joint_names=hip_joints # 只考虑髋关节和膝关节
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
# 设置自由度扭矩L2惩罚的权重和配置参数
|
||||||
|
self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7
|
||||||
|
self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg(
|
||||||
|
"robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度命令的范围
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0(禁止横向移动)
|
||||||
|
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置终止条件的配置参数
|
||||||
|
self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link"
|
||||||
|
# 终止条件基于躯干接触
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class G1RoughEnvCfg_PLAY(G1RoughEnvCfg):
|
||||||
|
def __post_init__(self):
|
||||||
|
# 调用父类的__post_init__方法
|
||||||
|
super().__post_init__()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置场景参数
|
||||||
|
self.scene.num_envs = 50 # 环境数量
|
||||||
|
self.scene.env_spacing = 2.5 # 环境间距
|
||||||
|
self.episode_length_s = 40.0 # 每集长度(秒)
|
||||||
|
self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制
|
||||||
|
# 如果地形生成器存在,则设置其参数
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数
|
||||||
|
self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# 设置基础速度命令范围
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围(固定值1.0)
|
||||||
|
self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围(固定值0.0)
|
||||||
|
self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围(-1.0到1.0)
|
||||||
|
self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围(固定值0.0)
|
||||||
|
# 禁用策略观察值的损坏
|
||||||
|
self.observations.policy.enable_corruption = False
|
||||||
|
# 禁用外部力和推力事件
|
||||||
|
self.events.base_external_force_torque = None
|
||||||
|
self.events.push_robot = None
|
||||||
|
|
@ -0,0 +1,10 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
"""This sub-module contains the functions that are specific to the environment."""
|
||||||
|
|
||||||
|
from isaaclab.utils.module import lazy_export
|
||||||
|
|
||||||
|
lazy_export()
|
||||||
|
|
@ -0,0 +1,15 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
__all__ = [
|
||||||
|
"joint_pos_target_l2",
|
||||||
|
"root_low_speed_for_duration",
|
||||||
|
]
|
||||||
|
|
||||||
|
# Forward stable MDP terms lazily, then override with environment-specific terms below.
|
||||||
|
from isaaclab.envs.mdp import * # noqa: F401, F403
|
||||||
|
|
||||||
|
from .rewards import joint_pos_target_l2
|
||||||
|
from .terminations import root_low_speed_for_duration
|
||||||
|
|
@ -0,0 +1,27 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
|
import torch
|
||||||
|
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.utils.math import wrap_to_pi
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
|
from isaaclab.assets import Articulation
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnv
|
||||||
|
|
||||||
|
|
||||||
|
def joint_pos_target_l2(env: ManagerBasedRLEnv, target: float, asset_cfg: SceneEntityCfg) -> torch.Tensor:
|
||||||
|
"""Penalize joint position deviation from a target value."""
|
||||||
|
# extract the used quantities (to enable type-hinting)
|
||||||
|
asset: Articulation = env.scene[asset_cfg.name]
|
||||||
|
# wrap the joint positions to (-pi, pi)
|
||||||
|
joint_pos = wrap_to_pi(asset.data.joint_pos[:, asset_cfg.joint_ids])
|
||||||
|
# compute the reward
|
||||||
|
return torch.sum(torch.square(joint_pos - target), dim=1)
|
||||||
|
|
@ -0,0 +1,56 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from typing import TYPE_CHECKING
|
||||||
|
|
||||||
|
import torch
|
||||||
|
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
|
||||||
|
if TYPE_CHECKING:
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnv
|
||||||
|
|
||||||
|
|
||||||
|
def root_low_speed_for_duration(
|
||||||
|
env: ManagerBasedRLEnv,
|
||||||
|
threshold: float,
|
||||||
|
duration: float,
|
||||||
|
asset_cfg: SceneEntityCfg,
|
||||||
|
) -> torch.Tensor:
|
||||||
|
"""Terminate environments whose robot stays below a planar speed threshold for a duration."""
|
||||||
|
if threshold < 0.0:
|
||||||
|
raise ValueError(f"Speed threshold must be non-negative, got {threshold}.")
|
||||||
|
if duration <= 0.0:
|
||||||
|
raise ValueError(f"Duration must be positive, got {duration}.")
|
||||||
|
|
||||||
|
asset = env.scene[asset_cfg.name]
|
||||||
|
speed = torch.linalg.vector_norm(asset.data.root_lin_vel_w[:, :2], dim=1)
|
||||||
|
episode_length = env.episode_length_buf
|
||||||
|
|
||||||
|
elapsed = getattr(env, "_root_low_speed_elapsed", None)
|
||||||
|
last_episode_length = getattr(env, "_root_low_speed_last_episode_length", None)
|
||||||
|
if elapsed is None:
|
||||||
|
elapsed = torch.zeros_like(speed)
|
||||||
|
last_episode_length = episode_length.clone()
|
||||||
|
setattr(env, "_root_low_speed_elapsed", elapsed)
|
||||||
|
elif not isinstance(last_episode_length, torch.Tensor):
|
||||||
|
raise RuntimeError("Low-speed termination episode state is missing or invalid.")
|
||||||
|
|
||||||
|
if elapsed.shape != speed.shape or elapsed.device != speed.device:
|
||||||
|
raise RuntimeError("Low-speed termination state does not match the environment.")
|
||||||
|
if last_episode_length.shape != episode_length.shape or last_episode_length.device != episode_length.device:
|
||||||
|
raise RuntimeError("Low-speed termination episode state does not match the environment.")
|
||||||
|
|
||||||
|
elapsed[episode_length <= last_episode_length] = 0.0
|
||||||
|
below_threshold = speed < threshold
|
||||||
|
elapsed[below_threshold] += env.step_dt
|
||||||
|
elapsed[~below_threshold] = 0.0
|
||||||
|
|
||||||
|
terminated = elapsed > duration
|
||||||
|
elapsed[terminated] = 0.0
|
||||||
|
setattr(env, "_root_low_speed_last_episode_length", episode_length.clone())
|
||||||
|
return terminated
|
||||||
|
|
@ -0,0 +1,78 @@
|
||||||
|
"""自定义速度命令:直走 -> 原地转向 -> 直走 循环。
|
||||||
|
|
||||||
|
使用 heading_command=True,转向由 heading 目标平滑驱动。
|
||||||
|
"""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
import torch
|
||||||
|
from collections.abc import Sequence
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
|
||||||
|
class SequenceHeadingWalkTurnCommand(mdp.UniformVelocityCommand):
|
||||||
|
"""直走 -> 原地转向 -> 直走 循环,转向通过 heading 目标实现。
|
||||||
|
|
||||||
|
阶段循环:0(直走) -> 1(转向) -> 2(直走) -> 0(直走) -> ...
|
||||||
|
每个阶段持续 resampling_time_range 指定的时间。
|
||||||
|
|
||||||
|
与直接写 vel_command_b[2] 的区别:
|
||||||
|
这里只设置 heading_target,由基类的 _update_command 根据 heading 误差
|
||||||
|
实时计算角速度,转向更平滑。
|
||||||
|
"""
|
||||||
|
|
||||||
|
cfg: "SequenceHeadingWalkTurnCommandCfg"
|
||||||
|
|
||||||
|
def __init__(self, cfg: "SequenceHeadingWalkTurnCommandCfg", env):
|
||||||
|
super().__init__(cfg, env)
|
||||||
|
# 当前阶段:0=直走,1=转向,2=直走
|
||||||
|
self._phase = torch.zeros(self.num_envs, dtype=torch.long, device=self.device)
|
||||||
|
|
||||||
|
def _resample_command(self, env_ids: Sequence[int]):
|
||||||
|
# 先让基类采样默认速度 / heading,避免遗漏默认行为
|
||||||
|
super()._resample_command(env_ids)
|
||||||
|
|
||||||
|
env_ids_t = torch.as_tensor(env_ids, dtype=torch.long, device=self.device)
|
||||||
|
phase = self._phase[env_ids_t]
|
||||||
|
num = len(env_ids_t)
|
||||||
|
|
||||||
|
# 前进速度:在配置范围内随机
|
||||||
|
vx_min, vx_max = self.cfg.ranges.lin_vel_x
|
||||||
|
vx = torch.empty(num, device=self.device).uniform_(vx_min, vx_max)
|
||||||
|
|
||||||
|
# 当前世界朝向
|
||||||
|
current_heading = self.robot.data.heading_w[env_ids_t]
|
||||||
|
|
||||||
|
# 阶段 0 和 2:直走,heading 锁定当前朝向
|
||||||
|
mask_walk = (phase == 0) | (phase == 2)
|
||||||
|
if mask_walk.any():
|
||||||
|
ids = env_ids_t[mask_walk]
|
||||||
|
self.vel_command_b[ids, 0] = vx[mask_walk]
|
||||||
|
self.vel_command_b[ids, 1] = 0.0
|
||||||
|
# vel_command_b[2] 由基类 _update_command 根据 heading 误差计算
|
||||||
|
self.heading_target[ids] = current_heading[mask_walk]
|
||||||
|
|
||||||
|
# 阶段 1:原地转向,heading 目标 = 当前朝向 + 90°(左转)
|
||||||
|
mask_turn = phase == 1
|
||||||
|
if mask_turn.any():
|
||||||
|
ids = env_ids_t[mask_turn]
|
||||||
|
self.vel_command_b[ids, 0] = 0.0
|
||||||
|
self.vel_command_b[ids, 1] = 0.0
|
||||||
|
self.heading_target[ids] = current_heading[mask_turn] + math.pi / 2.0
|
||||||
|
|
||||||
|
# 推进 phase
|
||||||
|
self._phase[env_ids_t] = (self._phase[env_ids_t] + 1) % 3
|
||||||
|
|
||||||
|
def _update_command(self):
|
||||||
|
# 基类会处理 heading_command=True 时的角速度计算
|
||||||
|
super()._update_command()
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class SequenceHeadingWalkTurnCommandCfg(mdp.UniformVelocityCommandCfg):
|
||||||
|
"""序列 heading 命令的配置类。"""
|
||||||
|
|
||||||
|
class_type: type = SequenceHeadingWalkTurnCommand
|
||||||
|
|
@ -0,0 +1,703 @@
|
||||||
|
'''# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import math
|
||||||
|
|
||||||
|
import isaaclab.sim as sim_utils
|
||||||
|
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab.managers import ObservationGroupCfg as ObsGroup
|
||||||
|
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab.scene import InteractiveSceneCfg
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
|
||||||
|
from . import mdp
|
||||||
|
|
||||||
|
##
|
||||||
|
# Pre-defined configs
|
||||||
|
##
|
||||||
|
|
||||||
|
from isaaclab_assets.robots.cartpole import CARTPOLE_CFG # isort:skip
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Scene definition
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TwofootbotSceneCfg(InteractiveSceneCfg):
|
||||||
|
"""Configuration for a cart-pole scene."""
|
||||||
|
|
||||||
|
# ground plane
|
||||||
|
ground = AssetBaseCfg(
|
||||||
|
prim_path="/World/ground",
|
||||||
|
spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)),
|
||||||
|
)
|
||||||
|
|
||||||
|
# robot
|
||||||
|
robot: ArticulationCfg = CARTPOLE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
|
||||||
|
# lights
|
||||||
|
dome_light = AssetBaseCfg(
|
||||||
|
prim_path="/World/DomeLight",
|
||||||
|
spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# MDP settings
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ActionsCfg:
|
||||||
|
"""Action specifications for the MDP."""
|
||||||
|
|
||||||
|
joint_effort = mdp.JointEffortActionCfg(asset_name="robot", joint_names=["slider_to_cart"], scale=100.0)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ObservationsCfg:
|
||||||
|
"""Observation specifications for the MDP."""
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PolicyCfg(ObsGroup):
|
||||||
|
"""Observations for policy group."""
|
||||||
|
|
||||||
|
# observation terms (order preserved)
|
||||||
|
joint_pos_rel = ObsTerm(func=mdp.joint_pos_rel)
|
||||||
|
joint_vel_rel = ObsTerm(func=mdp.joint_vel_rel)
|
||||||
|
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
self.enable_corruption = False
|
||||||
|
self.concatenate_terms = True
|
||||||
|
|
||||||
|
# observation groups
|
||||||
|
policy: PolicyCfg = PolicyCfg()
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class EventCfg:
|
||||||
|
"""Configuration for events."""
|
||||||
|
|
||||||
|
# reset
|
||||||
|
reset_cart_position = EventTerm(
|
||||||
|
func=mdp.reset_joints_by_offset,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]),
|
||||||
|
"position_range": (-1.0, 1.0),
|
||||||
|
"velocity_range": (-0.5, 0.5),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
reset_pole_position = EventTerm(
|
||||||
|
func=mdp.reset_joints_by_offset,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"]),
|
||||||
|
"position_range": (-0.25 * math.pi, 0.25 * math.pi),
|
||||||
|
"velocity_range": (-0.25 * math.pi, 0.25 * math.pi),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RewardsCfg:
|
||||||
|
"""Reward terms for the MDP."""
|
||||||
|
|
||||||
|
# (1) Constant running reward
|
||||||
|
alive = RewTerm(func=mdp.is_alive, weight=1.0)
|
||||||
|
# (2) Failure penalty
|
||||||
|
terminating = RewTerm(func=mdp.is_terminated, weight=-2.0)
|
||||||
|
# (3) Primary task: keep pole upright
|
||||||
|
pole_pos = RewTerm(
|
||||||
|
func=mdp.joint_pos_target_l2,
|
||||||
|
weight=-1.0,
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"]), "target": 0.0},
|
||||||
|
)
|
||||||
|
# (4) Shaping tasks: lower cart velocity
|
||||||
|
cart_vel = RewTerm(
|
||||||
|
func=mdp.joint_vel_l1,
|
||||||
|
weight=-0.01,
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"])},
|
||||||
|
)
|
||||||
|
# (5) Shaping tasks: lower pole angular velocity
|
||||||
|
pole_vel = RewTerm(
|
||||||
|
func=mdp.joint_vel_l1,
|
||||||
|
weight=-0.005,
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["cart_to_pole"])},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TerminationsCfg:
|
||||||
|
"""Termination terms for the MDP."""
|
||||||
|
|
||||||
|
# (1) Time out
|
||||||
|
time_out = DoneTerm(func=mdp.time_out, time_out=True)
|
||||||
|
# (2) Cart out of bounds
|
||||||
|
cart_out_of_bounds = DoneTerm(
|
||||||
|
func=mdp.joint_pos_out_of_manual_limit,
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot", joint_names=["slider_to_cart"]), "bounds": (-3.0, 3.0)},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Environment configuration
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TwofootbotEnvCfg(ManagerBasedRLEnvCfg):
|
||||||
|
# Scene settings
|
||||||
|
scene: TwofootbotSceneCfg = TwofootbotSceneCfg(num_envs=4096, env_spacing=4.0)
|
||||||
|
# Basic settings
|
||||||
|
observations: ObservationsCfg = ObservationsCfg()
|
||||||
|
actions: ActionsCfg = ActionsCfg()
|
||||||
|
events: EventCfg = EventCfg()
|
||||||
|
# MDP settings
|
||||||
|
rewards: RewardsCfg = RewardsCfg()
|
||||||
|
terminations: TerminationsCfg = TerminationsCfg()
|
||||||
|
|
||||||
|
# Post initialization
|
||||||
|
def __post_init__(self) -> None:
|
||||||
|
"""Post initialization."""
|
||||||
|
# general settings
|
||||||
|
self.decimation = 2
|
||||||
|
self.episode_length_s = 5
|
||||||
|
# viewer settings
|
||||||
|
self.viewer.eye = (8.0, 0.0, 5.0)
|
||||||
|
# simulation settings
|
||||||
|
self.sim.dt = 1 / 120
|
||||||
|
self.sim.render_interval = self.decimation'''
|
||||||
|
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import MISSING
|
||||||
|
from pathlib import Path
|
||||||
|
import torch
|
||||||
|
from isaaclab.sensors import ContactSensorCfg
|
||||||
|
from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg
|
||||||
|
from isaaclab_newton.sensors import ContactSensorCfg as NewtonContactSensorCfg
|
||||||
|
from isaaclab_ovphysx.sensors import ContactSensorCfg as OvPhysXContactSensorCfg
|
||||||
|
from isaaclab_physx.physics import PhysxCfg
|
||||||
|
from isaaclab_physx.sensors import ContactSensorCfg as PhysXContactSensorCfg
|
||||||
|
|
||||||
|
from isaaclab.actuators import ImplicitActuatorCfg
|
||||||
|
|
||||||
|
import isaaclab.sim as sim_utils
|
||||||
|
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.managers import CurriculumTermCfg as CurrTerm
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab.managers import ObservationGroupCfg as ObsGroup
|
||||||
|
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab.scene import InteractiveSceneCfg
|
||||||
|
from isaaclab.sensors import RayCasterCfg, patterns
|
||||||
|
from isaaclab.sim import SimulationCfg
|
||||||
|
from isaaclab.terrains import TerrainImporterCfg
|
||||||
|
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
from isaaclab.utils.noise import UniformNoiseCfg as Unoise
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
#from isaaclab.managers import mdp as base_mdp
|
||||||
|
from isaaclab_tasks.utils import PresetCfg, preset
|
||||||
|
|
||||||
|
from isaaclab.sensors import CameraCfg
|
||||||
|
from isaaclab.sensors import TiledCameraCfg
|
||||||
|
from isaaclab.sim import PinholeCameraCfg
|
||||||
|
|
||||||
|
JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。
|
||||||
|
"jlf0_link",
|
||||||
|
"jlf1_link",
|
||||||
|
"jlf2_link",
|
||||||
|
"jlf3_link",
|
||||||
|
"jrf0_link",
|
||||||
|
"jrf1_link",
|
||||||
|
"jrf2_link",
|
||||||
|
"jrf3_link",
|
||||||
|
]
|
||||||
|
BOT_PACKAGE_PATH = Path(__file__).resolve().parents[5] / "bot/2foot" # 获取机器人 ROS 包目录。
|
||||||
|
URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。
|
||||||
|
|
||||||
|
def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs):
|
||||||
|
"""Spawn the URDF and enable contact reporting on every rigid link."""
|
||||||
|
from pxr import Usd, UsdPhysics
|
||||||
|
from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf
|
||||||
|
from isaaclab.sim.utils import safe_set_attribute_on_usd_prim
|
||||||
|
|
||||||
|
spawn_cfg = cfg.replace(activate_contact_sensors=False)
|
||||||
|
prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs)
|
||||||
|
for child_prim in Usd.PrimRange(prim):
|
||||||
|
if not child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||||
|
continue
|
||||||
|
applied_schemas = child_prim.GetAppliedSchemas()
|
||||||
|
if "PhysxRigidBodyAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
|
||||||
|
if "PhysxContactReportAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxContactReportAPI")
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxContactReport:threshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
return prim
|
||||||
|
|
||||||
|
TOFOOTBOT_CFG = ArticulationCfg(
|
||||||
|
spawn=sim_utils.UrdfFileCfg(
|
||||||
|
func=spawn_twofootbot_from_urdf,
|
||||||
|
asset_path=str(URDF_PATH),
|
||||||
|
fix_base=False,
|
||||||
|
activate_contact_sensors=False,
|
||||||
|
ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}],
|
||||||
|
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
|
||||||
|
drive_type="force",
|
||||||
|
target_type="position",
|
||||||
|
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(stiffness=2.0, damping=0.08),
|
||||||
|
),
|
||||||
|
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||||
|
disable_gravity=False, # 不启用重力。
|
||||||
|
max_linear_velocity=100.0,
|
||||||
|
max_angular_velocity=100.0,
|
||||||
|
max_depenetration_velocity=5.0,
|
||||||
|
),
|
||||||
|
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||||
|
enabled_self_collisions=False, # # 启用自碰撞检测。
|
||||||
|
solver_position_iteration_count=8,
|
||||||
|
solver_velocity_iteration_count=2,
|
||||||
|
),
|
||||||
|
),
|
||||||
|
init_state=ArticulationCfg.InitialStateCfg( # 初始状态配置。单位: 弧度
|
||||||
|
pos=(0.0, 0.0, 0.18),
|
||||||
|
joint_pos={"jlf0_link": 0.0, "jlf1_link": 0.0, "jlf2_link": 0.0, "jlf3_link": 0.0,
|
||||||
|
"jrf0_link": 0.0, "jrf1_link": 0.0, "jrf2_link": 0.0, "jrf3_link": 0.0},
|
||||||
|
joint_vel={".*": 0.0},
|
||||||
|
),
|
||||||
|
actuators={ # 动力配置。
|
||||||
|
"legs": ImplicitActuatorCfg(
|
||||||
|
joint_names_expr=JOINT_NAMES, # 控制所有腿的关节。
|
||||||
|
effort_limit_sim=10, # 力矩限制。单位:牛顿米。
|
||||||
|
velocity_limit_sim=8.0554, # 速度限制 (rad/s)
|
||||||
|
stiffness=2.0, # 刚度。单位:牛顿/米。
|
||||||
|
damping=0.08, # 阻尼。单位:牛顿秒/米。
|
||||||
|
)
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
##
|
||||||
|
# Physics presets
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RoughPhysicsCfg(PresetCfg):
|
||||||
|
"""Shared physics preset for all rough-terrain locomotion envs."""
|
||||||
|
|
||||||
|
default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15)
|
||||||
|
newton_mjwarp = NewtonCfg(
|
||||||
|
solver_cfg=MJWarpSolverCfg(
|
||||||
|
njmax=200,
|
||||||
|
nconmax=100,
|
||||||
|
cone="pyramidal",
|
||||||
|
impratio=1.0,
|
||||||
|
integrator="implicitfast",
|
||||||
|
use_mujoco_contacts=False,
|
||||||
|
),
|
||||||
|
collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000),
|
||||||
|
num_substeps=1,
|
||||||
|
debug_mode=False,
|
||||||
|
# 1 cm shape margin is the single most important Newton setting for rough
|
||||||
|
# terrain — without it, non-Anymal-D robots fail to learn stable contact
|
||||||
|
# on triangle-mesh terrain. See isaaclab_newton 0.5.22 changelog.
|
||||||
|
default_shape_cfg=NewtonShapeCfg(margin=0.01),
|
||||||
|
)
|
||||||
|
physx = default
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Scene definition
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class VelocityEnvContactSensorCfg(PresetCfg):
|
||||||
|
default = PhysXContactSensorCfg(
|
||||||
|
prim_path="{ENV_REGEX_NS}/Robot/.*",
|
||||||
|
history_length=3,
|
||||||
|
track_air_time=True)
|
||||||
|
newton_mjwarp = NewtonContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||||
|
physx = default
|
||||||
|
ovphysx = OvPhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class MySceneCfg(InteractiveSceneCfg):
|
||||||
|
head_camera = TiledCameraCfg(
|
||||||
|
prim_path="{ENV_REGEX_NS}/Robot/head_link/camera", # 摄像头在场景中的路径
|
||||||
|
update_period=0.1, # 数据更新周期 (秒)
|
||||||
|
height=480, # 图像高度
|
||||||
|
width=640, # 图像宽度
|
||||||
|
data_types=["rgb"], # 获取RGB图像
|
||||||
|
spawn=PinholeCameraCfg( # 相机内参配置
|
||||||
|
focal_length=24.0, # 焦距
|
||||||
|
focus_distance=400.0, # 对焦距离
|
||||||
|
horizontal_aperture=20.955, # 水平孔径
|
||||||
|
clipping_range=(0.1, 1.0e5), # 裁剪范围
|
||||||
|
),
|
||||||
|
offset=CameraCfg.OffsetCfg( # 摄像头相对于挂载点的偏移
|
||||||
|
pos=(0.0, 0.0, 0.09), # 位置偏移 (x, y, z)
|
||||||
|
rot=(1.0, 0.0, 0.0, 0.0), # 旋转 (w, x, y, z)
|
||||||
|
convention="world", # 坐标约定
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
"""Configuration for the terrain scene with a legged robot."""
|
||||||
|
|
||||||
|
# ground terrain
|
||||||
|
terrain = TerrainImporterCfg(
|
||||||
|
prim_path="/World/ground",
|
||||||
|
terrain_type="plane",
|
||||||
|
collision_group=-1,
|
||||||
|
physics_material=sim_utils.RigidBodyMaterialCfg(
|
||||||
|
friction_combine_mode="multiply",
|
||||||
|
restitution_combine_mode="multiply",
|
||||||
|
static_friction=1.0,
|
||||||
|
dynamic_friction=1.0,
|
||||||
|
),
|
||||||
|
visual_material=sim_utils.MdlFileCfg(
|
||||||
|
mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl",
|
||||||
|
project_uvw=True,
|
||||||
|
texture_scale=(0.25, 0.25),
|
||||||
|
),
|
||||||
|
debug_vis=False,
|
||||||
|
)
|
||||||
|
# robots
|
||||||
|
robot: ArticulationCfg = TOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
# sensors
|
||||||
|
height_scanner = None
|
||||||
|
'''height_scanner = RayCasterCfg(
|
||||||
|
prim_path="{ENV_REGEX_NS}/Robot/head_link",
|
||||||
|
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
|
||||||
|
ray_alignment="yaw",
|
||||||
|
pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[0.40, 0.25]),
|
||||||
|
debug_vis=False,
|
||||||
|
mesh_prim_paths=["/World/ground"],
|
||||||
|
)'''
|
||||||
|
contact_forces = VelocityEnvContactSensorCfg()
|
||||||
|
|
||||||
|
# lights
|
||||||
|
sky_light = AssetBaseCfg(
|
||||||
|
prim_path="/World/skyLight",
|
||||||
|
spawn=sim_utils.DomeLightCfg(
|
||||||
|
intensity=750.0,
|
||||||
|
texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr",
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# MDP settings
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CommandsCfg:
|
||||||
|
"""Command specifications for the MDP."""
|
||||||
|
|
||||||
|
base_velocity = mdp.UniformVelocityCommandCfg(
|
||||||
|
asset_name="robot",
|
||||||
|
resampling_time_range=(10.0, 10.0),
|
||||||
|
rel_standing_envs=0.02,
|
||||||
|
rel_heading_envs=1.0,
|
||||||
|
heading_command=True,
|
||||||
|
heading_control_stiffness=0.5,
|
||||||
|
debug_vis=True,
|
||||||
|
ranges=mdp.UniformVelocityCommandCfg.Ranges(
|
||||||
|
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi)
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ActionsCfg:
|
||||||
|
"""Action specifications for the MDP."""
|
||||||
|
|
||||||
|
joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True)
|
||||||
|
|
||||||
|
|
||||||
|
def camera_rgb(env, sensor_cfg: SceneEntityCfg):
|
||||||
|
"""Return RGB images in the channel-first format expected by CNN policies."""
|
||||||
|
img = env.scene.sensors[sensor_cfg.name].data.output["rgb"] # shape (num_envs, H, W, 3)
|
||||||
|
return img.permute(0, 3, 1, 2).float()
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ObservationsCfg:
|
||||||
|
"""Observation specifications for the MDP."""
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PolicyCfg(ObsGroup):
|
||||||
|
"""Observations for policy group."""
|
||||||
|
#添加摄像头观测
|
||||||
|
# observation terms (order preserved)
|
||||||
|
base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) # 添加线速度观测
|
||||||
|
base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2)) # 添加角速度观测
|
||||||
|
projected_gravity = ObsTerm( # 添加重力方向观测
|
||||||
|
func=mdp.projected_gravity,
|
||||||
|
noise=Unoise(n_min=-0.05, n_max=0.05),
|
||||||
|
)
|
||||||
|
velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) # 添加速度命令观测
|
||||||
|
joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) # 添加关节位置观测
|
||||||
|
joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) # 添加关节速度观测
|
||||||
|
actions = ObsTerm(func=mdp.last_action) # 添加动作观测
|
||||||
|
''' height_scan = ObsTerm(
|
||||||
|
func=mdp.height_scan,
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("height_scanner")},
|
||||||
|
noise=Unoise(n_min=-0.1, n_max=0.1),
|
||||||
|
clip=(-1.0, 1.0),
|
||||||
|
)
|
||||||
|
'''
|
||||||
|
def __post_init__(self):
|
||||||
|
self.enable_corruption = True # 添加噪声
|
||||||
|
self.concatenate_terms = True # 添加观测
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CameraCfg(ObsGroup):
|
||||||
|
"""添加摄像头观测"""
|
||||||
|
|
||||||
|
camera_rgb = ObsTerm(
|
||||||
|
func=camera_rgb,
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("head_camera")},
|
||||||
|
noise=Unoise(n_min=-0.1, n_max=0.1),
|
||||||
|
clip=(-1.0, 1.0),
|
||||||
|
)
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
self.enable_corruption = True
|
||||||
|
self.concatenate_terms = True
|
||||||
|
|
||||||
|
# observation groups
|
||||||
|
policy: PolicyCfg = PolicyCfg()
|
||||||
|
camera: CameraCfg = CameraCfg()
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class EventsCfg:
|
||||||
|
"""Configuration for events."""
|
||||||
|
|
||||||
|
# startup
|
||||||
|
physics_material = EventTerm(
|
||||||
|
func=mdp.randomize_rigid_body_material,
|
||||||
|
mode="startup",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||||
|
"static_friction_range": (0.8, 0.8),
|
||||||
|
"dynamic_friction_range": (0.6, 0.6),
|
||||||
|
"restitution_range": (0.0, 0.0),
|
||||||
|
"num_buckets": 64,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
add_base_mass = EventTerm(
|
||||||
|
func=mdp.randomize_rigid_body_mass,
|
||||||
|
mode="startup",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
# Multiplicative ±25% log-uniform. Scale-invariant across robot sizes
|
||||||
|
# (no per-robot kg overrides needed) with geometric mean 1.0 and
|
||||||
|
# symmetric inverse perturbation (acceleration symmetric around nominal).
|
||||||
|
"mass_distribution_params": (1 / 1.25, 1.25),
|
||||||
|
"operation": "scale",
|
||||||
|
"distribution": "log_uniform",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
base_com = preset(
|
||||||
|
default=EventTerm(
|
||||||
|
func=mdp.randomize_rigid_body_com,
|
||||||
|
mode="startup",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
"com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)},
|
||||||
|
},
|
||||||
|
),
|
||||||
|
newton_mjwarp=None,
|
||||||
|
)
|
||||||
|
|
||||||
|
# reset
|
||||||
|
base_external_force_torque = EventTerm(
|
||||||
|
func=mdp.apply_external_force_torque,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
"force_range": (0.0, 0.0),
|
||||||
|
"torque_range": (-0.0, 0.0),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
reset_base = EventTerm(
|
||||||
|
func=mdp.reset_root_state_uniform,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"pose_range": {"x": (-0, 0), "y": (-0, 0), "yaw": (-3.14, 3.14)},
|
||||||
|
"velocity_range": {
|
||||||
|
"x": (-0.0, 0.0),
|
||||||
|
"y": (-0.0, 0.0),
|
||||||
|
"z": (-0.0, 0.0),
|
||||||
|
"roll": (-0.0, 0.0),
|
||||||
|
"pitch": (-0.0, 0.0),
|
||||||
|
"yaw": (-0.0, 0.0),
|
||||||
|
},
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
reset_robot_joints = EventTerm(
|
||||||
|
func=mdp.reset_joints_by_scale,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"position_range": (0.5, 1.5),
|
||||||
|
"velocity_range": (0.0, 0.0),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# interval
|
||||||
|
push_robot = EventTerm(
|
||||||
|
func=mdp.push_by_setting_velocity,
|
||||||
|
mode="interval",
|
||||||
|
interval_range_s=(10.0, 15.0),
|
||||||
|
params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RewardsCfg:
|
||||||
|
"""Reward terms for the MDP."""
|
||||||
|
|
||||||
|
# -- task
|
||||||
|
track_lin_vel_xy_exp = RewTerm(
|
||||||
|
func=mdp.track_lin_vel_xy_exp, weight=7.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
|
||||||
|
)#水平跟随
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_exp, weight=2.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
|
||||||
|
)#偏航角跟随
|
||||||
|
# -- penalties
|
||||||
|
lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=-1.0)#垂直速度分量
|
||||||
|
ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=-1.0) #机身翻滚
|
||||||
|
dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)#关节力矩平方和
|
||||||
|
dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)#关节加速度平方和
|
||||||
|
action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3)#惩罚相邻动作之间的变化量平方
|
||||||
|
|
||||||
|
#feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.1) # 足端空中时间奖励
|
||||||
|
|
||||||
|
feet_air_time = RewTerm(
|
||||||
|
func=mdp.feet_air_time,
|
||||||
|
weight=0.125,
|
||||||
|
params={
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
"command_name": "base_velocity",
|
||||||
|
"threshold": 0.5,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
# 定义一个不期望接触的惩罚项,用于优化机器人运动
|
||||||
|
# 使用RewTerm类创建,其权重为负值,表示对不期望接触的行为进行惩罚
|
||||||
|
undesired_contacts = RewTerm(
|
||||||
|
func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts,用于计算不期望接触的惩罚
|
||||||
|
weight=-1.0, # 设置权重为-1.0,表示不期望接触的行为会带来负奖励
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0},
|
||||||
|
)
|
||||||
|
|
||||||
|
# -- optional penalties
|
||||||
|
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=-1.5) #惩罚倾斜角度
|
||||||
|
dof_pos_limits = RewTerm(func=mdp.joint_pos_limits, weight=0.0) #关节位置限制惩罚
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TerminationsCfg:
|
||||||
|
"""Termination terms for the MDP."""
|
||||||
|
|
||||||
|
time_out = DoneTerm(func=mdp.time_out, time_out=True)
|
||||||
|
base_contact = DoneTerm(
|
||||||
|
func=mdp.illegal_contact,
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[
|
||||||
|
"head_link",
|
||||||
|
"lf0_link", "lf1_link", "lf2_link",
|
||||||
|
"rf0_link", "rf1_link", "rf2_link",
|
||||||
|
]), "threshold": 1.0},
|
||||||
|
)
|
||||||
|
#机器人Z轴与世界Z轴夹角超90度则终止
|
||||||
|
'''upside_down = DoneTerm(
|
||||||
|
func=mdp.upside_down,
|
||||||
|
params={"asset_cfg": SceneEntityCfg("robot"), "threshold": 0.0},)'''
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CurriculumCfg:
|
||||||
|
"""Curriculum terms for the MDP."""
|
||||||
|
|
||||||
|
# The scene uses a flat plane, so there is no terrain generator to update.
|
||||||
|
terrain_levels = None
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Environment configuration
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TwofootbotcnnEnvCfg(ManagerBasedRLEnvCfg):
|
||||||
|
"""Configuration for the locomotion velocity-tracking environment."""
|
||||||
|
|
||||||
|
# Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs
|
||||||
|
sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg())
|
||||||
|
# Scene settings
|
||||||
|
scene: MySceneCfg = MySceneCfg(num_envs=100, env_spacing=0.5)
|
||||||
|
# Basic settings
|
||||||
|
observations: ObservationsCfg = ObservationsCfg()
|
||||||
|
actions: ActionsCfg = ActionsCfg()
|
||||||
|
commands: CommandsCfg = CommandsCfg()
|
||||||
|
# MDP settings
|
||||||
|
rewards: RewardsCfg = RewardsCfg()
|
||||||
|
terminations: TerminationsCfg = TerminationsCfg()
|
||||||
|
events: EventsCfg = EventsCfg()
|
||||||
|
curriculum: CurriculumCfg = CurriculumCfg()
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
"""Post initialization."""
|
||||||
|
# general settings
|
||||||
|
self.decimation = 4
|
||||||
|
self.episode_length_s = 20.0 #time_out = 20s
|
||||||
|
# simulation settings
|
||||||
|
self.sim.dt = 0.005
|
||||||
|
self.sim.render_interval = self.decimation
|
||||||
|
self.sim.physics_material = self.scene.terrain.physics_material
|
||||||
|
self.curriculum.terrain_levels = None
|
||||||
|
# update sensor update periods
|
||||||
|
# we tick all the sensors based on the smallest update period (physics update period)
|
||||||
|
if self.scene.height_scanner is not None:
|
||||||
|
self.scene.height_scanner.update_period = self.decimation * self.sim.dt
|
||||||
|
if self.scene.contact_forces is not None:
|
||||||
|
self.scene.contact_forces.update_period = self.sim.dt
|
||||||
|
|
||||||
|
# check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator
|
||||||
|
# this generates terrains with increasing difficulty and is useful for training
|
||||||
|
if getattr(self.curriculum, "terrain_levels", None) is not None:
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = True
|
||||||
|
else:
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = False
|
||||||
|
|
@ -0,0 +1,574 @@
|
||||||
|
# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
|
||||||
|
# All rights reserved.
|
||||||
|
#
|
||||||
|
# SPDX-License-Identifier: BSD-3-Clause
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import MISSING
|
||||||
|
from pathlib import Path
|
||||||
|
import torch
|
||||||
|
from isaaclab.sensors import ContactSensorCfg
|
||||||
|
from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg
|
||||||
|
from isaaclab_newton.sensors import ContactSensorCfg as NewtonContactSensorCfg
|
||||||
|
from isaaclab_ovphysx.sensors import ContactSensorCfg as OvPhysXContactSensorCfg
|
||||||
|
from isaaclab_physx.physics import PhysxCfg
|
||||||
|
from isaaclab_physx.sensors import ContactSensorCfg as PhysXContactSensorCfg
|
||||||
|
|
||||||
|
from isaaclab.actuators import ImplicitActuatorCfg
|
||||||
|
|
||||||
|
import isaaclab.sim as sim_utils
|
||||||
|
from isaaclab.assets import ArticulationCfg, AssetBaseCfg
|
||||||
|
from isaaclab.envs import ManagerBasedRLEnvCfg
|
||||||
|
from isaaclab.managers import CurriculumTermCfg as CurrTerm
|
||||||
|
from isaaclab.managers import EventTermCfg as EventTerm
|
||||||
|
from isaaclab.managers import ObservationGroupCfg as ObsGroup
|
||||||
|
from isaaclab.managers import ObservationTermCfg as ObsTerm
|
||||||
|
from isaaclab.managers import RewardTermCfg as RewTerm
|
||||||
|
from isaaclab.managers import SceneEntityCfg
|
||||||
|
from isaaclab.managers import TerminationTermCfg as DoneTerm
|
||||||
|
from isaaclab.scene import InteractiveSceneCfg
|
||||||
|
from isaaclab.sensors import RayCasterCfg, patterns
|
||||||
|
from isaaclab.sim import SimulationCfg
|
||||||
|
from isaaclab.terrains import TerrainImporterCfg
|
||||||
|
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
|
||||||
|
from isaaclab.utils.configclass import configclass
|
||||||
|
from isaaclab.utils.noise import UniformNoiseCfg as Unoise
|
||||||
|
|
||||||
|
import isaaclab_tasks.core.velocity.mdp as mdp
|
||||||
|
#from isaaclab.managers import mdp as base_mdp
|
||||||
|
from isaaclab_tasks.utils import PresetCfg, preset
|
||||||
|
|
||||||
|
from isaaclab.sensors import CameraCfg
|
||||||
|
from isaaclab.sensors import TiledCameraCfg
|
||||||
|
from isaaclab.sim import PinholeCameraCfg
|
||||||
|
|
||||||
|
|
||||||
|
import isaaclab.sim as sim_utils
|
||||||
|
from isaaclab.actuators import ImplicitActuatorCfg
|
||||||
|
from isaaclab.assets import ArticulationCfg
|
||||||
|
|
||||||
|
from isaaclab.managers import (
|
||||||
|
RewardTermCfg,
|
||||||
|
TerminationTermCfg,
|
||||||
|
EventTermCfg as EventTerm,
|
||||||
|
ObservationGroupCfg as ObsGroup,
|
||||||
|
ObservationTermCfg as ObsTerm,
|
||||||
|
CurriculumTermCfg as CurriculumTerm,
|
||||||
|
SceneEntityCfg,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。
|
||||||
|
"jlf0_link",
|
||||||
|
"jlf1_link",
|
||||||
|
"jlf2_link",
|
||||||
|
"jlf3_link",
|
||||||
|
"jrf0_link",
|
||||||
|
"jrf1_link",
|
||||||
|
"jrf2_link",
|
||||||
|
"jrf3_link",
|
||||||
|
]
|
||||||
|
BOT_PACKAGE_PATH = Path(__file__).resolve().parents[5] / "bot/2foot" # 获取机器人 ROS 包目录。
|
||||||
|
URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。
|
||||||
|
|
||||||
|
|
||||||
|
def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs):
|
||||||
|
"""Spawn the URDF and enable contact reporting on every rigid link."""
|
||||||
|
from pxr import Usd, UsdPhysics
|
||||||
|
from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf
|
||||||
|
from isaaclab.sim.utils import safe_set_attribute_on_usd_prim
|
||||||
|
|
||||||
|
spawn_cfg = cfg.replace(activate_contact_sensors=False)
|
||||||
|
prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs)
|
||||||
|
for child_prim in Usd.PrimRange(prim):
|
||||||
|
if child_prim.HasAPI(UsdPhysics.RigidBodyAPI):
|
||||||
|
applied_schemas = child_prim.GetAppliedSchemas()
|
||||||
|
if "PhysxRigidBodyAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxRigidBodyAPI")
|
||||||
|
if "PhysxContactReportAPI" not in applied_schemas:
|
||||||
|
child_prim.AddAppliedSchema("PhysxContactReportAPI")
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
safe_set_attribute_on_usd_prim(
|
||||||
|
child_prim, "physxContactReport:threshold", 0.0, camel_case=False
|
||||||
|
)
|
||||||
|
return prim
|
||||||
|
|
||||||
|
|
||||||
|
# 双足机器人URDF配置对象
|
||||||
|
TWOFOOTBOT_URDF_CFG = ArticulationCfg(
|
||||||
|
# 配置双足机器人从URDF文件生成
|
||||||
|
spawn=sim_utils.UrdfFileCfg(
|
||||||
|
# 指定用于生成机器人的函数
|
||||||
|
func=spawn_twofootbot_from_urdf,
|
||||||
|
asset_path=URDF_PATH,
|
||||||
|
# 设置参数:是否固定基座
|
||||||
|
fix_base=False,
|
||||||
|
# 设置参数:是否激活接触传感器
|
||||||
|
activate_contact_sensors=False,
|
||||||
|
# ROS包路径配置
|
||||||
|
ros_package_paths=[{"name": "2foot", "path": BOT_PACKAGE_PATH}],
|
||||||
|
# 关节驱动配置
|
||||||
|
# 创建一个关节驱动配置,使用URDF文件配置中的JointDriveCfg类
|
||||||
|
joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg(
|
||||||
|
# 设置驱动类型为"force",表示通过力来控制关节
|
||||||
|
drive_type="force",
|
||||||
|
# 设置目标类型为"position",表示目标是控制关节位置
|
||||||
|
target_type="position",
|
||||||
|
# 设置控制增益参数,使用PD控制器配置
|
||||||
|
gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(
|
||||||
|
stiffness=17,
|
||||||
|
damping=0
|
||||||
|
),
|
||||||
|
),
|
||||||
|
# 刚体属性配置
|
||||||
|
rigid_props=sim_utils.RigidBodyPropertiesCfg(
|
||||||
|
disable_gravity=False,
|
||||||
|
max_linear_velocity=100.0,
|
||||||
|
max_angular_velocity=100.0,
|
||||||
|
max_depenetration_velocity=5.0,
|
||||||
|
),
|
||||||
|
# 关节属性配置
|
||||||
|
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
|
||||||
|
enabled_self_collisions=False,
|
||||||
|
solver_position_iteration_count=4,
|
||||||
|
solver_velocity_iteration_count=0,
|
||||||
|
),
|
||||||
|
),
|
||||||
|
# 初始状态配置
|
||||||
|
init_state=ArticulationCfg.InitialStateCfg(
|
||||||
|
pos=(0.0, 0.0, 0.22),
|
||||||
|
joint_pos={
|
||||||
|
"jlf0_link": 0.0, "jlf1_link": 0.0, "jlf2_link": 0.0, "jlf3_link": 0.0,
|
||||||
|
"jrf0_link": 0.0, "jrf1_link": 0.0, "jrf2_link": 0.0, "jrf3_link": 0.0,
|
||||||
|
},
|
||||||
|
joint_vel={".*": 0.0},
|
||||||
|
),
|
||||||
|
# 执行器配置
|
||||||
|
actuators={
|
||||||
|
"legs": ImplicitActuatorCfg(
|
||||||
|
# 关节名称表达式列表
|
||||||
|
joint_names_expr=[
|
||||||
|
"jlf0_link", "jlf1_link", "jlf2_link", "jlf3_link",
|
||||||
|
"jrf0_link", "jrf1_link", "jrf2_link", "jrf3_link",
|
||||||
|
],
|
||||||
|
# 仿真中的力限制
|
||||||
|
effort_limit_sim=0.5,
|
||||||
|
# 仿真中的速度限制
|
||||||
|
velocity_limit_sim=5.0, # 速度限制 (rad/s)
|
||||||
|
# 刚度系数
|
||||||
|
stiffness=5,
|
||||||
|
# 阻尼系数
|
||||||
|
damping=0.3,
|
||||||
|
)
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Physics presets
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RoughPhysicsCfg(PresetCfg):
|
||||||
|
"""Shared physics preset for all rough-terrain locomotion envs."""
|
||||||
|
|
||||||
|
default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15)
|
||||||
|
newton_mjwarp = NewtonCfg(
|
||||||
|
solver_cfg=MJWarpSolverCfg(
|
||||||
|
njmax=200,
|
||||||
|
nconmax=100,
|
||||||
|
cone="pyramidal",
|
||||||
|
impratio=1.0,
|
||||||
|
integrator="implicitfast",
|
||||||
|
use_mujoco_contacts=False,
|
||||||
|
),
|
||||||
|
collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000),
|
||||||
|
num_substeps=1,
|
||||||
|
debug_mode=False,
|
||||||
|
# 1 cm shape margin is the single most important Newton setting for rough
|
||||||
|
# terrain — without it, non-Anymal-D robots fail to learn stable contact
|
||||||
|
# on triangle-mesh terrain. See isaaclab_newton 0.5.22 changelog.
|
||||||
|
default_shape_cfg=NewtonShapeCfg(margin=0.01),
|
||||||
|
)
|
||||||
|
physx = default
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Scene definition
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class VelocityEnvContactSensorCfg(PresetCfg):
|
||||||
|
default = PhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||||
|
newton_mjwarp = NewtonContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||||
|
physx = default
|
||||||
|
ovphysx = OvPhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class MySceneCfg(InteractiveSceneCfg):
|
||||||
|
"""Configuration for the terrain scene with a legged robot."""
|
||||||
|
|
||||||
|
# ground terrain
|
||||||
|
terrain = TerrainImporterCfg(
|
||||||
|
prim_path="/World/ground",
|
||||||
|
terrain_type="plane",
|
||||||
|
collision_group=-1,
|
||||||
|
physics_material=sim_utils.RigidBodyMaterialCfg(
|
||||||
|
friction_combine_mode="multiply",
|
||||||
|
restitution_combine_mode="multiply",
|
||||||
|
static_friction=1.0,
|
||||||
|
dynamic_friction=1.0,
|
||||||
|
),
|
||||||
|
visual_material=sim_utils.MdlFileCfg(
|
||||||
|
mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl",
|
||||||
|
project_uvw=True,
|
||||||
|
texture_scale=(0.25, 0.25),
|
||||||
|
),
|
||||||
|
debug_vis=False,
|
||||||
|
)
|
||||||
|
# robots
|
||||||
|
robot: ArticulationCfg = TWOFOOTBOT_URDF_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
|
||||||
|
# sensors
|
||||||
|
height_scanner = None
|
||||||
|
'''height_scanner = RayCasterCfg(
|
||||||
|
prim_path="{ENV_REGEX_NS}/Robot/head_link",
|
||||||
|
offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)),
|
||||||
|
ray_alignment="yaw",
|
||||||
|
pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]),
|
||||||
|
debug_vis=False,
|
||||||
|
mesh_prim_paths=["/World/ground"],
|
||||||
|
)'''
|
||||||
|
contact_forces = VelocityEnvContactSensorCfg()
|
||||||
|
# lights
|
||||||
|
sky_light = AssetBaseCfg(
|
||||||
|
prim_path="/World/skyLight",
|
||||||
|
spawn=sim_utils.DomeLightCfg(
|
||||||
|
intensity=750.0,
|
||||||
|
texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr",
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# MDP settings
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CommandsCfg:
|
||||||
|
"""Command specifications for the MDP."""
|
||||||
|
|
||||||
|
base_velocity = mdp.UniformVelocityCommandCfg(
|
||||||
|
asset_name="robot",
|
||||||
|
resampling_time_range=(10.0, 10.0),
|
||||||
|
rel_standing_envs=0.02,
|
||||||
|
rel_heading_envs=1.0,
|
||||||
|
heading_command=True,
|
||||||
|
heading_control_stiffness=0.5,
|
||||||
|
debug_vis=True,
|
||||||
|
ranges=mdp.UniformVelocityCommandCfg.Ranges(
|
||||||
|
lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi)
|
||||||
|
),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ActionsCfg:
|
||||||
|
"""Action specifications for the MDP."""
|
||||||
|
|
||||||
|
joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class ObservationsCfg:
|
||||||
|
"""Observation specifications for the MDP."""
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class PolicyCfg(ObsGroup):
|
||||||
|
"""Observations for policy group."""
|
||||||
|
|
||||||
|
# observation terms (order preserved)
|
||||||
|
base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1))
|
||||||
|
base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2))
|
||||||
|
projected_gravity = ObsTerm(
|
||||||
|
func=mdp.projected_gravity,
|
||||||
|
noise=Unoise(n_min=-0.05, n_max=0.05),
|
||||||
|
)
|
||||||
|
velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"})
|
||||||
|
joint_pos = ObsTerm(
|
||||||
|
# 指定计算函数,用于获取关节相对位置
|
||||||
|
func=mdp.joint_pos_rel,
|
||||||
|
# 设置噪声参数,使用均匀分布噪声,最小值为-1e-3,最大值为1e-3
|
||||||
|
noise=Unoise(n_min=-1e-3, n_max=1e-3),
|
||||||
|
# 配置观测参数
|
||||||
|
params={
|
||||||
|
# 指定观测的实体为机器人,并配置关节名称和顺序
|
||||||
|
"asset_cfg": SceneEntityCfg(
|
||||||
|
"robot", # 实体名称为"robot"
|
||||||
|
joint_names=JOINT_NAMES, # 指定要观测的关节名称列表
|
||||||
|
preserve_order=True, # 保持关节顺序不变
|
||||||
|
)
|
||||||
|
},
|
||||||
|
# 设置历史记录长度为5,即保留最近5个时间步的观测数据
|
||||||
|
history_length=5,
|
||||||
|
)
|
||||||
|
joint_vel = ObsTerm(
|
||||||
|
func=mdp.joint_vel,
|
||||||
|
noise=Unoise(n_min=-0.04, n_max=0.04),
|
||||||
|
scale=0.05,
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg(
|
||||||
|
"robot",
|
||||||
|
joint_names=JOINT_NAMES,
|
||||||
|
preserve_order=True,
|
||||||
|
)
|
||||||
|
},
|
||||||
|
history_length=5,
|
||||||
|
)
|
||||||
|
actions = ObsTerm(func=mdp.last_action)
|
||||||
|
velocity_commands = ObsTerm(
|
||||||
|
func=mdp.generated_commands, # 指定调用的函数,即MDP中的generated_commands方法
|
||||||
|
params={"command_name": "base_velocity"}, # 传递给函数的参数,指定命令名称为"base_velocity"
|
||||||
|
scale=5.0, # 缩放因子,用于调整输出值的大小
|
||||||
|
)
|
||||||
|
'''height_scan = ObsTerm(
|
||||||
|
func=mdp.height_scan,
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("height_scanner")},
|
||||||
|
noise=Unoise(n_min=-0.1, n_max=0.1),
|
||||||
|
clip=(-1.0, 1.0),
|
||||||
|
)'''
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
self.enable_corruption = True
|
||||||
|
self.concatenate_terms = True
|
||||||
|
|
||||||
|
# observation groups
|
||||||
|
policy: PolicyCfg = PolicyCfg()
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class EventsCfg:
|
||||||
|
"""Configuration for events."""
|
||||||
|
|
||||||
|
# startup
|
||||||
|
# 创建一个物理材质事件项
|
||||||
|
physics_material = EventTerm(
|
||||||
|
# 指定函数为随机化刚体材质的函数
|
||||||
|
func=mdp.randomize_rigid_body_material,
|
||||||
|
# 设置事件模式为启动时执行
|
||||||
|
mode="startup",
|
||||||
|
# 设置事件参数
|
||||||
|
params={
|
||||||
|
# 配置场景实体,指定为机器人,并匹配所有身体部位
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names=".*"),
|
||||||
|
# 设置静态摩擦力的范围为固定值0.8
|
||||||
|
"static_friction_range": (0.8, 0.8),
|
||||||
|
# 设置动态摩擦力的范围为固定值0.6
|
||||||
|
"dynamic_friction_range": (0.6, 0.6),
|
||||||
|
# 设置恢复系数的范围为固定值0.0(完全非弹性碰撞)
|
||||||
|
"restitution_range": (0.0, 0.0),
|
||||||
|
# 设置材质分桶数量为64
|
||||||
|
"num_buckets": 64,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
add_base_mass = EventTerm(
|
||||||
|
func=mdp.randomize_rigid_body_mass,
|
||||||
|
mode="startup",
|
||||||
|
params={
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
# Multiplicative ±25% log-uniform. Scale-invariant across robot sizes
|
||||||
|
# (no per-robot kg overrides needed) with geometric mean 1.0 and
|
||||||
|
# symmetric inverse perturbation (acceleration symmetric around nominal).
|
||||||
|
"mass_distribution_params": (1 / 1.25, 1.25),
|
||||||
|
"operation": "scale",
|
||||||
|
"distribution": "log_uniform",
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 预设基座质心随机化配置
|
||||||
|
base_com = preset( # 使用preset函数创建预设配置
|
||||||
|
default=EventTerm( # 默认事件项配置
|
||||||
|
func=mdp.randomize_rigid_body_com, # 执行的函数:随机化刚体质心
|
||||||
|
mode="startup", # 事件模式:启动时执行
|
||||||
|
params={ # 函数参数配置
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"), # 场景实体配置:机器人基座
|
||||||
|
"com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)},
|
||||||
|
},
|
||||||
|
),
|
||||||
|
newton_mjwarp=None,
|
||||||
|
)
|
||||||
|
|
||||||
|
# reset
|
||||||
|
# 创建一个外部力矩事件项,用于在重置时对机器人基座施加外部力和力矩
|
||||||
|
base_external_force_torque = EventTerm(
|
||||||
|
# 指定事件处理函数为mdp.apply_external_force_torque
|
||||||
|
func=mdp.apply_external_force_torque,
|
||||||
|
# 设置事件模式为"reset",表示在环境重置时触发
|
||||||
|
mode="reset",
|
||||||
|
# 设置事件参数
|
||||||
|
params={
|
||||||
|
# 指定目标实体为机器人,并指定施加力的部位为基座
|
||||||
|
"asset_cfg": SceneEntityCfg("robot", body_names="head_link"),
|
||||||
|
# 设置力的范围为(0.0, 0.0),表示不施加任何力
|
||||||
|
"force_range": (0.0, 0.0),
|
||||||
|
# 设置力矩的范围为(-0.0, 0.0),表示不施加任何力矩
|
||||||
|
"torque_range": (-0.0, 0.0),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
reset_base = EventTerm(
|
||||||
|
func=mdp.reset_root_state_uniform,
|
||||||
|
mode="reset",
|
||||||
|
params={
|
||||||
|
"pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)},
|
||||||
|
"velocity_range": {
|
||||||
|
"x": (-0.5, 0.5),
|
||||||
|
"y": (-0.5, 0.5),
|
||||||
|
"z": (-0.5, 0.5),
|
||||||
|
"roll": (-0.5, 0.5),
|
||||||
|
"pitch": (-0.5, 0.5),
|
||||||
|
"yaw": (-0.5, 0.5),
|
||||||
|
},
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# 定义一个重置机器人关节的事件项
|
||||||
|
reset_robot_joints = EventTerm(
|
||||||
|
# 指定事件处理函数为mdp.reset_joints_by_scale
|
||||||
|
func=mdp.reset_joints_by_scale,
|
||||||
|
# 设置事件模式为"reset"
|
||||||
|
mode="reset",
|
||||||
|
# 设置事件参数
|
||||||
|
params={
|
||||||
|
# 设置位置范围为(0.5, 1.5)
|
||||||
|
"position_range": (0.5, 1.5),
|
||||||
|
# 设置速度范围为(0.0, 0.0)
|
||||||
|
"velocity_range": (0.0, 0.0),
|
||||||
|
},
|
||||||
|
)
|
||||||
|
|
||||||
|
# interval
|
||||||
|
# 创建一个名为push_robot的EventTerm对象,用于执行机器人推动操作
|
||||||
|
push_robot = EventTerm(
|
||||||
|
func=mdp.push_by_setting_velocity, # 指定执行函数为mdp中的push_by_setting_velocity
|
||||||
|
mode="interval", # 设置执行模式为间隔模式
|
||||||
|
interval_range_s=(10.0, 15.0), # 设置执行间隔时间范围为10.0到15.0秒
|
||||||
|
params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}}, # 设置推动速度范围,x和y方向的速度范围均为-0.5到0.5
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class RewardsCfg:
|
||||||
|
"""Reward terms for the MDP."""
|
||||||
|
|
||||||
|
# -- task
|
||||||
|
track_lin_vel_xy_exp = RewTerm(
|
||||||
|
func=mdp.track_lin_vel_xy_exp, weight=1.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
|
||||||
|
)
|
||||||
|
track_ang_vel_z_exp = RewTerm(
|
||||||
|
func=mdp.track_ang_vel_z_exp, weight=0.5, params={"command_name": "base_velocity", "std": math.sqrt(0.25)}
|
||||||
|
)
|
||||||
|
# -- penalties
|
||||||
|
lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=-2.0)
|
||||||
|
ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=-0.05)
|
||||||
|
dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)
|
||||||
|
dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)
|
||||||
|
action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.01)
|
||||||
|
feet_air_time = RewTerm(
|
||||||
|
func=mdp.feet_air_time,
|
||||||
|
weight=0.125,
|
||||||
|
params={
|
||||||
|
"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]),
|
||||||
|
"command_name": "base_velocity",
|
||||||
|
"threshold": 0.5,
|
||||||
|
},
|
||||||
|
)
|
||||||
|
# 定义一个不期望接触的惩罚项,用于优化机器人运动
|
||||||
|
# 使用RewTerm类创建,其权重为负值,表示对不期望接触的行为进行惩罚
|
||||||
|
undesired_contacts = RewTerm(
|
||||||
|
func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts,用于计算不期望接触的惩罚
|
||||||
|
weight=-1.0, # 设置权重为-1.0,表示不期望接触的行为会带来负奖励
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0},
|
||||||
|
)
|
||||||
|
# -- optional penalties
|
||||||
|
flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0)
|
||||||
|
dof_pos_limits = RewTerm(func=mdp.joint_pos_limits, weight=0.0)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class TerminationsCfg:
|
||||||
|
"""Termination terms for the MDP."""
|
||||||
|
|
||||||
|
time_out = DoneTerm(func=mdp.time_out, time_out=True)
|
||||||
|
base_contact = DoneTerm(
|
||||||
|
func=mdp.illegal_contact,
|
||||||
|
params={"sensor_cfg": SceneEntityCfg("contact_forces",
|
||||||
|
body_names=[
|
||||||
|
"head_link",
|
||||||
|
"lf0_link", "lf1_link", "lf2_link",
|
||||||
|
"rf0_link", "rf1_link", "rf2_link",
|
||||||
|
]), "threshold": 1.0},
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class CurriculumCfg:
|
||||||
|
"""Curriculum terms for the MDP."""
|
||||||
|
|
||||||
|
# The scene uses a flat plane, so there is no terrain generator to update.
|
||||||
|
terrain_levels = None
|
||||||
|
|
||||||
|
|
||||||
|
##
|
||||||
|
# Environment configuration base
|
||||||
|
##
|
||||||
|
|
||||||
|
|
||||||
|
@configclass
|
||||||
|
class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg):
|
||||||
|
"""Configuration for the locomotion velocity-tracking environment."""
|
||||||
|
|
||||||
|
# Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs
|
||||||
|
sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg())
|
||||||
|
# Scene settings
|
||||||
|
scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=0.5)
|
||||||
|
# Basic settings
|
||||||
|
observations: ObservationsCfg = ObservationsCfg()
|
||||||
|
actions: ActionsCfg = ActionsCfg()
|
||||||
|
commands: CommandsCfg = CommandsCfg()
|
||||||
|
# MDP settings
|
||||||
|
rewards: RewardsCfg = RewardsCfg()
|
||||||
|
terminations: TerminationsCfg = TerminationsCfg()
|
||||||
|
events: EventsCfg = EventsCfg()
|
||||||
|
curriculum: CurriculumCfg = CurriculumCfg()
|
||||||
|
|
||||||
|
def __post_init__(self):
|
||||||
|
"""Post initialization."""
|
||||||
|
# general settings
|
||||||
|
self.decimation = 4
|
||||||
|
self.episode_length_s = 20.0
|
||||||
|
# simulation settings
|
||||||
|
self.sim.dt = 0.005
|
||||||
|
self.sim.render_interval = self.decimation
|
||||||
|
self.sim.physics_material = self.scene.terrain.physics_material
|
||||||
|
# update sensor update periods
|
||||||
|
# we tick all the sensors based on the smallest update period (physics update period)
|
||||||
|
if self.scene.height_scanner is not None:
|
||||||
|
self.scene.height_scanner.update_period = self.decimation * self.sim.dt
|
||||||
|
if self.scene.contact_forces is not None:
|
||||||
|
self.scene.contact_forces.update_period = self.sim.dt
|
||||||
|
|
||||||
|
# check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator
|
||||||
|
# this generates terrains with increasing difficulty and is useful for training
|
||||||
|
if getattr(self.curriculum, "terrain_levels", None) is not None:
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = True
|
||||||
|
else:
|
||||||
|
if self.scene.terrain.terrain_generator is not None:
|
||||||
|
self.scene.terrain.terrain_generator.curriculum = False
|
||||||
|
|
@ -11,9 +11,19 @@ import TwoFootBot_isaaclab.tasks # noqa: F401
|
||||||
|
|
||||||
|
|
||||||
def test_task_registrations():
|
def test_task_registrations():
|
||||||
"""The project tasks must expose valid environment and agent entry points."""
|
"""The migrated MLP tasks must expose their environment and agent configurations."""
|
||||||
assert all(
|
for task_id, env_cfg, agent_cfg in (
|
||||||
not str(spec.kwargs.get("env_cfg_entry_point", "")).startswith("TwoFootBot_isaaclab.tasks")
|
(
|
||||||
for spec in gym.registry.values()
|
"Template-Twofootbot-mlp-v0",
|
||||||
)
|
"TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv0:TwofootbotdemoEnvCfg",
|
||||||
|
"TwoFootBot_isaaclab.tasks.manager_based.twofootbot.agents.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
),
|
||||||
|
(
|
||||||
|
"Template-Twofootbot-mlp-v1",
|
||||||
|
"TwoFootBot_isaaclab.tasks.manager_based.twofootbot.Lv1:Lv1Cfg",
|
||||||
|
"TwoFootBot_isaaclab.tasks.manager_based.twofootbot.agents.rsl_rl_ppo_cfg:PPORunnerCfg",
|
||||||
|
),
|
||||||
|
):
|
||||||
|
spec = gym.spec(task_id)
|
||||||
|
assert spec.kwargs["env_cfg_entry_point"] == env_cfg
|
||||||
|
assert spec.kwargs["rsl_rl_cfg_entry_point"] == agent_cfg
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue