diff --git a/scripts/reinforcement_learning/rsl_rl/export.py b/scripts/reinforcement_learning/rsl_rl/export.py index 5b4d20f10218..a47c2a239fca 100644 --- a/scripts/reinforcement_learning/rsl_rl/export.py +++ b/scripts/reinforcement_learning/rsl_rl/export.py @@ -1,6 +1,11 @@ # scripts/reinforcement_learning/rsl_rl/export.py # # Export a trained rsl_rl policy as Kinfer binary. +# +# MODIFIED: This export script now accepts 16-dimensional command vectors from the deployment +# platform but only uses the first 3 dimensions (lin_vel_x, lin_vel_y, ang_vel_z) that the +# original model was trained on. The remaining 13 dimensions are ignored. +# # Example usage: python scripts/reinforcement_learning/rsl_rl/export.py --task=Isaac-Velocity-Rough-Kbot-v0 --checkpoint ~/Github/IsaacLab/logs/rsl_rl/kbot_rough/[path_to_checkpoint].pt # Or you can omit the checkpoint arg and it will use the latest checkpoint in the logs/rsl_rl/agent_name/ directory import argparse @@ -237,7 +242,12 @@ def main(): command_tensor = torch.cat([command_manager.get_command(name) for name in command_term_names], dim=-1) command_tensor = command_tensor.to("cpu").flatten() - NUM_COMMANDS = command_tensor.shape[0] + # Original model expects 3D commands (lin_vel_x, lin_vel_y, ang_vel_z) + # But deployment platform provides 16D commands - we only use first 3 + MODEL_NUM_COMMANDS = command_tensor.shape[0] # This is 3 for the original model + PLATFORM_NUM_COMMANDS = 16 # Platform provides 16D commands + + print(f"[INFO] Model expects {MODEL_NUM_COMMANDS} command dimensions, but deployment platform provides {PLATFORM_NUM_COMMANDS} dimensions.") def construct_obs_rnn( projected_gravity: torch.Tensor, @@ -247,12 +257,18 @@ def construct_obs_rnn( gyroscope: torch.Tensor, carry: torch.Tensor, ) -> torch.Tensor: + # Extract only the first 3 dimensions from the 16D command vector + # [0] x linear velocity [m/s] + # [1] y linear velocity [m/s] + # [2] z angular velocity [rad/s] + model_command = command[:MODEL_NUM_COMMANDS] + offset_joint_angles = joint_angles - _INIT_JOINT_POS scaled_projected_gravity = projected_gravity / 9.81 obs = torch.cat( ( scaled_projected_gravity, - command, + model_command, offset_joint_angles, joint_angular_velocities, gyroscope, @@ -301,7 +317,7 @@ def _init_fn() -> torch.Tensor: torch.zeros(3), torch.zeros(NUM_JOINTS), torch.zeros(NUM_JOINTS), - torch.zeros(NUM_COMMANDS), + torch.zeros(PLATFORM_NUM_COMMANDS), # Use 16D command vector torch.zeros(3), torch.zeros(*CARRY_SHAPE), ) @@ -312,7 +328,7 @@ def _init_fn() -> torch.Tensor: joint_names = list(env.unwrapped.scene["robot"].data.joint_names) metadata = PyModelMetadata( joint_names=joint_names, - num_commands=NUM_COMMANDS, + num_commands=PLATFORM_NUM_COMMANDS, # Use 16D command vector size carry_size=list(CARRY_SHAPE), ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/kscale.py b/source/isaaclab_assets/isaaclab_assets/robots/kscale.py index 101a9436ea5d..0145f94a07e8 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/kscale.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/kscale.py @@ -234,6 +234,6 @@ joint_pos=_INIT_JOINT_POS, joint_vel={".*": 0.0}, ), - soft_joint_pos_limit_factor=0.9, + soft_joint_pos_limit_factor=1.0, actuators=_ACTUATORS, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/.asset_hash b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/.asset_hash index b21927de222b..31f5f828a640 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/.asset_hash +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/.asset_hash @@ -1 +1 @@ -eb61befdd2f61e6071524a9a0e9dae4a \ No newline at end of file +b01cba76b5e9249e61450f34aa6d792e \ No newline at end of file diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/README.md b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/README.md new file mode 100644 index 000000000000..2bf05225cc84 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/README.md @@ -0,0 +1,80 @@ + +# Instructions + +For importing the kbot into Isaaclab + + +## URDF Importer + +Instead of using the GUI option, use the script with command + +```bash +# cd into Isaaclab folder +./isaaclab.sh -p scripts/tools/convert_urdf.py /path/to/robot.urdf /path/to/desired/output/robot.usd +``` + +The reason for this is that you need to NOT merge the joints, because otherwise the imu will get merged into the base and not have a "RigidBodyAPI" which is needed. + +## Update training script + +Note that you will need to change the names of anything referencing the geoms in the train config ,for example in: + +`IsaacLab/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/kbot/rough_rnn_env_cfg.py` + +```python +# Physics material randomization (friction with the floor) +self.events.physics_material = EventTerm( + func=mdp.randomize_rigid_body_material, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg( + "robot", body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"] + ), + "static_friction_range": (0.1, 2.0), + "dynamic_friction_range": (0.1, 2.0), + "restitution_range": (0.0, 0.1), + "num_buckets": 64, + "make_consistent": True, # Ensure dynamic friction is always less than static friction + }, +) + +# Individual link mass randomization for robustness +self.events.add_limb_masses = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg( + "robot", + body_names=[ + "KD_B_102B_TORSO_BTM", + "KD_D_102L_L_Hip_Yoke_Drive", + "KD_C_101L_ShldYokeDrive", + "KD_D_102R", + "KC_C_101R_ShldYokeDrive", + "L_Hip_Roll_RS03", + "L_Hip_Roll_RS03_2", + "RS03_4", + "RS03_3", + "KD_D_301L_L_Femur_Lower_Drive", + "KD_C_301L_LowerBicepDrive", + "KD_D_301R", + "KC_C_202R", + "KD_D_401L_L_Shin_Drive", + "KC_C_401L_Up_Forearm_Drive", + "KD_D_401R", + "KC_C_401R_R_UpForearmDrive", + "LFootBushing_GPF_1517_12", + "PRT0001_2", + "RFootBushing_GPF_1517_12", + "PRT0001", + ], + ), + "mass_distribution_params": (0.8, 1.2), + "operation": "scale", + "distribution": "uniform", + "recompute_inertia": True, + }, +) +``` + + diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/config.yaml b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/config.yaml index c70d54215cca..c8ec5d3a0228 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/config.yaml +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/config.yaml @@ -1,5 +1,5 @@ -asset_path: /home/ali/repos/ksim_stuff_new/make-assets/gen/kbot-headless-full-collisions/robot.urdf -usd_dir: /home/ali/repos/ksim_stuff_new/make-assets/gen/kbot-headless-full-collisions/w_imu +asset_path: /home/ali/repos/klab_stuff/kbot-headless/robot.urdf +usd_dir: /home/ali/repos/klab_stuff/kbot-headless/usd_out usd_file_name: robot.usd force_usd_conversion: true make_instanceable: true @@ -19,5 +19,5 @@ self_collision: false replace_cylinders_with_capsules: false collision_from_visuals: false ## -# Generated by UrdfConverter on 2025-07-22 at 14:22:48. +# Generated by UrdfConverter on 2025-09-09 at 19:00:07. ## diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_base.usd b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_base.usd index f274954789d8..495474ea9d46 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_base.usd +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_base.usd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e38178a639e35fc5751d3311437f1b5ffba0e8313e67d214488008525b661062 -size 14895165 +oid sha256:c88985103f8f2badbdb61bdcf54e8ad9ebd0448f8e24b00dfde583520c70fd1b +size 22732199 diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_physics.usd b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_physics.usd index 9db2bea98323..3df884c8934c 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_physics.usd +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_physics.usd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c625d60f6604ad656149568c27b805286489a93b25d9a3895e264a0340245a09 -size 9098 +oid sha256:b988acfefec72671423b91cb62ed0697579642dd9d11cb2e5c0667849d4e589d +size 9223 diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_sensor.usd b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_sensor.usd index 4762928feadc..27e39e29b605 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_sensor.usd +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/configuration/robot_sensor.usd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:46da21e25ce797f03d8919b343a475708b5ef156b33f4c4db900b8dcb1394d48 -size 657 +oid sha256:1dbd9efea02f69660d4a28a55f60d03353937bfcb768ca95b6d3a587f23606d0 +size 652 diff --git a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/robot.usd b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/robot.usd index b7147ba7f9cd..63efc438122c 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/robot.usd +++ b/source/isaaclab_assets/isaaclab_assets/robots/temp_kbot_usd/robot.usd @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:084bae57718c1cb3cf57bb6e5b4dcfeb332d6c30cfb9dab5c24b229d0a3be583 -size 1612 +oid sha256:7df9514843dcc0dc2002ead3fcde3c7c4d653cf2e98e5dbad0f51143cb88c2ea +size 1601 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/kbot/rough_rnn_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/kbot/rough_rnn_env_cfg.py index f7552774fc93..3387eb8f3a19 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/kbot/rough_rnn_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/kbot/rough_rnn_env_cfg.py @@ -1,6 +1,7 @@ """Rough terrain locomotion environment config for kbot.""" from typing import Dict, Optional, Tuple +import math import torch @@ -31,6 +32,7 @@ RewardsCfg, ) from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.assets import RigidObject, Articulation def action_acceleration_l2(env: ManagerBasedRLEnv) -> torch.Tensor: @@ -125,6 +127,46 @@ def foot_height_reward( return reward + +def standing_lin_vel_l1( + env: ManagerBasedRLEnv, + command_name: str, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + threshold: float = 0.1, +) -> torch.Tensor: + """Penalize base linear velocity when the command is (almost) zero. + + This term encourages the robot to remain stable during *standing* episodes + (i.e. when the commanded linear velocity magnitude is below ``threshold``). + + The returned value is the L1 norm of the xy-components of the base linear + velocity expressed in the robot frame, gated so that it contributes **only** + for standing commands. + """ + + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + + # commanded linear velocity (body frame) + cmd_lin_vel_xy = env.command_manager.get_command(command_name)[:, :2] + cmd_speed = torch.norm(cmd_lin_vel_xy, dim=1) + + # determine which envs are in standing mode + is_standing = cmd_speed < threshold + + # L1 error (command is ~0) – use absolute actual velocity + lin_vel_error = torch.sum(torch.abs(asset.data.root_lin_vel_b[:, :2]), dim=1) + + # only penalize standing envs + return lin_vel_error * is_standing.float() + +def flat_orientation_l1( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """L1 penalty on roll/pitch: sum(|gravity_body_frame.xy|). Zero when upright.""" + asset: RigidObject = env.scene[asset_cfg.name] + return torch.sum(torch.abs(asset.data.projected_gravity_b[:, :2]), dim=1) def randomize_imu_mount( env: ManagerBasedEnv, @@ -281,6 +323,25 @@ def velocity_push_curriculum( "push_velocity_magnitude": current_velocity, } +def command_pos_limits( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """ + Penalise commanded targets from the policy that exceed the limits. + + Works with any PD/implicit/explicit actuator as long as the action is + interpreted as a desired joint position. + """ + asset = env.scene[asset_cfg.name] + processed_actions = asset.data.joint_pos_target[:, asset_cfg.joint_ids] + + low_limits = asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 0] + high_limits = asset.data.soft_joint_pos_limits[:, asset_cfg.joint_ids, 1] + + below = (low_limits - processed_actions).clamp(min=0.0) + above = (processed_actions - high_limits).clamp(min=0.0) + return torch.sum(below + above, dim=1) @configclass class KBotRewards(RewardsCfg): @@ -298,6 +359,8 @@ class KBotRewards(RewardsCfg): weight=1.0, params={"command_name": "base_velocity", "std": 0.5}, ) + + feet_air_time = RewTerm( func=mdp.feet_air_time_positive_biped, @@ -306,7 +369,7 @@ class KBotRewards(RewardsCfg): "command_name": "base_velocity", "sensor_cfg": SceneEntityCfg( "contact_forces", - body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"], + body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"], ), "threshold": 0.4, }, @@ -317,10 +380,10 @@ class KBotRewards(RewardsCfg): params={ "sensor_cfg": SceneEntityCfg( "contact_forces", - body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"], + body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"], ), "asset_cfg": SceneEntityCfg( - "robot", body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"] + "robot", body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"] ), }, ) @@ -329,11 +392,11 @@ class KBotRewards(RewardsCfg): func=foot_height_reward, weight=0.5, params={ - "target_height": 0.2, # 15cm target height for swing phase - "std": 0.05, # Standard deviation for exponential kernel - "tanh_mult": 2.0, # Velocity multiplier for swing detection + "target_height": 0.2, + "std": 0.05, + "tanh_mult": 2.0, "asset_cfg": SceneEntityCfg( - "robot", body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"] + "robot", body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"] ), }, ) @@ -348,7 +411,7 @@ class KBotRewards(RewardsCfg): ) }, ) - + joint_deviation_hip = RewTerm( func=mdp.joint_deviation_l1, weight=-0.5, @@ -399,13 +462,13 @@ class KBotRewards(RewardsCfg): "robot", joint_names=[ # left arm - # "dof_left_shoulder_pitch_03", + "dof_left_shoulder_pitch_03", "dof_left_shoulder_roll_03", "dof_left_shoulder_yaw_02", "dof_left_elbow_02", "dof_left_wrist_00", # right arm - # "dof_right_shoulder_pitch_03", + "dof_right_shoulder_pitch_03", "dof_right_shoulder_roll_03", "dof_right_shoulder_yaw_02", "dof_right_elbow_02", @@ -415,19 +478,6 @@ class KBotRewards(RewardsCfg): }, ) - joint_deviation_shoulder_pitch = RewTerm( - func=mdp.joint_deviation_l1, - weight=-0.4, - params={ - "asset_cfg": SceneEntityCfg( - "robot", - joint_names=[ - "dof_left_shoulder_pitch_03", - "dof_right_shoulder_pitch_03", - ], - ) - }, - ) # Action smoothness penalty action_acceleration_l2 = RewTerm( @@ -435,56 +485,32 @@ class KBotRewards(RewardsCfg): weight=-0.1, ) - # Foot contact force penalty - L2 penalty above threshold - # foot_contact_force_l2 = RewTerm( - # func=contact_forces_l2_penalty, - # weight=-1.0e-7, - # params={ - # "threshold": 360.0, - # "sensor_cfg": SceneEntityCfg( - # "contact_forces", - # body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"], - # ), - # }, - # ) - - # No stomping reward - # Foot-impact regulariser (discourages stomping) - foot_impact_penalty = RewTerm( - func=mdp.contact_forces, - weight=-1.5e-3, + + foot_contact_force_l2 = RewTerm( + func=contact_forces_l2_penalty, + weight=-1.0e-7, params={ - "threshold": 358.0, # Manually checked static load of the kbot while standing + "threshold": 360.0, "sensor_cfg": SceneEntityCfg( "contact_forces", - body_names=[ - "KB_D_501L_L_LEG_FOOT", - "KB_D_501R_R_LEG_FOOT", - "Torso_Side_Right", - "KC_D_102L_L_Hip_Yoke_Drive", - "RS03_5", - "KC_D_301L_L_Femur_Lower_Drive", - "KC_D_401L_L_Shin_Drive", - "KC_C_104L_PitchHardstopDriven", - "RS03_6", - "KC_C_202L", - "KC_C_401L_L_UpForearmDrive", - "KB_C_501X_Left_Bayonet_Adapter_Hard_Stop", - "KC_D_102R_R_Hip_Yoke_Drive", - "RS03_4", - "KC_D_301R_R_Femur_Lower_Drive", - "KC_D_401R_R_Shin_Drive", - "KC_C_104R_PitchHardstopDriven", - "RS03_3", - "KC_C_202R", - "KC_C_401R_R_UpForearmDrive", - "KB_C_501X_Right_Bayonet_Adapter_Hard_Stop", - ], + body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"], ), }, ) + feet_separation_penalty = RewTerm( + func=mdp.body_distance_penalty, + weight=-2.0, + params={ + "min_distance": 0.25, # Manually measured distance between feet + "asset_cfg": SceneEntityCfg("robot"), + "body_a_names": ["LFootBushing_GPF_1517_12"], + "body_b_names": ["RFootBushing_GPF_1517_12"], + }, + ) + + @configclass class KBotObservations: @configclass @@ -503,11 +529,10 @@ class CriticCfg(ObservationGroupCfg): velocity_commands = ObsTerm( func=mdp.generated_commands, params={"command_name": "base_velocity"} ) - # Replaced with privileged observations without noise below - # joint_pos = ObsTerm( - # func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01) - # ) - # joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01) + ) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) actions = ObsTerm(func=mdp.last_action) height_scan = ObsTerm( func=mdp.height_scan, @@ -546,7 +571,7 @@ class CriticCfg(ObservationGroupCfg): scale=0.01, params={ "asset_cfg": SceneEntityCfg( - "robot", body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"] + "robot", body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"] ) }, ) @@ -557,22 +582,12 @@ class CriticCfg(ObservationGroupCfg): params={ "asset_cfg": SceneEntityCfg( "robot", - body_names=["base", "KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"], + body_names=["base", "LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"], ) }, noise=Unoise(n_min=-0.0001, n_max=0.0001), ) - # Joint positions and velocities with less noise (privileged accurate state) - joint_pos_accurate = ObsTerm( - func=mdp.joint_pos_rel, - noise=Unoise(n_min=-0.0001, n_max=0.0001), - ) - joint_vel_accurate = ObsTerm( - func=mdp.joint_vel_rel, - noise=Unoise(n_min=-0.0001, n_max=0.0001), - ) - # Base position (full pose information - privileged) base_pos = ObsTerm( func=mdp.base_pos_z, noise=Unoise(n_min=-0.0001, n_max=0.0001) @@ -596,23 +611,25 @@ class PolicyCfg(ObservationGroupCfg): projected_gravity = ObsTerm( func=mdp.imu_projected_gravity, params={"asset_cfg": SceneEntityCfg("imu")}, - noise=Unoise(n_min=-0.05, n_max=0.05), + noise=Unoise(n_min=-0.1, n_max=0.1), ) velocity_commands = ObsTerm( func=mdp.generated_commands, params={"command_name": "base_velocity"} ) joint_pos = ObsTerm( - func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.05, n_max=0.05) + func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.1, n_max=0.1) ) - joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-0.5, n_max=0.5)) - # IMU observations + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.0, n_max=1.0)) imu_ang_vel = ObsTerm( func=mdp.imu_ang_vel, params={"asset_cfg": SceneEntityCfg("imu")}, - noise=Unoise(n_min=-0.1, n_max=0.1), + noise=Unoise(n_min=-0.2, n_max=0.2), ) + + # Useful for MLP policies (not RNNs) # actions = ObsTerm(func=mdp.last_action) - # No linear acceleration for now + + # (Optional) Linear acceleration from IMU # imu_lin_acc = ObsTerm( # func=mdp.imu_lin_acc, # params={"asset_cfg": SceneEntityCfg("imu")}, @@ -640,8 +657,8 @@ class KBotCurriculumCfg: params={ "min_push": 0.01, "max_push": 0.5, - "curriculum_start_step": 24 * 500, - "curriculum_stop_step": 24 * 5500, + "curriculum_start_step": 12000, + "curriculum_stop_step": 132000, }, ) @@ -657,6 +674,9 @@ def __post_init__(self): # post init of parent super().__post_init__() + # Match Hellman scale and joint soft-limit margin + self.scene.num_envs = 8192 + # Scene self.scene.robot = KBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" @@ -686,13 +706,13 @@ def __post_init__(self): mode="reset", params={ "asset_cfg": SceneEntityCfg( - "robot", body_names=["KB_D_501L_L_LEG_FOOT", "KB_D_501R_R_LEG_FOOT"] + "robot", body_names=["LFootBushing_GPF_1517_12", "RFootBushing_GPF_1517_12"] ), "static_friction_range": (0.1, 2.0), "dynamic_friction_range": (0.1, 2.0), "restitution_range": (0.0, 0.1), "num_buckets": 64, - "make_consistent": True, # Ensure dynamic friction is always less than static friction + "make_consistent": True, # Ensure dynamic friction is always less than static friction }, ) @@ -704,27 +724,27 @@ def __post_init__(self): "asset_cfg": SceneEntityCfg( "robot", body_names=[ - "Torso_Side_Right", - "KC_D_102L_L_Hip_Yoke_Drive", - "KC_C_104L_PitchHardstopDriven", - "KC_D_102R_R_Hip_Yoke_Drive", - "KC_C_104R_PitchHardstopDriven", - "RS03_5", - "RS03_6", + "KD_D_101L_L_Hip_Static", + "KD_D_102L_L_Hip_Yoke_Drive", + "KD_C_101L_ShldYokeDrive", + "KD_D_102R", + "KC_C_101R_ShldYokeDrive", + "L_Hip_Roll_RS03", + "L_Hip_Roll_RS03_2", "RS03_4", "RS03_3", - "KC_D_301L_L_Femur_Lower_Drive", - "KC_C_202L", - "KC_D_301R_R_Femur_Lower_Drive", + "KD_D_301L_L_Femur_Lower_Drive", + "KD_C_301L_LowerBicepDrive", + "KD_D_301R", "KC_C_202R", - "KC_D_401L_L_Shin_Drive", - "KC_C_401L_L_UpForearmDrive", - "KC_D_401R_R_Shin_Drive", + "KD_D_401L_L_Shin_Drive", + "KC_C_401L_Up_Forearm_Drive", + "KD_D_401R", "KC_C_401R_R_UpForearmDrive", - "KB_D_501L_L_LEG_FOOT", - "KB_C_501X_Left_Bayonet_Adapter_Hard_Stop", - "KB_D_501R_R_LEG_FOOT", - "KB_C_501X_Right_Bayonet_Adapter_Hard_Stop", + "LFootBushing_GPF_1517_12", + "PRT0001_2", + "RFootBushing_GPF_1517_12", + "PRT0001", ], ), "mass_distribution_params": (0.8, 1.2), @@ -807,7 +827,7 @@ def __post_init__(self): # I think this is because the "base" is not a rigid body in the robot asset self.events.add_base_mass = None - self.events.base_com.params["asset_cfg"] = SceneEntityCfg("robot", body_names="Torso_Side_Right") + self.events.base_com.params["asset_cfg"] = SceneEntityCfg("robot", body_names="KD_D_101L_L_Hip_Static") # Rewards self.rewards.lin_vel_z_l2.weight = 0.0 @@ -856,24 +876,24 @@ def __post_init__(self): # Terminations self.terminations.base_contact.params["sensor_cfg"].body_names = [ "base", - "KC_D_102L_L_Hip_Yoke_Drive", - "RS03_5", - "KC_D_301L_L_Femur_Lower_Drive", - "KC_D_401L_L_Shin_Drive", - "KC_C_104L_PitchHardstopDriven", - "RS03_6", - "KC_C_202L", - "KC_C_401L_L_UpForearmDrive", - "KB_C_501X_Left_Bayonet_Adapter_Hard_Stop", - "KC_D_102R_R_Hip_Yoke_Drive", + "KD_D_102L_L_Hip_Yoke_Drive", + "L_Hip_Roll_RS03", + "KD_D_301L_L_Femur_Lower_Drive", + "KD_D_401L_L_Shin_Drive", + "KD_C_101L_ShldYokeDrive", + "L_Hip_Roll_RS03_2", + "KD_C_301L_LowerBicepDrive", + "KC_C_401L_Up_Forearm_Drive", + "PRT0001_2", + "KD_D_102R", "RS03_4", - "KC_D_301R_R_Femur_Lower_Drive", - "KC_D_401R_R_Shin_Drive", - "KC_C_104R_PitchHardstopDriven", + "KD_D_301R", + "KD_D_401R", + "KC_C_101R_ShldYokeDrive", "RS03_3", "KC_C_202R", "KC_C_401R_R_UpForearmDrive", - "KB_C_501X_Right_Bayonet_Adapter_Hard_Stop", + "PRT0001", ] # Apply randomization settings based on flag @@ -881,10 +901,7 @@ def __post_init__(self): self._disable_randomization() def _disable_randomization(self): - """Disable all randomization for easy early training. - - Use with command line arg: env.enable_randomization=false - """ + """Disable all randomization for easy early training.""" print("[INFO]: Disabling all domain randomization!\n" * 5, end="") @@ -901,7 +918,7 @@ def _disable_randomization(self): ) self.events.reset_robot_joints.func = mdp.reset_joints_by_scale self.events.reset_base.params = { - "pose_range": {"x": (0.0, 0.0), "y": (0.0, 0.0), "yaw": (0.0, 0.0)}, + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, "velocity_range": { "x": (0.0, 0.0), "y": (0.0, 0.0), @@ -930,6 +947,8 @@ def _disable_randomization(self): act_cfg.min_delay = 0 if hasattr(act_cfg, "max_delay"): act_cfg.max_delay = 0 + + @configclass