diff --git a/src/mjlab/tasks/velocity/config/g1/env_cfgs.py b/src/mjlab/tasks/velocity/config/g1/env_cfgs.py index 6c4a66975..8df322225 100644 --- a/src/mjlab/tasks/velocity/config/g1/env_cfgs.py +++ b/src/mjlab/tasks/velocity/config/g1/env_cfgs.py @@ -70,7 +70,16 @@ def unitree_g1_rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg: ].site_names = site_names cfg.events["foot_friction"].params["asset_cfg"].geom_names = geom_names - + cfg.events["base_com"].params["asset_cfg"].body_names = ("torso_link",) + + # Rationale for std values: + # - Knees/hip_pitch get the loosest std to allow natural leg bending during stride. + # - Hip roll/yaw stay tighter to prevent excessive lateral sway and keep gait stable. + # - Ankle roll is very tight for balance; ankle pitch looser for foot clearance. + # - Waist roll/pitch stay tight to keep the torso upright and stable. + # - Shoulders/elbows get moderate freedom for natural arm swing during walking. + # - Wrists are loose (0.3) since they don't affect balance much. + # Running values are ~1.5-2x walking values to accommodate larger motion range. cfg.rewards["pose"].params["std_standing"] = {".*": 0.05} cfg.rewards["pose"].params["std_walking"] = { # Lower body. diff --git a/src/mjlab/tasks/velocity/config/go1/env_cfgs.py b/src/mjlab/tasks/velocity/config/go1/env_cfgs.py index 0d5e37008..17e3d039a 100644 --- a/src/mjlab/tasks/velocity/config/go1/env_cfgs.py +++ b/src/mjlab/tasks/velocity/config/go1/env_cfgs.py @@ -64,6 +64,7 @@ def unitree_go1_rough_env_cfg(play: bool = False) -> ManagerBasedRlEnvCfg: ].site_names = site_names cfg.events["foot_friction"].params["asset_cfg"].geom_names = geom_names + cfg.events["base_com"].params["asset_cfg"].body_names = ("trunk",) cfg.rewards["pose"].params["std_standing"] = { r".*(FR|FL|RR|RL)_(hip|thigh)_joint.*": 0.05, diff --git a/src/mjlab/tasks/velocity/mdp/rewards.py b/src/mjlab/tasks/velocity/mdp/rewards.py index 0b0d75755..265044af0 100644 --- a/src/mjlab/tasks/velocity/mdp/rewards.py +++ b/src/mjlab/tasks/velocity/mdp/rewards.py @@ -289,7 +289,20 @@ def soft_landing( class variable_posture: - """Penalize deviation from default pose, with tighter constraints when standing.""" + """Penalize deviation from default pose with speed-dependent tolerance. + + Uses per-joint standard deviations to control how much each joint can deviate + from default pose. Smaller std = stricter (less deviation allowed), larger + std = more forgiving. The reward is: exp(-mean(error² / std²)) + + Three speed regimes (based on linear + angular command velocity): + - std_standing (speed < walking_threshold): Tight tolerance for holding pose. + - std_walking (walking_threshold <= speed < running_threshold): Moderate. + - std_running (speed >= running_threshold): Loose tolerance for large motion. + + Tune std values per joint based on how much motion that joint needs at each + speed. Map joint name patterns to std values, e.g. {".*knee.*": 0.35}. + """ def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRlEnv): asset: Entity = env.scene[cfg.params["asset_cfg"].name] diff --git a/src/mjlab/tasks/velocity/velocity_env_cfg.py b/src/mjlab/tasks/velocity/velocity_env_cfg.py index cf9916755..34f2f6113 100644 --- a/src/mjlab/tasks/velocity/velocity_env_cfg.py +++ b/src/mjlab/tasks/velocity/velocity_env_cfg.py @@ -161,7 +161,16 @@ def make_velocity_env_cfg() -> ManagerBasedRlEnvCfg: func=mdp.push_by_setting_velocity, mode="interval", interval_range_s=(1.0, 3.0), - params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}}, + params={ + "velocity_range": { + "x": (-0.5, 0.5), + "y": (-0.5, 0.5), + "z": (-0.4, 0.4), + "roll": (-0.52, 0.52), + "pitch": (-0.52, 0.52), + "yaw": (-0.78, 0.78), + }, + }, ), "foot_friction": EventTermCfg( mode="startup", @@ -174,6 +183,29 @@ def make_velocity_env_cfg() -> ManagerBasedRlEnvCfg: "ranges": (0.3, 1.2), }, ), + "encoder_bias": EventTermCfg( + mode="startup", + func=mdp.randomize_encoder_bias, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "bias_range": (-0.015, 0.015), + }, + ), + "base_com": EventTermCfg( + mode="startup", + func=mdp.randomize_field, + domain_randomization=True, + params={ + "asset_cfg": SceneEntityCfg("robot", body_names=()), # Set per-robot. + "operation": "add", + "field": "body_ipos", + "ranges": { + 0: (-0.025, 0.025), + 1: (-0.025, 0.025), + 2: (-0.03, 0.03), + }, + }, + ), } ##