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

chore: release v0.1.0

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

4
motrix_envs/pyproject.toml
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,64 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/bounce_ball_ctrl.xml"
@registry.envcfg("bounce_ball")
@dataclass
class BounceBallEnvCfg(EnvCfg):
model_file: str = model_file
reset_noise_scale: float = 0.01
max_episode_seconds: float = 20.0
# Ball and paddle physics parameters
ball_restitution: float = 0.9 # Slightly less than perfect for realistic bouncing
ball_linear_damping: float = 0.55
ball_lateral_friction: float = 1.1
paddle_restitution: float = 0.8
paddle_linear_damping: float = 0.55
paddle_lateral_friction: float = 1.1
# Initial conditions
ball_init_pos: list = None
ball_init_vel: list = None
arm_init_qpos: list = None
# Target height for bouncing (configurable parameter)
target_ball_height: float = 0.8 # Default target height in meters
height_tolerance: float = 0.1 # Tolerance for reward calculation
# Action scaling parameters
action_scale: list = None
action_bias: list = None
def __post_init__(self):
if self.ball_init_pos is None:
self.ball_init_pos = [0.58856, 0, 1.27796] # Slightly above paddle (paddle z=0.2803)
if self.ball_init_vel is None:
self.ball_init_vel = [0.0, 0.0, 0.0]
if self.arm_init_qpos is None:
self.arm_init_qpos = [0, 40, 110, 0, -60, 0]
if self.action_scale is None:
self.action_scale = [0.0008] * 6
if self.action_bias is None:
self.action_bias = [0.0] * 6

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,120 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.basic.cheetah.cfg import CheetahEnvCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-cheetah", "np")
class CheetahEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: CheetahEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_obs_space()
self._init_action_space()
self._torso = self._model.get_link("torso")
self._run_speed = cfg.run_speed
self._joint_limits = self._model.joint_limits
def _init_obs_space(self):
obs_dim = (self._model.num_dof_pos - 1) + self._model.num_dof_vel
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (obs_dim,), dtype=np.float64)
def _init_action_space(self):
model = self._model
self._action_space = gym.spaces.Box(
low=-1.0,
high=1.0,
shape=(model.num_actuators,),
dtype=np.float32,
)
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def apply_action(self, actions, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
pos = qpos[:, 1:].copy() # exclude x position
vel = data.dof_vel
obs = np.concatenate([pos, vel], axis=-1)
return obs
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
# === compute obs ===
obs = self._get_obs(data)
# === Terminated ===
terminated = np.zeros(data.shape[0], dtype=bool)
# ==== compute reward ====
vel = self._model.get_sensor_value("torso_subtreelinvel", data)
rwd_speed = reward.tolerance(
vel[:, 0],
bounds=(self._run_speed, float("inf")),
margin=self._run_speed,
value_at_margin=0.0,
sigmoid="linear",
)
torso_height = self._torso.get_position(data)[:, 2]
rwd_posture = -1.5 * (torso_height - 0.75) ** 2
rwd_posture = np.clip(rwd_posture, -1.0, 1.0)
rwd = rwd_speed + rwd_posture
return state.replace(
obs=obs,
reward=rwd,
terminated=terminated,
)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
data.reset(self._model)
num = data.shape[0]
limited_idx = np.where(self._model.joint_limits == 1)[0]
low = self._joint_limits[0, limited_idx]
high = self._joint_limits[1, limited_idx]
qpos = data.dof_pos
qpos[:, limited_idx] = np.random.uniform(low, high, size=(num, len(limited_idx)))
data.set_dof_pos(qpos, self._model)
for _ in range(200):
self._model.step(data)
obs = self._get_obs(data)
return obs, {}

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

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

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

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

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

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

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

@@ -0,0 +1,200 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.basic.hopper.cfg import HopperStandCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-hopper-stand", "np")
@registry.env("dm-hopper-hop", "np")
class HopperEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: HopperStandCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_obs_space()
self._init_action_space()
self._torso = self._model.get_link("torso")
self._foot = self._model.get_link("foot")
self._stand_height = cfg.stand_height
self._hop_speed = cfg.hop_speed
self._joint_limits = self._model.joint_limits
def _init_obs_space(self):
model = self._model
num = 0
num += model.num_dof_pos - 1
num += model.num_dof_vel
num += 2
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num,), dtype=np.float32)
def _init_action_space(self):
model = self._model
self._action_space = gym.spaces.Box(
model.actuator_ctrl_limits[0],
model.actuator_ctrl_limits[1],
(model.num_actuators,),
dtype=np.float32,
)
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
def apply_action(self, actions, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos[:, 1:]
qvel = data.dof_vel
num_env = int(data.shape[0])
toe = np.asarray(self._model.get_sensor_value("touch_toe", data)).reshape(num_env, -1)[:, 0]
heel = np.asarray(self._model.get_sensor_value("touch_heel", data)).reshape(num_env, -1)[:, 0]
toe = np.log1p(toe)
heel = np.log1p(heel)
touch = np.stack([toe, heel], axis=-1) # shape -> (num_env, 2)
return np.concatenate([qpos, qvel, touch], axis=-1)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
# === obs ===
obs = self._get_obs(data)
num_env = int(data.shape[0])
toe = np.asarray(self._model.get_sensor_value("touch_toe", data)).reshape(num_env, -1)[:, 0]
heel = np.asarray(self._model.get_sensor_value("touch_heel", data)).reshape(num_env, -1)[:, 0]
toe = np.log1p(toe)
heel = np.log1p(heel)
# === physical values ===
torso_pos = self._torso.get_position(data)
foot_pos = self._foot.get_position(data)
torso_height = torso_pos[:, 2] - foot_pos[:, 2]
torso_vel = self._model.get_sensor_value("torso_subtreelinvel", data)
speed = torso_vel[:, 0]
# === terminated ===
over_speed = np.sum(np.square(data.dof_vel[:, 4:7]), axis=-1) > 1e8
terminated = np.isnan(obs).any(axis=-1)
terminated |= over_speed
standing = reward.tolerance(
torso_height,
bounds=(self._stand_height, 2.0),
margin=self._stand_height * 0.5,
)
if self._hop_speed > 0.0:
hopping = reward.tolerance(
speed,
bounds=(self._hop_speed * 0.3, float("inf")),
margin=self._hop_speed * 0.3,
value_at_margin=0.0,
sigmoid="linear",
)
leg_vel = np.linalg.norm(data.dof_vel[:, 4:7], axis=-1)
leg_bonus = np.tanh(leg_vel * 0.3) * 0.2 * standing
knee_vel = data.dof_vel[:, 5]
extend_reward = np.maximum(knee_vel, 0) * 0.2 * standing
stand_condition = (torso_height > self._stand_height * 0.8).astype(np.float32)
effective_hop_reward = hopping * stand_condition
contact_strength = toe + heel
contact_reward = np.clip(contact_strength, 0.0, 1.0) * 0.1 * standing
rwd = standing * 0.8 + effective_hop_reward * 0.8 + leg_bonus * 0.5 + extend_reward + contact_reward
if np.average(rwd) > 1000:
print(
"standing",
np.sum(standing),
"effective_hop_reward",
np.sum(effective_hop_reward),
"leg_bonus",
np.sum(leg_bonus),
"extend_reward",
np.sum(extend_reward),
"contact_reward",
np.sum(contact_reward),
)
else:
control_magnitude = np.linalg.norm(data.actuator_ctrls, axis=-1)
small_control = reward.tolerance(
control_magnitude,
bounds=(0, 1),
margin=1,
value_at_margin=0,
sigmoid="quadratic",
)
small_control = (small_control + 4) / 5
rwd = standing * small_control
state.info["Reward"] = {"stand": standing, "control": small_control, "total": rwd}
rwd[terminated] = 0.0
return state.replace(
obs=obs,
reward=rwd,
terminated=terminated,
)
def reset(self, data: mtx.SceneData):
data.reset(self._model)
num_env = data.shape[0]
dof_pos = np.zeros((num_env, self._model.num_dof_pos))
dof_pos[:, 2] = 0
if self._model.num_dof_pos > 3:
dof_pos[:, 3:] = np.random.uniform(
low=self._joint_limits[0, 3:],
high=self._joint_limits[1, 3:],
size=(num_env, self._model.num_dof_pos - 3),
)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
obs = self._get_obs(data)
rewards = {"stand": np.zeros((num_env,))}
if self._hop_speed > 0.0:
rewards["hop"] = np.zeros((num_env,))
return obs, {"Reward": rewards}

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

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

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

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

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

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

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

@@ -0,0 +1,115 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.basic.reacher.cfg import ReacherEnvCfg
from motrix_envs.np import reward
from motrix_envs.np.env import NpEnv, NpEnvState
@registry.env("dm-reacher", "np")
class Reacher2DEnv(NpEnv):
_observation_space: gym.spaces.Box
_action_space: gym.spaces.Box
def __init__(self, cfg: ReacherEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._target_size = cfg.target_size
self._finger = self._model.get_link("finger")
self._joint_limits = self._model.joint_limits
self._target_body = self._model.get_body("target")
self._target_xyz = np.zeros((num_envs, 3), dtype=np.float32)
self._init_obs_space()
self._init_action_space()
def _init_obs_space(self):
num_obs = self._model.num_dof_pos + 2 + self._model.num_dof_vel
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
low, high = self._model.actuator_ctrl_limits
self._action_space = gym.spaces.Box(low, high, (self._model.num_actuators,), dtype=np.float32)
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
def apply_action(self, actions, state):
state.data.actuator_ctrls = actions
return state
def _get_obs(self, data: mtx.SceneData) -> np.ndarray:
qpos = data.dof_pos
qvel = data.dof_vel
finger_xy = self._finger.get_pose(data)[:, :2]
to_target = self._target_xyz[:, :2] - finger_xy
return np.concatenate([qpos, to_target, qvel], axis=-1)
def update_state(self, state: NpEnvState) -> NpEnvState:
data = state.data
obs = self._get_obs(data)
finger_xy = self._finger.get_pose(data)[:, :2]
dist = np.linalg.norm(self._target_xyz[:, :2] - finger_xy, axis=-1)
rwd = reward.tolerance(
dist, bounds=(0.0, self._target_size), margin=self._target_size, value_at_margin=0.0, sigmoid="linear"
)
terminated = np.isnan(obs).any(axis=-1)
rwd[terminated] = 0.0
state.info["target_info"] = {"positions": self._target_xyz.copy(), "size": self._target_size}
state.info["Reward"] = {"distance": dist, "tolerance": rwd.copy()}
return state.replace(obs=obs, reward=rwd, terminated=terminated)
def reset(self, data: mtx.SceneData) -> tuple[np.ndarray, dict]:
"""Reset environment with randomized target position in xy plane (z=0)."""
data.reset(self._model)
num_reset = data.shape[0]
dof_pos = np.zeros((num_reset, self._model.num_dof_pos))
dof_pos[:, 0] = np.random.uniform(-np.pi, np.pi, size=(num_reset,))
dof_pos[:, 1] = np.random.uniform(-np.pi, np.pi, size=(num_reset,))
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
target_x = np.random.uniform(-0.15, 0.15, size=(num_reset,))
target_y = np.random.uniform(0.15, 0.15, size=(num_reset,))
target_dof_pos = np.stack([target_x, target_y], axis=-1)
self._target_body.set_dof_pos(data, target_dof_pos)
self._model.forward_kinematic(data)
target_pose = self._target_body.get_pose(data)
self._target_xyz = target_pose.copy()
self._target_xyz[:, 2] = 0.0
obs = self._get_obs(data)
rewards = {"distance": np.zeros((num_reset,)), "tolerance": np.zeros((num_reset,))}
info = {"Reward": rewards, "target_info": {"positions": self._target_xyz.copy(), "size": self._target_size}}
return obs, info

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

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

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

@@ -5,19 +5,20 @@ for example receiving a positive reward.
-->
<mujoco>
<asset>
<texture name="grid" type="2d" builtin="checker" mark="edge" rgb1="0.4314 0.5294 0.6431" rgb2="0.8157 0.8549 0.9059" markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
<material name="self" rgba=".7 .5 .3 1"/>
<material name="self_default" rgba=".7 .5 .3 1"/>
<material name="self_highlight" rgba="0 .5 .3 1"/>
<material name="effector" rgba=".7 .4 .2 1"/>
<material name="effector_default" rgba=".7 .4 .2 1"/>
<material name="effector_highlight" rgba="0 .5 .3 1"/>
<material name="decoration" rgba=".3 .5 .7 1"/>
<material name="eye" rgba="0 .2 1 1"/>
<material name="target" rgba=".6 .3 .3 1"/>
<material name="target_default" rgba=".6 .3 .3 1"/>
<material name="target_highlight" rgba=".6 .3 .3 .4"/>
<material name="site" rgba=".5 .5 .5 .3"/>
<texture name="grid" type="2d" builtin="checker" mark="edge" rgb1="0.4314 0.5294 0.6431"
rgb2="0.8157 0.8549 0.9059" markrgb="0.8 0.8 0.8" width="300" height="300" />
<material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2" />
<material name="self" rgba=".7 .5 .3 1" />
<material name="self_default" rgba=".7 .5 .3 1" />
<material name="self_highlight" rgba="0 .5 .3 1" />
<material name="effector" rgba=".7 .4 .2 1" />
<material name="effector_default" rgba=".7 .4 .2 1" />
<material name="effector_highlight" rgba="0 .5 .3 1" />
<material name="decoration" rgba=".3 .5 .7 1" />
<material name="eye" rgba="0 .2 1 1" />
<material name="target" rgba=".6 .3 .3 1" />
<material name="target_default" rgba=".6 .3 .3 1" />
<material name="target_highlight" rgba=".6 .3 .3 .4" />
<material name="site" rgba=".5 .5 .5 .3" />
</asset>
</mujoco>
</mujoco>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3
motrix_envs/src/motrix_envs/locomotion/anymal_c/xmls/assets/cone.stl Normal file
View File

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

@@ -0,0 +1,26 @@
<mujoco model="anymal_c scene">
<include file="anymal_c.xml" />
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
<rgba haze="0.15 0.25 0.35 1" />
<global azimuth="120" elevation="-20" />
</visual>
<statistic center="0 0 .3" extent="1.2" />
<asset>
<texture name="skybox" type="skybox" builtin="gradient" rgb1="0.4 0.4 0.4" rgb2="0 0 0"
width="512"
height="512" />
<texture name="motphys-ground" type="2d" file="../../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
<worldbody>
<light diffuse="0.6 0.6 0.6" pos="0 0 1.5"
dir="-0.49835488200187683 0.2925136387348175 -0.8161361217498779" directional="true" />
<geom name="ground" size="0 0 0.05" type="plane" material="motphys-ground" />
</worldbody>
</mujoco>

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

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

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

@@ -19,8 +19,6 @@ from dataclasses import dataclass, field
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/xmls/scene_motor_actuator.xml"
@dataclass
class NoiseConfig:
@@ -65,7 +63,7 @@ class InitState:
@dataclass
class Commands:
vel_limit = [
[0.0, -1.0, -1.0], # min: vel_x [m/s], vel_y [m/s], ang_vel [rad/s]
[-1.0, -1.0, -1.0], # min: vel_x [m/s], vel_y [m/s], ang_vel [rad/s]
[2.0, 1.0, 1.0], # max
]
@@ -82,17 +80,16 @@ class Normalization:
class Asset:
body_name = "trunk"
foot_name = "foot"
ground_name = "floor"
penalize_contacts_on = ["thigh", "calf"]
terminate_after_contacts_on = [
"trunk",
]
ground = "floor"
terminate_after_contacts_on = ["trunk"]
@dataclass
class Sensor:
local_linvel = "local_linvel"
gyro = "gyro"
feet = ["FR", "FL", "RR", "RL"]
@dataclass
@@ -111,7 +108,6 @@ class RewardConfig:
"base_height": -0.0,
"feet_air_time": 1.0,
"collision": -1.0 * 0,
"feet_stumble": -0.0,
"action_rate": -0.001,
"stand_still": -0.0,
"hip_pos": -1,
@@ -127,7 +123,7 @@ class RewardConfig:
@dataclass
class Go1WalkNpEnvCfg(EnvCfg):
max_episode_seconds: float = 20.0
model_file: str = model_file
model_file: str = os.path.dirname(__file__) + "/xmls/scene_motor_actuator.xml"
noise_config: NoiseConfig = field(default_factory=NoiseConfig)
control_config: ControlConfig = field(default_factory=ControlConfig)
reward_config: RewardConfig = field(default_factory=RewardConfig)
@@ -138,3 +134,54 @@ class Go1WalkNpEnvCfg(EnvCfg):
sensor: Sensor = field(default_factory=Sensor)
sim_dt: float = 0.01
ctrl_dt: float = 0.01
@registry.envcfg("go1-rough-terrain-walk")
@dataclass
class Go1WalkNpRoughEnvCfg(Go1WalkNpEnvCfg):
render_spacing: float = 0.0
model_file: str = os.path.dirname(__file__) + "/xmls/scene_rough_terrain.xml"
@registry.envcfg("go1-stairs-terrain-walk")
@dataclass
class Go1WalkNpStairsEnvCfg(Go1WalkNpEnvCfg):
render_spacing: float = 0.0
model_file: str = os.path.dirname(__file__) + "/xmls/scene_stairs_terrain.xml"
@dataclass
class Commands:
vel_limit = [
[0.5, -0.0, 0.0], # min: vel_x [m/s], vel_y [m/s], ang_vel [rad/s]
[1.0, 0.0, 0.0], # max
]
@dataclass
class RewardConfig:
scales: dict[str, float] = field(
default_factory=lambda: {
"termination": -0.0,
"tracking_lin_vel": 1.0,
"tracking_ang_vel": 0.5,
"lin_vel_z": -2.0,
"ang_vel_xy": -0.05,
"orientation": -0.0,
"torques": -0.00001,
"dof_vel": -0.0,
"dof_acc": -2.5e-7,
"base_height": -0.0,
"feet_air_time": 1.0,
"collision": -1.0 * 0,
"feet_stumble": -0.1,
"action_rate": -0.001,
"stand_still": -0.0,
"hip_pos": -1,
"calf_pos": -0.3 * 0,
}
)
tracking_sigma: float = 0.25
max_foot_height: float = 0.1
commands: Commands = field(default_factory=Commands)
reward_config: RewardConfig = field(default_factory=RewardConfig)

49
motrix_envs/src/motrix_envs/locomotion/go1/common.py Normal file
View File

@@ -0,0 +1,49 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import numpy as np
def generate_repeating_array(N, L, i):
"""
Generate an array of length L with values repeating in [0, N],
starting with the value at index i of the period.
Parameters:
N: maximum value in the period (0 to N)
L: length of the output array
i: starting index within the period
Returns:
numpy array with the repeating pattern
"""
# Create the base period [0, 1, 2, ..., N-1]
period = np.arange(N)
# Create the starting sequence by rolling the period to start at index i
start_from_i = np.roll(period, -i)
# Calculate how many full periods we need
full_periods = L // len(start_from_i)
remainder = L % len(start_from_i)
# Create the array by repeating the rolled period
result = np.tile(start_from_i, full_periods)
# Add the remaining elements if needed
if remainder > 0:
result = np.concatenate([result, start_from_i[:remainder]])
return result

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

@@ -19,41 +19,10 @@ import numpy as np
from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpEnvCfg
from motrix_envs.math.quaternion import Quaternion
from motrix_envs.np.env import NpEnv, NpEnvState
## provide quat math utility from motrixsim.
def quat_rotate_inverse(quats, v):
"""
Rotate a fixed vector v by a list of quaternions using a vectorized approach.
Parameters:
quats (np.ndarray): Array of quaternions of shape (N, 4). Each quaternion is in [w, x, y, z] format.
v (np.ndarray): Fixed vector of shape (3,) to be rotated.
Returns:
np.ndarray: Array of rotated vectors of shape (N, 3).
"""
# Normalize the quaternions to ensure they are unit quaternions
# Extract the scalar (w) and vector (x, y, z) parts of the quaternions
w = quats[:, -1] # Shape (N,)
im = quats[:, :3] # Shape (N, 3)
# Compute the cross product between the imaginary part of each quaternion and the fixed vector v.
# np.cross broadcasts v to match each row in im, resulting in an array of shape (N, 3)
cross_im_v = np.cross(im, v)
# Compute the intermediate terms for the rotation formula:
term1 = w[:, np.newaxis] * cross_im_v # w * cross(im, v)
term2 = np.cross(im, cross_im_v) # cross(im, cross(im, v))
# Apply the rotation formula: v_rot = v + 2 * (term1 + term2)
v_rotated = v + 2 * (term1 + term2)
return v_rotated
@registry.env("go1-flat-terrain-walk", sim_backend="np")
class Go1WalkTask(NpEnv):
_init_dof_pos: np.ndarray
@@ -146,39 +115,43 @@ class Go1WalkTask(NpEnv):
self._init_dof_pos[-self._num_action :] = self.default_angles
self.ground = self._model.get_geom_index(cfg.asset.ground)
self.ground = []
for geom_name in self._model.geom_names:
if geom_name is not None and cfg.asset.ground_name in geom_name:
self.ground.append(self._model.get_geom_index(geom_name))
self.termination_contact = None
self.foot = []
for name in cfg.asset.terminate_after_contacts_on:
if self.termination_contact is None:
self.termination_contact = np.array([[self._model.get_geom_index(name), self.ground]], dtype=np.uint32)
else:
self.termination_contact = np.append(
self.termination_contact,
np.array(
[[self._model.get_geom_index(name), self.ground]],
dtype=np.uint32,
),
axis=0,
)
for name in cfg.asset.foot_name:
self.foot.append([self._model.get_geom_index(name), self.ground])
self.num_check = self.termination_contact.shape[0]
self.foot = None
for i in self._model.geom_names:
if i is not None and cfg.asset.foot_name in i:
if self.foot is None:
self.foot = np.array([[self._model.get_geom_index(i), self.ground]], dtype=np.uint32)
for gournd_index in self.ground:
for name in cfg.asset.terminate_after_contacts_on:
if self.termination_contact is None:
self.termination_contact = np.array(
[[self._model.get_geom_index(name), gournd_index]], dtype=np.uint32
)
else:
self.foot = np.append(
self.foot,
self.termination_contact = np.append(
self.termination_contact,
np.array(
[[self._model.get_geom_index(i), self.ground]],
[[self._model.get_geom_index(name), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.num_check = self.termination_contact.shape[0]
self.foot = None
for gournd_index in self.ground:
for i in self._model.geom_names:
if i is not None and cfg.asset.foot_name in i:
if self.foot is None:
self.foot = np.array([[self._model.get_geom_index(i), gournd_index]], dtype=np.uint32)
else:
self.foot = np.append(
self.foot,
np.array(
[[self._model.get_geom_index(i), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.foot_check_num = self.foot.shape[0]
self.foot_check = self.foot
@@ -217,7 +190,7 @@ class Go1WalkTask(NpEnv):
gyro = self.get_gyro(data)
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
local_gravity = quat_rotate_inverse(base_quat, self.gravity_vec)
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel
@@ -347,7 +320,7 @@ class Go1WalkTask(NpEnv):
# Penalize non flat base orientation
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
gravity = quat_rotate_inverse(base_quat, self.gravity_vec)
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData):

437
motrix_envs/src/motrix_envs/locomotion/go1/walk_rough_terrain.py Normal file
View File

@@ -0,0 +1,437 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpRoughEnvCfg
from motrix_envs.math.quaternion import Quaternion
from motrix_envs.np.env import NpEnv, NpEnvState
from .common import generate_repeating_array
@registry.env("go1-rough-terrain-walk", sim_backend="np")
class Go1WalkRoughTask(NpEnv):
_init_dof_pos: np.ndarray
_init_dof_vel: np.ndarray
def __init__(self, cfg: Go1WalkNpRoughEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_action_space()
self._init_obs_space()
self._body = self._model.get_body(self.cfg.asset.body_name)
self._num_action = self._action_space.shape[0]
self._num_observation = self._observation_space.shape[0]
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._init_dof_vel = np.zeros(
(self._num_dof_vel,),
dtype=np.float32,
)
self.height_list = np.array([-2.5, 0.5, 2.0])
offset_h = [[2, 2, 1, 1, 1], [2, 2, 1, 1, 2], [2, 1, 1, 1, 1], [2, 1, 1, 1, 1], [2, 1, 1, 1, 1]]
offset = []
for i in range(5):
for j in range(5):
h_index = offset_h[j][i]
offset.append([(i - 2) * 8.0, (j - 2) * 8.0, self.height_list[h_index]])
self.offset_list = np.array(offset)
self._init_dof_pos = self._model.compute_init_dof_pos()
self._init_dof_pos[2] = self.height_list[0]
self._init_buffer()
self.height_counter = 0
def _init_obs_space(self):
model = self.model
num_dof_vel = model.num_dof_vel # linvel + gyro + joint_vel
num_joint_angle = model.num_dof_pos - 7
num_gravity = 3
num_actions = model.num_actuators
num_command = 3
num_obs = num_dof_vel + num_joint_angle + num_gravity + num_actions + num_command
assert num_obs == 48
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
model = self.model
self._action_space = gym.spaces.Box(
np.array(model.actuator_ctrl_limits[0, :]),
np.array(model.actuator_ctrl_limits[1, :]),
(model.num_actuators,),
dtype=np.float32,
)
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
def get_dof_pos(self, data: mtx.SceneModel):
return self._body.get_joint_dof_pos(data)
def get_dof_vel(self, data: mtx.SceneModel):
return self._body.get_joint_dof_vel(data)
def _init_buffer(self):
cfg = self._cfg
assert isinstance(cfg, Go1WalkNpRoughEnvCfg)
# init buffers
self.reset_buf = np.ones(self._num_envs, dtype=np.bool)
self.kps = np.ones(self._num_action, dtype=np.float32) * cfg.control_config.stiffness
self.kds = np.ones(self._num_action, dtype=np.float32) * cfg.control_config.damping
self.gravity_vec = np.array([0, 0, -1], dtype=np.float32)
self.commands_scale = np.array(
(
[
cfg.normalization.lin_vel,
cfg.normalization.lin_vel,
cfg.normalization.ang_vel,
]
),
dtype=np.float32,
)
self.default_angles = np.zeros(self._num_action, dtype=np.float32)
self.hip_indices = []
self.calf_indices = []
for i in range(self._model.num_actuators):
for name in cfg.init_state.default_joint_angles.keys():
if name in self._model.actuator_names[i]:
self.default_angles[i] = cfg.init_state.default_joint_angles[name]
if "hip" in self._model.actuator_names[i]:
self.hip_indices.append(i)
if "calf" in self._model.actuator_names[i]:
self.calf_indices.append(i)
self._init_dof_pos[-self._num_action :] = self.default_angles
self.ground = []
for geom_name in self._model.geom_names:
if geom_name is not None and cfg.asset.ground_name in geom_name:
self.ground.append(self._model.get_geom_index(geom_name))
self.termination_contact = None
for gournd_index in self.ground:
for name in cfg.asset.terminate_after_contacts_on:
if self.termination_contact is None:
self.termination_contact = np.array(
[[self._model.get_geom_index(name), gournd_index]], dtype=np.uint32
)
else:
self.termination_contact = np.append(
self.termination_contact,
np.array(
[[self._model.get_geom_index(name), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.num_check = self.termination_contact.shape[0]
self.foot = None
for gournd_index in self.ground:
for i in self._model.geom_names:
if i is not None and cfg.asset.foot_name in i:
if self.foot is None:
self.foot = np.array([[self._model.get_geom_index(i), gournd_index]], dtype=np.uint32)
else:
self.foot = np.append(
self.foot,
np.array(
[[self._model.get_geom_index(i), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.foot_check_num = self.foot.shape[0]
self.foot_check = self.foot
self.termination_check = self.termination_contact
spacing = 2.0
cols = int(np.ceil(np.sqrt(self._num_envs)))
offsets = []
for i in range(self._num_envs):
row = i // cols
col = i % cols
x = col * spacing
y = row * spacing
z = 0.0
offsets.append([x, y, z])
self.offsets = np.array(offsets)
self.training_level = 0
def apply_action(self, actions, state):
state.info["last_dof_vel"] = self.get_dof_vel(state.data)
state.info["last_actions"] = state.info["current_actions"]
state.info["current_actions"] = actions
state.data.actuator_ctrls = self._compute_torques(actions, state.data)
return state
def _compute_torques(self, actions, data):
# Compute torques from actions.
# pd controller
actions_scaled = actions * self.cfg.control_config.action_scale
torques = self.kps * (
actions_scaled + self.default_angles - self.get_dof_pos(data)
) - self.kds * self.get_dof_vel(data)
return torques
def get_local_linvel(self, data: mtx.SceneData) -> np.ndarray:
return self._model.get_sensor_value(self.cfg.sensor.local_linvel, data)
def get_gyro(self, data: mtx.SceneData) -> np.ndarray:
return self._model.get_sensor_value(self.cfg.sensor.gyro, data)
def update_state(self, state):
state = self.update_observation(state)
state = self.update_terminated(state)
state = self.update_reward(state)
return state
def _get_obs(self, data: mtx.SceneData, info: dict) -> np.ndarray:
linear_vel = self.get_local_linvel(data)
gyro = self.get_gyro(data)
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel
noisy_joint_angle = diff * self.cfg.normalization.dof_pos
noisy_joint_vel = self.get_dof_vel(data) * self.cfg.normalization.dof_vel
command = info["commands"] * self.commands_scale
last_actions = info["current_actions"]
obs = np.hstack(
[
noisy_linvel,
noisy_gyro,
local_gravity,
noisy_joint_angle,
noisy_joint_vel,
last_actions,
command,
]
)
return obs
def update_observation(self, state: NpEnvState):
data = state.data
self.border_check(data, state.info)
obs = self._get_obs(data, state.info)
cquerys = self._model.get_contact_query(data)
foot_contact = cquerys.is_colliding(self.foot_check)
state.info["contacts"] = foot_contact.reshape((self._num_envs, self.foot_check_num))
state.info["feet_air_time"] = self.update_feet_air_time(state.info)
return state.replace(obs=obs)
def update_terminated(self, state: NpEnvState) -> NpEnvState:
data = state.data
cquerys = self._model.get_contact_query(data)
termination_check = cquerys.is_colliding(self.termination_check)
termination_check.reshape((self._num_envs, self.num_check))
terminated = termination_check.any(axis=1)
over_speed = np.sum(np.square(self.get_local_linvel(data)[:, :2]), axis=1) > 1e8
terminated = terminated | over_speed
return state.replace(
terminated=terminated,
)
def update_feet_air_time(self, info: dict):
feet_air_time = info["feet_air_time"]
feet_air_time += self.cfg.ctrl_dt
feet_air_time *= ~info["contacts"]
return feet_air_time
def resample_commands(self, num_envs: int):
commands = np.random.uniform(
low=self.cfg.commands.vel_limit[0],
high=self.cfg.commands.vel_limit[1],
size=(num_envs, 3),
)
# commands[:, 2] = 0
return commands
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data
terminated = state.terminated
reward_dict = self._get_reward(data, state.info)
rewards = {k: v * self.cfg.reward_config.scales[k] for k, v in reward_dict.items()}
rwd = sum(rewards.values())
rwd = np.clip(rwd, 0.0, 10000.0)
# for k,v in rewards.items():
# print(k,v)
if "termination" in self.cfg.reward_config.scales:
termination = self._reward_termination(terminated) * self.cfg.reward_config.scales["termination"]
rwd += termination
rwd = np.where(terminated, np.array(0.0), rwd)
average_reward = np.average(rwd)
if 0.9 < average_reward and self.training_level == 0:
self.training_level = 1
# elif 1.35 < average_reward and self.training_level == 1:
# self.training_level = 2
return state.replace(reward=rwd)
def reset(self, data) -> tuple[np.ndarray, dict]:
num_reset = data.shape[0]
dof_pos = np.tile(self._init_dof_pos, (num_reset, 1))
dof_vel = np.tile(self._init_dof_vel, (num_reset, 1))
if self.training_level == 1:
num_period = 25
idx = generate_repeating_array(num_period, num_reset, self.height_counter)
self.height_counter = (self.height_counter + num_reset) % num_period
dof_pos[:, :3] = self.offset_list[idx]
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
info = {
"current_actions": np.zeros((num_reset, self._num_action), dtype=np.float32),
"last_actions": np.zeros((num_reset, self._num_action), dtype=np.float32),
"commands": self.resample_commands(num_reset),
"last_dof_vel": np.zeros((num_reset, self._num_action), dtype=np.float32),
"feet_air_time": np.zeros((num_reset, self.foot_check_num), dtype=np.float32),
"contacts": np.zeros((num_reset, self.foot_check_num), dtype=np.bool),
}
obs = self._get_obs(data, info)
return obs, info
def _get_reward(
self,
data: mtx.SceneData,
info: dict,
) -> dict[str, np.ndarray]:
commands = info["commands"]
return {
"lin_vel_z": self._reward_lin_vel_z(data),
"ang_vel_xy": self._reward_ang_vel_xy(data),
"orientation": self._reward_orientation(data),
"torques": self._reward_torques(data),
"dof_vel": self._reward_dof_vel(data),
"dof_acc": self._reward_dof_acc(data, info),
"action_rate": self._reward_action_rate(info),
"tracking_lin_vel": self._reward_tracking_lin_vel(data, commands),
"tracking_ang_vel": self._reward_tracking_ang_vel(data, commands),
"stand_still": self._reward_stand_still(data, commands),
"hip_pos": self._reward_hip_pos(data, commands),
"calf_pos": self._reward_calf_pos(data, commands),
"feet_air_time": self._reward_feet_air_time(commands, info),
}
# ------------ reward functions----------------
def _reward_lin_vel_z(self, data):
# Penalize z axis base linear velocity
return np.square(self.get_local_linvel(data)[:, 2])
def _reward_ang_vel_xy(self, data):
# Penalize xy axes base angular velocity
return np.sum(np.square(self.get_gyro(data)[:, :2]), axis=1)
def _reward_orientation(self, data):
# Penalize non flat base orientation
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData):
# Penalize torques
return np.sum(np.square(data.actuator_ctrls), axis=1)
def _reward_dof_vel(self, data):
# Penalize dof velocities
return np.sum(np.square(self.get_dof_vel(data)), axis=1)
def _reward_dof_acc(self, data, info):
# Penalize dof accelerations
return np.sum(
np.square((info["last_dof_vel"] - self.get_dof_vel(data)) / self.cfg.ctrl_dt),
axis=1,
)
def _reward_action_rate(self, info: dict):
# Penalize changes in actions
action_diff = info["current_actions"] - info["last_actions"]
return np.sum(np.square(action_diff), axis=1)
def _reward_termination(self, done):
# Terminal reward / penalty
return done
def _reward_feet_air_time(self, commands: np.ndarray, info: dict):
# Reward long steps
feet_air_time = info["feet_air_time"]
first_contact = (feet_air_time > 0.0) * info["contacts"]
# reward only on first contact with the ground
rew_airTime = np.sum((feet_air_time - 0.5) * first_contact, axis=1)
# no reward for zero command
rew_airTime *= np.linalg.norm(commands[:, :2], axis=1) > 0.1
return rew_airTime
def _reward_tracking_lin_vel(self, data, commands: np.ndarray):
# Tracking of linear velocity commands (xy axes)
lin_vel_error = np.sum(np.square(commands[:, :2] - self.get_local_linvel(data)[:, :2]), axis=1)
return np.exp(-lin_vel_error / self.cfg.reward_config.tracking_sigma)
def _reward_tracking_ang_vel(self, data, commands: np.ndarray):
# Tracking of angular velocity commands (yaw)
ang_vel_error = np.square(commands[:, 2] - self.get_gyro(data)[:, 2])
return np.exp(-ang_vel_error / self.cfg.reward_config.tracking_sigma)
def _reward_stand_still(self, data, commands: np.ndarray):
# Penalize motion at zero commands
return np.sum(np.abs(self.get_dof_pos(data) - self.default_angles), axis=1) * (
np.linalg.norm(commands, axis=1) < 0.1
)
def _reward_hip_pos(self, data, commands: np.ndarray):
return (0.8 - np.abs(commands[:, 1])) * np.sum(
np.square(self.get_dof_pos(data)[:, self.hip_indices] - self.default_angles[self.hip_indices]),
axis=1,
)
def _reward_calf_pos(self, data, commands: np.ndarray):
return (0.8 - np.abs(commands[:, 1])) * np.sum(
np.square(self.get_dof_pos(data)[:, self.calf_indices] - self.default_angles[self.calf_indices]),
axis=1,
)
def border_check(self, data, info: dict):
# check whether the robot reaching into the terrain border and change the move direction
border_size = 19.0
position = self._body.get_position(data)
is_out = (np.square(position[:, :2]) > border_size**2).any(axis=1)
info["commands"][is_out] = [0, 0, 0]

444
motrix_envs/src/motrix_envs/locomotion/go1/walk_stairs_terrain.py Normal file
View File

@@ -0,0 +1,444 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.locomotion.go1.cfg import Go1WalkNpStairsEnvCfg
from motrix_envs.math.quaternion import Quaternion
from motrix_envs.np.env import NpEnv, NpEnvState
from .common import generate_repeating_array
@registry.env("go1-stairs-terrain-walk", sim_backend="np")
class Go1WalkStairsTask(NpEnv):
_init_dof_pos: np.ndarray
_init_dof_vel: np.ndarray
def __init__(self, cfg: Go1WalkNpStairsEnvCfg, num_envs=1):
super().__init__(cfg, num_envs)
self._init_action_space()
self._init_obs_space()
self._body = self._model.get_body(self.cfg.asset.body_name)
self._num_action = self._action_space.shape[0]
self._num_observation = self._observation_space.shape[0]
self._num_dof_pos = self._model.num_dof_pos
self._num_dof_vel = self._model.num_dof_vel
self._init_dof_vel = np.zeros(
(self._num_dof_vel,),
dtype=np.float32,
)
height_list = np.array([-1, 0.5, 1.5])
offset_h = [[2, 0, 2, 1, 1], [2, 2, 1, 0, 0], [1, 1, 2, 1, 2], [0, 1, 0, 2, 0], [0, 1, 1, 0, 2]]
offset = []
for i in range(5):
for j in range(5):
h_index = offset_h[j][i]
offset.append([(i - 2) * 8.0, (j - 2) * 8.0, height_list[h_index]])
self.offset_list = np.array(offset)
self._init_dof_pos = self._model.compute_init_dof_pos()
self._init_buffer()
self.period_counter = 0
def _init_obs_space(self):
model = self.model
num_dof_vel = model.num_dof_vel # linvel + gyro + joint_vel
num_joint_angle = model.num_dof_pos - 7
num_gravity = 3
num_actions = model.num_actuators
num_command = 3
num_contact_force = 12
num_obs = num_dof_vel + num_joint_angle + num_gravity + num_actions + num_command + num_contact_force
assert num_obs == 60
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (num_obs,), dtype=np.float32)
def _init_action_space(self):
model = self.model
self._action_space = gym.spaces.Box(
np.array(model.actuator_ctrl_limits[0, :]),
np.array(model.actuator_ctrl_limits[1, :]),
(model.num_actuators,),
dtype=np.float32,
)
@property
def action_space(self) -> gym.spaces.Box:
return self._action_space
@property
def observation_space(self) -> gym.spaces.Box:
return self._observation_space
def get_dof_pos(self, data: mtx.SceneModel):
return self._body.get_joint_dof_pos(data)
def get_dof_vel(self, data: mtx.SceneModel):
return self._body.get_joint_dof_vel(data)
def _init_buffer(self):
cfg = self._cfg
assert isinstance(cfg, Go1WalkNpStairsEnvCfg)
# init buffers
self.reset_buf = np.ones(self._num_envs, dtype=np.bool)
self.kps = np.ones(self._num_action, dtype=np.float32) * cfg.control_config.stiffness
self.kds = np.ones(self._num_action, dtype=np.float32) * cfg.control_config.damping
self.gravity_vec = np.array([0, 0, -1], dtype=np.float32)
self.commands_scale = np.array(
(
[
cfg.normalization.lin_vel,
cfg.normalization.lin_vel,
cfg.normalization.ang_vel,
]
),
dtype=np.float32,
)
self.default_angles = np.zeros(self._num_action, dtype=np.float32)
self.hip_indices = []
self.calf_indices = []
for i in range(self._model.num_actuators):
for name in cfg.init_state.default_joint_angles.keys():
if name in self._model.actuator_names[i]:
self.default_angles[i] = cfg.init_state.default_joint_angles[name]
if "hip" in self._model.actuator_names[i]:
self.hip_indices.append(i)
if "calf" in self._model.actuator_names[i]:
self.calf_indices.append(i)
self._init_dof_pos[-self._num_action :] = self.default_angles
self.ground = []
for geom_name in self._model.geom_names:
if geom_name is not None and cfg.asset.ground_name in geom_name:
self.ground.append(self._model.get_geom_index(geom_name))
self.termination_contact = None
self.foot = []
for gournd_index in self.ground:
for name in cfg.asset.terminate_after_contacts_on:
if self.termination_contact is None:
self.termination_contact = np.array(
[[self._model.get_geom_index(name), gournd_index]], dtype=np.uint32
)
else:
self.termination_contact = np.append(
self.termination_contact,
np.array(
[[self._model.get_geom_index(name), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.num_check = self.termination_contact.shape[0]
self.foot = None
for gournd_index in self.ground:
for i in self._model.geom_names:
if i is not None and cfg.asset.foot_name in i:
if self.foot is None:
self.foot = np.array([[self._model.get_geom_index(i), gournd_index]], dtype=np.uint32)
else:
self.foot = np.append(
self.foot,
np.array(
[[self._model.get_geom_index(i), gournd_index]],
dtype=np.uint32,
),
axis=0,
)
self.foot_check_num = self.foot.shape[0]
self.foot_check = self.foot
self.termination_check = self.termination_contact
spacing = 2.0
cols = int(np.ceil(np.sqrt(self._num_envs)))
offsets = []
for i in range(self._num_envs):
row = i // cols
col = i % cols
x = col * spacing
y = row * spacing
z = 0.0
offsets.append([x, y, z])
self.offsets = np.array(offsets)
def apply_action(self, actions, state):
state.info["last_dof_vel"] = self.get_dof_vel(state.data)
state.info["last_actions"] = state.info["current_actions"]
state.info["current_actions"] = actions
state.data.actuator_ctrls = self._compute_torques(actions, state.data)
return state
def _compute_torques(self, actions, data):
# Compute torques from actions.
# pd controller
actions_scaled = actions * self.cfg.control_config.action_scale
torques = self.kps * (
actions_scaled + self.default_angles - self.get_dof_pos(data)
) - self.kds * self.get_dof_vel(data)
return torques
def get_local_linvel(self, data: mtx.SceneData) -> np.ndarray:
return self._model.get_sensor_value(self.cfg.sensor.local_linvel, data)
def get_gyro(self, data: mtx.SceneData) -> np.ndarray:
return self._model.get_sensor_value(self.cfg.sensor.gyro, data)
def update_state(self, state):
state = self.update_observation(state)
state = self.update_terminated(state)
state = self.update_reward(state)
return state
def _get_obs(self, data: mtx.SceneData, info: dict) -> np.ndarray:
linear_vel = self.get_local_linvel(data)
gyro = self.get_gyro(data)
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
local_gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
diff = self.get_dof_pos(data) - self.default_angles
noisy_linvel = linear_vel * self.cfg.normalization.lin_vel
noisy_gyro = gyro * self.cfg.normalization.ang_vel
noisy_joint_angle = diff * self.cfg.normalization.dof_pos
noisy_joint_vel = self.get_dof_vel(data) * self.cfg.normalization.dof_vel
command = info["commands"] * self.commands_scale
last_actions = info["current_actions"]
contact_force = info["contact_force"]
obs = np.hstack(
[
noisy_linvel,
noisy_gyro,
local_gravity,
noisy_joint_angle,
noisy_joint_vel,
last_actions,
command,
contact_force,
]
)
return obs
def update_observation(self, state: NpEnvState):
data = state.data
# self.border_check(data, state.info)
obs = self._get_obs(data, state.info)
cquerys = self._model.get_contact_query(data)
foot_contact = cquerys.is_colliding(self.foot_check)
state.info["contacts"] = foot_contact.reshape((self._num_envs, self.foot_check_num))
state.info["feet_air_time"] = self.update_feet_air_time(state.info)
state.info["contact_force"] = self.update_contact_force(state)
return state.replace(obs=obs)
def update_terminated(self, state: NpEnvState) -> NpEnvState:
data = state.data
cquerys = self._model.get_contact_query(data)
termination_check = cquerys.is_colliding(self.termination_check)
termination_check.reshape((self._num_envs, self.num_check))
terminated = termination_check.any(axis=1)
over_speed = np.sum(np.square(self.get_local_linvel(data)[:, :2]), axis=1) > 1e8
terminated = terminated | over_speed
return state.replace(
terminated=terminated,
)
def update_feet_air_time(self, info: dict):
feet_air_time = info["feet_air_time"]
feet_air_time += self.cfg.ctrl_dt
feet_air_time *= ~info["contacts"]
return feet_air_time
def update_contact_force(self, state: NpEnvState):
data = state.data
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
force = []
for foot in self.cfg.sensor.feet:
contact_force = self._model.get_sensor_value(foot + "_foot_contact", data)
contact_force = Quaternion.rotate_inverse(base_quat, contact_force)
force.append(contact_force)
return np.concatenate(force, axis=1)
def resample_commands(self, num_envs: int):
commands = np.random.uniform(
low=self.cfg.commands.vel_limit[0],
high=self.cfg.commands.vel_limit[1],
size=(num_envs, 3),
)
return commands
def update_reward(self, state: NpEnvState) -> NpEnvState:
data = state.data
terminated = state.terminated
reward_dict = self._get_reward(data, state.info)
rewards = {k: v * self.cfg.reward_config.scales[k] for k, v in reward_dict.items()}
rwd = sum(rewards.values())
rwd = np.clip(rwd, 0.0, 10000.0)
if "termination" in self.cfg.reward_config.scales:
termination = self._reward_termination(terminated) * self.cfg.reward_config.scales["termination"]
rwd += termination
rwd = np.where(terminated, np.array(0.0), rwd)
return state.replace(reward=rwd)
def reset(self, data) -> tuple[np.ndarray, dict]:
num_reset = data.shape[0]
dof_pos = np.tile(self._init_dof_pos, (num_reset, 1))
dof_vel = np.tile(self._init_dof_vel, (num_reset, 1))
num_period = 25
idx = generate_repeating_array(num_period, num_reset, self.period_counter)
self.period_counter = (self.period_counter + num_reset) % num_period
dof_pos[:, :3] = self.offset_list[idx]
data.reset(self._model)
data.set_dof_vel(dof_vel)
data.set_dof_pos(dof_pos, self._model)
self._model.forward_kinematic(data)
info = {
"current_actions": np.zeros((num_reset, self._num_action), dtype=np.float32),
"last_actions": np.zeros((num_reset, self._num_action), dtype=np.float32),
"commands": self.resample_commands(num_reset),
"last_dof_vel": np.zeros((num_reset, self._num_action), dtype=np.float32),
"feet_air_time": np.zeros((num_reset, self.foot_check_num), dtype=np.float32),
"contacts": np.zeros((num_reset, self.foot_check_num), dtype=np.bool),
"contact_force": np.zeros((num_reset, 12), dtype=np.float32),
}
obs = self._get_obs(data, info)
return obs, info
def _get_reward(
self,
data: mtx.SceneData,
info: dict,
) -> dict[str, np.ndarray]:
commands = info["commands"]
return {
"lin_vel_z": self._reward_lin_vel_z(data),
"ang_vel_xy": self._reward_ang_vel_xy(data),
"orientation": self._reward_orientation(data),
"torques": self._reward_torques(data),
"dof_vel": self._reward_dof_vel(data),
"dof_acc": self._reward_dof_acc(data, info),
"action_rate": self._reward_action_rate(info),
"tracking_lin_vel": self._reward_tracking_lin_vel(data, commands),
"tracking_ang_vel": self._reward_tracking_ang_vel(data, commands),
"stand_still": self._reward_stand_still(data, commands),
"hip_pos": self._reward_hip_pos(data, commands),
"calf_pos": self._reward_calf_pos(data, commands),
"feet_air_time": self._reward_feet_air_time(commands, info),
"feet_stumble": self._reward_feet_stumble(data),
}
# ------------ reward functions----------------
def _reward_lin_vel_z(self, data):
# Penalize z axis base linear velocity
return np.square(self.get_local_linvel(data)[:, 2])
def _reward_ang_vel_xy(self, data):
# Penalize xy axes base angular velocity
return np.sum(np.square(self.get_gyro(data)[:, :2]), axis=1)
def _reward_orientation(self, data):
# Penalize non flat base orientation
pose = self._body.get_pose(data)
base_quat = pose[:, 3:7]
gravity = Quaternion.rotate_inverse(base_quat, self.gravity_vec)
return np.sum(np.square(gravity[:, :2]), axis=1)
def _reward_torques(self, data: mtx.SceneData):
# Penalize torques
return np.sum(np.square(data.actuator_ctrls), axis=1)
def _reward_dof_vel(self, data):
# Penalize dof velocities
return np.sum(np.square(self.get_dof_vel(data)), axis=1)
def _reward_dof_acc(self, data, info):
# Penalize dof accelerations
return np.sum(
np.square((info["last_dof_vel"] - self.get_dof_vel(data)) / self.cfg.ctrl_dt),
axis=1,
)
def _reward_action_rate(self, info: dict):
# Penalize changes in actions
action_diff = info["current_actions"] - info["last_actions"]
return np.sum(np.square(action_diff), axis=1)
def _reward_termination(self, done):
# Terminal reward / penalty
return done
def _reward_feet_air_time(self, commands: np.ndarray, info: dict):
# Reward long steps
feet_air_time = info["feet_air_time"]
first_contact = (feet_air_time > 0.0) * info["contacts"]
# reward only on first contact with the ground
rew_airTime = np.sum((feet_air_time - 0.5) * first_contact, axis=1)
# no reward for zero command
rew_airTime *= np.linalg.norm(commands[:, :2], axis=1) > 0.1
return rew_airTime
def _reward_tracking_lin_vel(self, data, commands: np.ndarray):
# Tracking of linear velocity commands (xy axes)
lin_vel_error = np.sum(np.square(commands[:, :2] - self.get_local_linvel(data)[:, :2]), axis=1)
return np.exp(-lin_vel_error / self.cfg.reward_config.tracking_sigma)
def _reward_tracking_ang_vel(self, data, commands: np.ndarray):
# Tracking of angular velocity commands (yaw)
ang_vel_error = np.square(commands[:, 2] - self.get_gyro(data)[:, 2])
return np.exp(-ang_vel_error / self.cfg.reward_config.tracking_sigma)
def _reward_stand_still(self, data, commands: np.ndarray):
# Penalize motion at zero commands
return np.sum(np.abs(self.get_dof_pos(data) - self.default_angles), axis=1) * (
np.linalg.norm(commands, axis=1) < 0.1
)
def _reward_hip_pos(self, data, commands: np.ndarray):
return (0.8 - np.abs(commands[:, 1])) * np.sum(
np.square(self.get_dof_pos(data)[:, self.hip_indices] - self.default_angles[self.hip_indices]),
axis=1,
)
def _reward_calf_pos(self, data, commands: np.ndarray):
return (0.8 - np.abs(commands[:, 1])) * np.sum(
np.square(self.get_dof_pos(data)[:, self.calf_indices] - self.default_angles[self.calf_indices]),
axis=1,
)
def _reward_feet_stumble(self, data):
# Penalize feet hitting vertical surfaces
is_stumble = 0
for foot in self.cfg.sensor.feet:
contact_force = self._model.get_sensor_value(foot + "_foot_contact", data)
is_stumble += (np.linalg.norm(contact_force, axis=1) > 5 * np.abs(contact_force[:, 2])) * 1.0
return is_stumble

3
motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/heightmap.png Normal file
View File

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

3
motrix_envs/src/motrix_envs/locomotion/go1/xmls/assets/heightmap_stairs.hfield Normal file
View File

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

7
motrix_envs/src/motrix_envs/locomotion/go1/xmls/materials.xml Normal file
View File

@@ -0,0 +1,7 @@
<mujoco>
<asset>
<texture name="motphys-ground" type="2d" file="../../../common/motphys-ground.png" />
<material name="motphys-ground" texture="motphys-ground" texuniform="true"
texrepeat="0.4 0.4" />
</asset>
</mujoco>

54
motrix_envs/src/motrix_envs/locomotion/go1/xmls/scene_motor_actuator.xml
View File

@@ -1,48 +1,50 @@
<mujoco model="go1 feetonly flat terrain scene">
<include file="go1_motor_actuator.xml"/>
<statistic center="0 0 0.1" extent="0.8" meansize="0.04"/>
<include file="go1_motor_actuator.xml" />
<include file="materials.xml" />
<statistic center="0 0 0.1" extent="0.8" meansize="0.04" />
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0"/>
<rgba haze="0.15 0.25 0.35 1"/>
<global azimuth="120" elevation="-20"/>
<map force="0.01"/>
<scale forcewidth="0.3" contactwidth="0.5" contactheight="0.2"/>
<quality shadowsize="8192"/>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
<rgba haze="0.15 0.25 0.35 1" />
<global azimuth="120" elevation="-20" />
<map force="0.01" />
<scale forcewidth="0.3" contactwidth="0.5" contactheight="0.2" />
<quality shadowsize="8192" />
</visual>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4314 0.5294 0.6431" rgb2="0 0 0" width="512" height="512"/>
<texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.4314 0.5294 0.6431" rgb2="0.8157 0.8549 0.9059"
markrgb="0.8 0.8 0.8" width="300" height="300"/>
<material name="groundplane" texture="groundplane" texuniform="true" texrepeat="1 1" reflectance="0.2"/>
<texture type="skybox" builtin="gradient" rgb1="0.4314 0.5294 0.6431" rgb2="0 0 0" width="512"
height="512" />
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
<geom name="floor" size="0 0 0.01" type="plane" material="groundplane" contype="1" conaffinity="0" priority="1"
friction="0.6" condim="3"/>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" size="0 0 0.01" type="plane" material="motphys-ground" contype="1"
conaffinity="0" priority="1"
friction="0.6" condim="3" />
</worldbody>
<keyframe>
<key name="home" qpos="
<key name="home"
qpos="
0 0 0.278
1 0 0 0
0.1 0.9 -1.8
-0.1 0.9 -1.8
0.1 0.9 -1.8
-0.1 0.9 -1.8"
ctrl="0.1 0.9 -1.8 -0.1 0.9 -1.8 0.1 0.9 -1.8 -0.1 0.9 -1.8"/>
<key name="home_higher" qpos="0 0 0.31 1 0 0 0 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63"
ctrl="0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63"/>
ctrl="0.1 0.9 -1.8 -0.1 0.9 -1.8 0.1 0.9 -1.8 -0.1 0.9 -1.8" />
<key name="home_higher"
qpos="0 0 0.31 1 0 0 0 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63"
ctrl="0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63 0 0.82 -1.63" />
<key
name="footstand"
qpos='
name="footstand"
qpos='
0 0 0.54
0.8 0 -0.8 0
0 0.82 -1.6 0 0.82 -1.68 0 1.82 -1.16 0.0 1.82 -1.16'
ctrl='0 0.82 -1.6 0 0.82 -1.68 0 1.82 -1.16 0.0 1.82 -1.16'/>
ctrl='0 0.82 -1.6 0 0.82 -1.68 0 1.82 -1.16 0.0 1.82 -1.16' />
<key name="handstand"
qpos="0 0 0.54
0.8 0 0.8 0
@@ -50,9 +52,9 @@
0 -0.686 -1.16
0 1.7 -1.853
0 1.7 -1.853"
ctrl="0 -0.686 -1.16 0 -0.686 -1.16 0 1.7 -1.853 0 1.7 -1.853"/>
ctrl="0 -0.686 -1.16 0 -0.686 -1.16 0 1.7 -1.853 0 1.7 -1.853" />
<key name="pre_recovery"
qpos="-0.0318481 -0.000215369 0.0579031 1 -2.70738e-05 6.06169e-05 0.000231261 -0.352275 1.18554 -2.80738 0.360892 1.1806 -2.80281 -0.381197 1.16812 -2.79123 0.391054 1.1622 -2.78576"
ctrl="-0.352275 1.18554 -2.80738 0.360892 1.1806 -2.80281 -0.381197 1.16812 -2.79123 0.391054 1.1622 -2.78576"/>
ctrl="-0.352275 1.18554 -2.80738 0.360892 1.1806 -2.80281 -0.381197 1.16812 -2.79123 0.391054 1.1622 -2.78576" />
</keyframe>
</mujoco>
</mujoco>

26
motrix_envs/src/motrix_envs/locomotion/go1/xmls/scene_rough_terrain.xml Normal file
View File

@@ -0,0 +1,26 @@
<mujoco model="go1 feetonly rough terrain scene">
<include file="go1_motor_actuator.xml" />
<include file="materials.xml" />
<statistic center="0 0 0.1" extent="0.8" meansize="0.04" />
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
<rgba haze="0.15 0.25 0.35 1" />
<global azimuth="120" elevation="-20" />
<map force="0.01" />
<scale forcewidth="0.3" contactwidth="0.5" contactheight="0.2" />
<quality shadowsize="8192" />
</visual>
<asset>
<hfield name="hfield" file="assets/heightmap.png" size="20 20 2.5 0.1" />
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" pos="0 0 -3" size="0 0 0.01" type="plane" material="motphys-ground"
contype="1" conaffinity="0" priority="1" friction="0.6" condim="3" />
<geom name="floor_rough" pos="0 0 -1" type="hfield" hfield="hfield" material="motphys-ground"
contype="1" conaffinity="0" priority="1" friction="0.6" />
</worldbody>
</mujoco>

32
motrix_envs/src/motrix_envs/locomotion/go1/xmls/scene_stairs_terrain.xml Normal file
View File

@@ -0,0 +1,32 @@
<mujoco model="go1 stairs terrain scene">
<include file="go1_motor_actuator.xml" />
<include file="materials.xml" />
<statistic center="0 0 0.1" extent="0.8" meansize="0.04" />
<visual>
<headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
<rgba haze="0.15 0.25 0.35 1" />
<global azimuth="120" elevation="-20" />
<map force="0.01" />
<scale forcewidth="0.3" contactwidth="0.5" contactheight="0.2" />
<quality shadowsize="8192" />
</visual>
<asset>
<hfield name="hfield" file="assets/heightmap_stairs.hfield" size="45 45 3.0 4" />
</asset>
<worldbody>
<light pos="0 0 1.5" dir="0 0 -1" directional="true" />
<geom name="floor" pos="0 0 -1.89" size="1 1 1" type="hfield" hfield="hfield"
material="motphys-ground" />
</worldbody>
<sensor>
<contact name="FR_foot_contact" geom2="FR_foot" geom1="floor" data="normal" num="1" />
<contact name="RR_foot_contact" geom2="RR_foot" geom1="floor" data="normal " num="1" />
<contact name="FL_foot_contact" geom2="FL_foot" geom1="floor" data="normal" num="1" />
<contact name="RL_foot_contact" geom2="RL_foot" geom1="floor" data="normal" num="1" />
</sensor>
</mujoco>

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

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

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

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

83
motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/cfg.py Executable file
View File

@@ -0,0 +1,83 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import os
from dataclasses import dataclass, field
import numpy as np
from motrix_envs import registry
from motrix_envs.base import EnvCfg
model_file = os.path.dirname(__file__) + "/xmls/mjx_scene.xml"
@dataclass
class InitState:
# robot joint names and default positions [rad]
joint_names = [
"joint1",
"joint2",
"joint3",
"joint4",
"joint5",
"joint6",
"joint7",
"finger_joint1",
"finger_joint2",
]
default_joint_pos = np.array([0.0, -0.569, 0.0, -2.810, 0.0, 3.037, 0.741, 0.04, 0.04], np.float32)
joint_pos_reset_noise_scale = 0.125
@dataclass
class ControlConfig:
# Position control
# The actuator defined in xml file is <position ..../>
# From ctrlrange in actuator in xml
# Using position control and action as offset effectively solves the problem of large joint angle changes
actuators = ["actuator1", "actuator2", "actuator3", "actuator4", "actuator5", "actuator6", "actuator7", "actuator8"]
min_pos = [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -np.pi / 2, 0]
max_pos = [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, np.pi / 2, 0.04]
@dataclass
class Commands:
target_pos_x = [0.4, 0.6]
target_pos_y = [-0.25, 0.25]
target_pos_z = [0.25, 0.5]
@dataclass
class Asset:
ground_name = "table"
terminate_after_contacts_on = ["left_finger_pad", "left_finger_pad"]
@registry.envcfg("franka-lift-cube")
@dataclass
class FrankaLiftCubeEnvCfg(EnvCfg):
render_spacing: float = 2.0
model_file: str = model_file
max_episode_seconds: float = 2.5
sim_dt: float = 0.01
move_speed: float = 1.0
ctrl_dt: float = 0.01
reset_noise_scale = 0.05
init_state: InitState = field(default_factory=InitState)
control_config: ControlConfig = field(default_factory=ControlConfig)
command_config: Commands = field(default_factory=Commands)
asset: Asset = field(default_factory=Asset)

291
motrix_envs/src/motrix_envs/manipulation/franka_lift_cube/franka_lift_cube_np.py Executable file
View File

@@ -0,0 +1,291 @@
# Copyright (C) 2020-2025 Motphys Technology Co., Ltd. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ==============================================================================
import gymnasium as gym
import motrixsim as mtx
import numpy as np
from motrix_envs import registry
from motrix_envs.np.env import NpEnv, NpEnvState
from .cfg import FrankaLiftCubeEnvCfg
# Decay parameters (constants, can be defined during class initialization)
START_EPSILON = 1.0 # Initial value
MIN_EPSILON = 0.05 # Minimum value (typically 0.01 or 0.05)
# Assume we want to complete decay in half of total steps (12000 steps)
END_STEP = 12000
@registry.env("franka-lift-cube", "np")
class FrankaLiftCubeEnv(NpEnv):
_cfg: FrankaLiftCubeEnvCfg
def __init__(self, cfg: FrankaLiftCubeEnvCfg, num_envs: int = 1):
super().__init__(cfg, num_envs=num_envs)
self.default_joint_pos = self._cfg.init_state.default_joint_pos
self._action_dim = 8
self._obs_dim = 36 # 9 + 9 + 3 + 7 + 8
self._action_space = gym.spaces.Box(-np.inf, np.inf, (self._action_dim,), dtype=np.float32)
self._observation_space = gym.spaces.Box(-np.inf, np.inf, (self._obs_dim,), dtype=np.float32)
self._num_dof_pos = 9 # self._model.num_dof_pos # 9
self._num_dof_vel = 9 # self._model.num_dof_vel # 9
self._init_dof_pos = self.default_joint_pos
self._init_dof_vel = np.zeros(self._num_dof_vel, dtype=np.float32)
self._cube = self._model.get_geom("cube")
self._body = self._model.get_body("link0")
self.hand = self._model.get_site("gripper")
self.joint_pos_min_limit = self._cfg.control_config.min_pos
self.joint_pos_max_limit = self._cfg.control_config.max_pos
self.epsilon = START_EPSILON
self._state_for_render = None
self.count = 0
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
def apply_action(self, actions: np.ndarray, state: NpEnvState):
state.info["last_actions"] = state.info["current_actions"]
state.info["current_actions"] = actions
# no gripper
old_joint_pos = self.get_dof_pos(state.data)[:, : self._action_dim - 1]
new_joint_pos = actions[:, : self._action_dim - 1] + old_joint_pos # action as offset
# with gripper
# 1. Map to probability p (using Sigmoid)
probabilities = 1 / (1 + np.exp(-actions[:, -1]))
# 2. Bernoulli sampling - probability always has chance to sample different results
# np.random.uniform(0, 1, size) generates a random number r ~ U(0, 1) for each environment
# If r < p, result is 1 (success/grasp), otherwise 0 (failure/release)
sampled_gripper_action = np.where(probabilities > np.random.rand(*probabilities.shape), 0, 0.04)[
:, None
] # Close 0, Open 0.04
state.info["current_gripper_action"] = sampled_gripper_action.squeeze(axis=-1)
new_pos = np.concatenate([new_joint_pos, sampled_gripper_action], axis=-1)
# step action
cliped_new_pos = np.clip(
new_pos, self.joint_pos_min_limit, self.joint_pos_max_limit, dtype=np.float32
) # clip new pos to limit
state.data.actuator_ctrls = cliped_new_pos
return state
def update_state(self, state: NpEnvState):
self._state_for_render = state
# compute observation
obs = self._compute_observation(state.data, state.info)
# compute truncated
truncated = self._check_termination(state)
# compute reward
reward = self._compute_reward(state, truncated)
state.obs = obs
state.reward = reward
state.terminated = truncated # np.logical_or(truncated, done)
self.count += 1
return state
def reset(self, data: mtx.SceneData):
num_reset = data.shape[0]
# Robot arm initial joint angle noise
noise_pos = np.random.uniform(
-self._cfg.init_state.joint_pos_reset_noise_scale,
self._cfg.init_state.joint_pos_reset_noise_scale,
self._num_dof_pos,
)
robot_dof_pos = self._init_dof_pos + noise_pos
# Domain randomization for cube position
# x -0.1, 0.1
# y -0.25, 0.25
x_low, x_high = -0.1, 0.1
y_low, y_high = -0.25, 0.25
pos_x = np.random.uniform(x_low, x_high)
pos_y = np.random.uniform(y_low, y_high)
scene_dof_pos = np.concatenate(
[robot_dof_pos, np.array([pos_x, pos_y, 0.05, 1, 0, 0, 0], dtype=np.float32)]
) # Added cube
scene_dof_pos = np.tile(scene_dof_pos, (num_reset, 1))
scene_dof_vel = np.concatenate([self._init_dof_vel, np.zeros(6, dtype=np.float32)])
scene_dof_vel = np.tile(scene_dof_vel, (num_reset, 1))
# Reset
data.reset(self._model)
data.set_dof_vel(scene_dof_vel)
data.set_dof_pos(scene_dof_pos, self._model)
self._model.forward_kinematic(data)
info = {
"current_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32),
"last_actions": np.zeros((num_reset, self._action_dim), dtype=np.float32),
"commands": self._generated_commands(num_reset), #
"current_gripper_action": np.zeros(num_reset, dtype=np.float32), # 1D
}
# Check for nan
assert not np.isnan(info["commands"]).any(), "commands contain nan"
obs = self._compute_observation(data, info)
return obs, info
def _compute_observation(self, data: mtx.SceneData, info: dict):
dof_pos = self.get_dof_pos(data) # shape: # not necessarily (self.num_envs, 9)
dof_vel = self.get_dof_vel(data) # shape: # not necessarily (num_envs, 9)
dof_pos_rel = self._get_joint_pos_rel(dof_pos)
dof_vel_rel = self._get_joint_vel_rel(dof_vel)
object_pick_pose = self._cube.get_pose(data)
object_lift_pos = info["commands"]
last_actions = info["current_actions"]
obs = np.concatenate([dof_pos_rel, dof_vel_rel, object_pick_pose, object_lift_pos, last_actions], axis=-1)
assert obs.shape == (data.shape[0], self._obs_dim)
assert not np.isnan(obs).any(), "obs contain nan"
return obs.astype(np.float32)
def _check_termination(self, state: NpEnvState):
cube_height = self._cube.get_pose(state.data)[:, 2]
truncated = cube_height < -0.05 # New truncated condition
# Check joint velocity is not too large (set to 5 radians per second here)
joint_vel = self.get_dof_vel(state.data)
truncated = np.logical_or(truncated, np.abs(joint_vel).max(axis=-1) > 10)
# Check cube velocity
cube_vel = self._cube.get_linear_velocity(state.data) # shape = (*data.shape, 3).
truncated = np.logical_or(truncated, np.abs(cube_vel).max(axis=-1) > 10)
return truncated
def _compute_reward(self, state: NpEnvState, truncated: np.ndarray):
hand_pose = self.hand.get_pose(state.data)
hand_pos = hand_pose[:, :3]
cube_pos = self._cube.get_pose(state.data)[:, :3]
# reach reward
hand_cube_distance = np.linalg.norm(cube_pos - hand_pos, axis=-1)
std = 0.1
reach_reward = 1 - np.tanh(hand_cube_distance / std)
# lift reward
lift_height = cube_pos[:, 2] # Cube center of mass height - initial center of mass height 0.02 = lift height
minimal_height = 0.04 # 4cm height limit
lifted = lift_height > minimal_height
# object_command_tracking reward
object_command_dist = np.linalg.norm(cube_pos - state.info["commands"], axis=-1)
def shifted_sigmoid_reward(d, k=8, center=0.3):
# Sigmoid(-k * (d - center))
# The larger d is, the more positive (d-center) is, the more negative -k*(...) is, Sigmoid closer to 0
# The smaller d is, the more negative (d-center) is, the more positive -k*(...) is, Sigmoid closer to 1
x = -k * (d - center)
return 1 / (1 + np.exp(-x))
object_command_tracking_reward = (
shifted_sigmoid_reward(object_command_dist) * (lift_height > 0.04) * (hand_cube_distance < 0.02)
)
object_command_tracking_fine_graind_reward = (
(1 - np.tanh(object_command_dist / 0.4)) * (lift_height > 0.04) * (hand_cube_distance < 0.02)
)
object_command_tracking_close_reward = (
(1 - np.tanh(object_command_dist / 0.05)) * (object_command_dist < 0.2) * (hand_cube_distance < 0.02)
)
# action_diff_sq: Sum of squares of action changes
action_diff_sq = np.sum(np.square(state.info["current_actions"] - state.info["last_actions"]), axis=-1)
# joint_vel_sq: Sum of squares of joint velocities
joint_vel_sq = np.sum(np.square(self.get_dof_vel(state.data)[:, : self._num_dof_vel]), axis=1)
## action penalty rate
reach_weight = 1.5 # Cannot be too small
cmd_tracking_weight = 10
cmd_tracking_fine_graind_weight = 20 # Should be larger, need strong pull to target area
object_command_tracking_close_reward_weight = 10
if self.count < 10000:
action_penalty_rate = 1e-4
joint_vel_penalty_rate = 1e-4
else:
action_penalty_rate = 1e-1
joint_vel_penalty_rate = 1e-1
reward = (
reach_weight * reach_reward
+ 30 * lifted * (hand_cube_distance < 0.05)
+ (cmd_tracking_weight * object_command_tracking_reward) ** 2
+ (cmd_tracking_fine_graind_weight * object_command_tracking_fine_graind_reward) ** 2
+ (object_command_tracking_close_reward_weight * object_command_tracking_close_reward) ** 2
+ 200 * object_command_tracking_close_reward
+ -action_penalty_rate * action_diff_sq
+ -joint_vel_penalty_rate * joint_vel_sq
)
return reward
def get_dof_pos(self, data: mtx.SceneModel):
return self._body.get_joint_dof_pos(data)
def get_dof_vel(self, data: mtx.SceneModel):
return self._body.get_joint_dof_vel(data)
def _get_joint_pos_rel(self, dof_pos: np.ndarray):
return dof_pos - self.default_joint_pos
def _get_joint_vel_rel(self, dof_vel: np.ndarray):
return dof_vel - self._init_dof_vel
def _generated_commands(self, num_envs: int):
# Command is the final object_pose that cube should reach
x_low, x_high = self._cfg.command_config.target_pos_x
y_low, y_high = self._cfg.command_config.target_pos_y
z_low, z_high = self._cfg.command_config.target_pos_z
pos_x = np.random.uniform(x_low, x_high, num_envs)
pos_y = np.random.uniform(y_low, y_high, num_envs)
pos_z = np.random.uniform(z_low, z_high, num_envs)
command_cube_target_pos = np.stack([pos_x, pos_y, pos_z], axis=-1)
assert not np.isnan(command_cube_target_pos).any(), "command_cube_target_pos contain nan"
return command_cube_target_pos

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

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

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