diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/Lv0.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/Lv0.py new file mode 100644 index 0000000..d6cdce6 --- /dev/null +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/Lv0.py @@ -0,0 +1,262 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from pathlib import Path + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg +from isaaclab.utils.configclass import configclass +from isaaclab.utils.noise import UniformNoiseCfg + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as velocity_mdp + +from . import mdp + +## +# Pre-defined configs +## + +DEMO_ROOT = Path(__file__).resolve().parents[6] +URDF_PATH = DEMO_ROOT / "bot" / "2foot" / "urdf" / "2foot.urdf" +BOT_PACKAGE_PATH = DEMO_ROOT / "bot" / "2foot" +JOINT_NAMES = [ + "jlf0_link", + "jlf1_link", + "jlf2_link", + "jlf3_link", + "jrf0_link", + "jrf1_link", + "jrf2_link", + "jrf3_link", +] + +TWOFOOTBOT_CFG = ArticulationCfg( + spawn=sim_utils.UrdfFileCfg( + asset_path=str(URDF_PATH), + fix_base=False, + activate_contact_sensors=True, + ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}], + joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg( + drive_type="force", + target_type="position", + gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg(stiffness=17.0, damping=0.3), + ), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_linear_velocity=100.0, + max_angular_velocity=100.0, + max_depenetration_velocity=5.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=8, + solver_velocity_iteration_count=1, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.22), + joint_pos={joint_name: 0.0 for joint_name in JOINT_NAMES}, + joint_vel={".*": 0.0}, + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=JOINT_NAMES, + effort_limit_sim=0.5, + velocity_limit_sim=5.0, + stiffness=5.0, + damping=0.3, + ) + }, +) + + +## +# Scene definition +## + + +@configclass +class TwofootbotdemoSceneCfg(InteractiveSceneCfg): + """Flat-ground scene for the 2foot velocity-tracking task.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(size=(100.0, 100.0)), + ) + + # robot + robot: ArticulationCfg = TWOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + contact_forces = None + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/DomeLight", + spawn=sim_utils.DomeLightCfg(color=(0.9, 0.9, 0.9), intensity=500.0), + ) + + +## +# MDP settings +## + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = velocity_mdp.JointPositionActionCfg( + asset_name="robot", joint_names=JOINT_NAMES, scale=0.5, use_default_offset=True + ) + + +@configclass +class CommandsCfg: + """The arrow points along +X at a fixed walking speed.""" + + base_velocity = velocity_mdp.UniformVelocityCommandCfg( + asset_name="robot", + resampling_time_range=(10.0, 10.0), + rel_standing_envs=0.0, + rel_heading_envs=0.0, + heading_command=False, + debug_vis=True, + ranges=velocity_mdp.UniformVelocityCommandCfg.Ranges( + lin_vel_x=(0.35, 0.35), + lin_vel_y=(0.0, 0.0), + ang_vel_z=(0.0, 0.0), + heading=(-math.pi, math.pi), + ), + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_lin_vel = ObsTerm(func=velocity_mdp.base_lin_vel, noise=UniformNoiseCfg(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=velocity_mdp.base_ang_vel, noise=UniformNoiseCfg(n_min=-0.2, n_max=0.2)) + projected_gravity = ObsTerm(func=velocity_mdp.projected_gravity) + velocity_commands = ObsTerm( + func=velocity_mdp.generated_commands, params={"command_name": "base_velocity"} + ) + joint_pos_rel = ObsTerm(func=velocity_mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("robot")}) + joint_vel_rel = ObsTerm(func=velocity_mdp.joint_vel_rel, params={"asset_cfg": SceneEntityCfg("robot")}) + actions = ObsTerm(func=velocity_mdp.last_action) + + def __post_init__(self) -> None: + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # reset + reset_base = EventTerm( + func=velocity_mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.2, 0.2), "y": (-0.05, 0.05), "yaw": (-0.1, 0.1)}, + "velocity_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.05, 0.05)}, + }, + ) + reset_joints = EventTerm( + func=velocity_mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (0.9, 1.1), + "velocity_range": (0.0, 0.0), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # (1) Constant running reward + # 轨迹线性速度奖励项 + track_lin_vel = RewTerm( # 定义一个奖励项，用于跟踪线性速度 + func=velocity_mdp.track_lin_vel_xy_exp, # 使用velocity_mdp模块中的track_lin_vel_xy_exp函数作为奖励计算方法 + weight=5.0, + params={"command_name": "base_velocity", "std": math.sqrt(0.25)}, + ) + # 创建奖励项，用于控制机器人的各种行为 + upright = RewTerm(func=velocity_mdp.flat_orientation_l2, weight=-1.0) # 控制机器人保持直立姿态，权重为-1.0 + vertical_velocity = RewTerm(func=velocity_mdp.lin_vel_z_l2, weight=-1.0) # 控制机器人垂直方向速度，权重为-1.0 + angular_velocity = RewTerm(func=velocity_mdp.ang_vel_xy_l2, weight=-0.05) # 控制机器人角速度，权重为-0.05 + action_rate = RewTerm(func=velocity_mdp.action_rate_l2, weight=-0.01) # 控制机器人动作变化率，权重为-0.01 + joint_acceleration = RewTerm(func=velocity_mdp.joint_acc_l2, weight=-2.5e-7) # 控制机器人关节加速度，权重为-2.5e-7 + joint_limits = RewTerm(func=velocity_mdp.joint_pos_limits, weight=-1.0) # 确保机器人关节位置在限制范围内，权重为-1.0 + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + # (1) Time out + time_out = DoneTerm(func=mdp.time_out, time_out=True) + # (2) The robot has tipped too far from upright. + fallen_orientation = DoneTerm( + func=mdp.bad_orientation, + params={"limit_angle": 0.7}, + ) + # (3) The base has collapsed onto the ground. + fallen_height = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.08}, + ) + + +## +# Environment configuration +## + + +@configclass +class TwofootbotdemoEnvCfg(ManagerBasedRLEnvCfg): + scene: TwofootbotdemoSceneCfg = TwofootbotdemoSceneCfg(num_envs=1024, env_spacing=1.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + events: EventCfg = EventCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + # Post initialization + def __post_init__(self) -> None: + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # viewer settings + self.viewer.eye = (3.0, 2.5, 1.8) + # simulation settings + self.sim.dt = 1 / 200 + self.sim.render_interval = self.decimation + self.sim.visualizer_cfgs = [] \ No newline at end of file diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/__init__.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/__init__.py index 43a887b..2df1fdc 100644 --- a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/__init__.py +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/__init__.py @@ -12,12 +12,51 @@ from . import agents ## -gym.register( - id="Template-Twofootbot-v0", +'''gym.register( + id="Template-G1-Twofootbot-mlp-v0", entry_point="isaaclab.envs:ManagerBasedRLEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.twofootbot_env_cfg:TwofootbotEnvCfg", + "env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", + }, +) +gym.register( + id="Template-G1-Twofootbot-cnn-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg", + }, +) + +gym.register( + id="Template-G1-Twofootbot-mlp-v0-Play", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.g1_twofootbot_env_cfg:G1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", + }, +) + +gym.register( + id="Template-G1-Twofootbot-cnn-v0-Play", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.g1_twofootbot_cnn_env_cfg:G1RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cnn_cfg:PPORunnerCfg", + }, +)''' + +gym.register( + id="Template-Twofootbotv0-Demo", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.Lv0:TwofootbotdemoEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", }, ) \ No newline at end of file diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cfg.py index 94aab8c..81ade99 100644 --- a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cfg.py +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cfg.py @@ -20,8 +20,9 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 5000 save_interval = 200 experiment_name = "cartpole_direct" - obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]} - actor = RslRlCNNModelCfg( + #obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]} + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + '''actor = RslRlCNNModelCfg( hidden_dims=[512, 256, 256, 128], activation="relu", obs_normalization=False, @@ -32,21 +33,27 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg): stride=[8,4,2], activation="relu", ), + )''' + actor = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128], + activation="relu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), ) critic = RslRlMLPModelCfg( - hidden_dims=[1024, 512, 512, 256, 256, 128], + hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128], activation="relu", - obs_normalization=False, + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, #价值函数损失系数 use_clipped_value_loss=True, #是否使用裁剪值损失 clip_param=0.2, #裁剪参数 - entropy_coef=0.05, #熵系数，探索的奖励 + entropy_coef=0.005, #熵系数，探索的奖励 num_learning_epochs=5, #学习的轮数 num_mini_batches=4, #小批量的数量 - learning_rate=5.0e-3, #学习率 + learning_rate=5.0e-4, #学习率 schedule="adaptive", #学习率调度器类型 gamma=0.99, #折扣因子 lam=0.95, #GAE的lambda参数 diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cnn_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cnn_cfg.py new file mode 100644 index 0000000..145ea1c --- /dev/null +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/agents/rsl_rl_ppo_cnn_cfg.py @@ -0,0 +1,50 @@ +from isaaclab.utils.configclass import configclass + +from isaaclab_rl.rsl_rl import ( + RslRlCNNModelCfg, + RslRlMLPModelCfg, + RslRlOnPolicyRunnerCfg, + RslRlPpoAlgorithmCfg, +) + + + +@configclass +class PPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 16 + max_iterations = 15000 + save_interval = 200 + experiment_name = "cartpole_direct" + obs_groups = {"actor": ["policy", "camera"], "critic": ["policy"]} + actor = RslRlCNNModelCfg( + hidden_dims=[1024, 512, 512, 256, 256, 128], + activation="relu", + obs_normalization=False, + distribution_cfg=RslRlCNNModelCfg.GaussianDistributionCfg(init_std=1.0), + cnn_cfg=RslRlCNNModelCfg.CNNCfg( + output_channels=[16,16,16], + kernel_size=[8, 4, 3], + stride=[8,4,2], + activation="relu", + ), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 512, 256, 256, 128], #[1024, 512, 512, 256, 256, 128], + activation="relu", + obs_normalization=True, + ) + + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, #价值函数损失系数 + use_clipped_value_loss=True, #是否使用裁剪值损失 + clip_param=0.2, #裁剪参数 + entropy_coef=0.005, #熵系数，探索的奖励 + num_learning_epochs=5, #学习的轮数 + num_mini_batches=4, #小批量的数量 + learning_rate=5.0e-4, #学习率 + schedule="adaptive", #学习率调度器类型 + gamma=0.99, #折扣因子 + lam=0.95, #GAE的lambda参数 + desired_kl=0.01, #期望的KL散度 + max_grad_norm=1.0, #梯度裁剪的最大范数 + ) \ No newline at end of file diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_cnn_env_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_cnn_env_cfg.py new file mode 100644 index 0000000..a3ebd56 --- /dev/null +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_cnn_env_cfg.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.configclass import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from TwoFootBot.tasks.manager_based.twofootbot.twofootbot_cnn_env_cfg import ( + RewardsCfg, + TwofootbotcnnEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets import G1_MINIMAL_CFG # isort: skip + + +@configclass +class G1Rewards(RewardsCfg): + + """ + G1机器人的奖励函数配置类，继承自RewardsCfg + 定义了各种奖励项及其权重，用于训练和评估G1机器人的行为 + """ + # 终止惩罚：当机器人终止时给予的惩罚 + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + # 跟踪线性速度奖励：鼓励机器人跟踪期望的xy方向线性速度（在坐标系中） + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + # 跟踪角速度奖励：鼓励机器人跟踪期望的z方向角速度（在世界坐标系中） + # 奖励项1：跟踪世界坐标系下的z轴角速度期望值 + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5} + ) +# 定义一个奖励项，用于评估双足机器人的脚部空中时间 + #feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, # 调用的函数，用于计算双足机器人脚部空中时间 + weight=0.25, # 该奖励项的权重，表示在总奖励中的重要性 + params={ # 函数所需的参数配置 + "command_name": "base_velocity", # 命令名称，可能与基础速度控制相关 + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置，指定监测接触力的脚部刚体 + "threshold": 0.4, # 阈值，用于判断脚部是否接触地面 + }, + ) + # 创建一个奖励项(RewTerm)，用于计算脚部滑动惩罚 + feet_slide = RewTerm( + # 指定使用的函数为mdp.feet_slide + func=mdp.feet_slide, + # 设置权重为-0.1，表示脚部滑动会带来负奖励 + weight=-0.1, + # 配置参数，包括传感器和资产的设置 + params={ + # 配置接触力传感器，监测所有脚踝滚动链接的接触力 + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), + # 配置机器人实体，指定监测所有脚踝滚动链接 + "asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]), + }, + ) + +# 定义一个奖励终端项(Reward Terminal)，用于评估关节位置限制的违反程度 + dof_pos_limits = RewTerm( + # 指定评估函数为mdp.joint_pos_limits，该函数用于计算关节位置限制的违反程度 + func=mdp.joint_pos_limits, + # 设置权重为-1.0，表示当关节位置限制被违反时，会给予负奖励 + weight=-1.0, + #params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])}, + ) +# 定义髋关节偏差奖励项 + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数 + weight=-0.1, # 奖励权重，负值表示惩罚偏差 + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数，指定机器人髋关节的名称模式 + ) + + + +@configclass +class G1RoughEnvCfg(TwofootbotcnnEnvCfg): + """ + G1机器人在粗糙环境中的配置类，继承自LocomotionVelocityRoughEnvCfg + 定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等 + """ + rewards: G1Rewards = G1Rewards() + + def __post_init__(self): + # 调用父类的__post_init__方法 + super().__post_init__() + + + # 所有髋关节 + hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"] + # ========== 膝关节 ========== + knee_joints = ["jlf2_link", "jrf2_link"] + + # ========== 踝关节 ========== + ankle_joints = ["jlf3_link", "jrf3_link"] + + + # 设置基础速度的偏航成功阈值 + self.commands.base_velocity.vel_yaw_success_threshold = 0.8 + # 注释：场景中的机器人配置 - 已被注释掉 + #self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # 注释：场景中高度扫描器的路径配置 - 已被注释掉 + #self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link" + + # 初始化事件相关参数 + self.events.add_base_mass = None + #elf.events.base_com = None + # 设置外部力矩事件参数，指定作用在机器人躯干链接上 + self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link" + # 重置机器人关节的位置范围参数 + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + + + + # 设置各种奖励函数的权重 + self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0 + self.rewards.undesired_contacts = None # 禁用不期望接触的奖励 + self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0 + self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005 + self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7 + # 设置自由度加速度L2惩罚的配置参数 + self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=hip_joints # 只考虑髋关节和膝关节 + ) + + + # 设置自由度扭矩L2惩罚的权重和配置参数 + self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7 + self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节 + ) + + + + # 设置基础速度命令的范围 + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1 + self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0（禁止横向移动） + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1 + + + + # 设置终止条件的配置参数 + self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link" + # 终止条件基于躯干接触 + +@configclass +class G1RoughEnvCfg_PLAY(G1RoughEnvCfg): + def __post_init__(self): + # 调用父类的__post_init__方法 + super().__post_init__() + + + + # 设置场景参数 + self.scene.num_envs = 50 # 环境数量 + self.scene.env_spacing = 2.5 # 环境间距 + self.episode_length_s = 40.0 # 每集长度（秒） + self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制 + # 如果地形生成器存在，则设置其参数 + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数 + self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数 + self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习 + + + + # 设置基础速度命令范围 + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围（固定值1.0） + self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围（固定值0.0） + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围（-1.0到1.0） + self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围（固定值0.0） + # 禁用策略观察值的损坏 + self.observations.policy.enable_corruption = False + # 禁用外部力和推力事件 + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_env_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_env_cfg.py new file mode 100644 index 0000000..e0cf7d3 --- /dev/null +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/g1_twofootbot_env_cfg.py @@ -0,0 +1,186 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.configclass import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from TwoFootBot.tasks.manager_based.twofootbot.velocity_env_cfg import ( + RewardsCfg, + LocomotionVelocityRoughEnvCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets import G1_MINIMAL_CFG # isort: skip + + +@configclass +class G1Rewards(RewardsCfg): + + """ + G1机器人的奖励函数配置类，继承自RewardsCfg + 定义了各种奖励项及其权重，用于训练和评估G1机器人的行为 + """ + # 终止惩罚：当机器人终止时给予的惩罚 + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + # 跟踪线性速度奖励：鼓励机器人跟踪期望的xy方向线性速度（在坐标系中） + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + # 跟踪角速度奖励：鼓励机器人跟踪期望的z方向角速度（在世界坐标系中） + # 奖励项1：跟踪世界坐标系下的z轴角速度期望值 + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=2.0, params={"command_name": "base_velocity", "std": 0.5} + ) +# 定义一个奖励项，用于评估双足机器人的脚部空中时间 + #feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.0) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, # 调用的函数，用于计算双足机器人脚部空中时间 + weight=0.25, # 该奖励项的权重，表示在总奖励中的重要性 + params={ # 函数所需的参数配置 + "command_name": "base_velocity", # 命令名称，可能与基础速度控制相关 + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), # 传感器配置，指定监测接触力的脚部刚体 + "threshold": 0.4, # 阈值，用于判断脚部是否接触地面 + }, + ) + # 创建一个奖励项(RewTerm)，用于计算脚部滑动惩罚 + feet_slide = RewTerm( + # 指定使用的函数为mdp.feet_slide + func=mdp.feet_slide, + # 设置权重为-0.1，表示脚部滑动会带来负奖励 + weight=-0.1, + # 配置参数，包括传感器和资产的设置 + params={ + # 配置接触力传感器，监测所有脚踝滚动链接的接触力 + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), + # 配置机器人实体，指定监测所有脚踝滚动链接 + "asset_cfg": SceneEntityCfg("robot", body_names=["lf3_link", "rf3_link"]), + }, + ) + +# 定义一个奖励终端项(Reward Terminal)，用于评估关节位置限制的违反程度 + dof_pos_limits = RewTerm( + # 指定评估函数为mdp.joint_pos_limits，该函数用于计算关节位置限制的违反程度 + func=mdp.joint_pos_limits, + # 设置权重为-1.0，表示当关节位置限制被违反时，会给予负奖励 + weight=-1.0, + #params={"asset_cfg": SceneEntityCfg("robot", joint_names=[])}, + ) +# 定义髋关节偏差奖励项 + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, # 使用L1范数计算关节偏差的函数 + weight=-0.1, # 奖励权重，负值表示惩罚偏差 + params={"asset_cfg": SceneEntityCfg("robot", joint_names=["jlf0_link", "jlf1_link", "jrf0_link", "jrf1_link"])}, # 配置参数，指定机器人髋关节的名称模式 + ) + + + +@configclass +class G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + """ + G1机器人在粗糙环境中的配置类，继承自LocomotionVelocityRoughEnvCfg + 定义了G1机器人的环境参数、奖励函数、命令范围和终止条件等 + """ + rewards: G1Rewards = G1Rewards() + + def __post_init__(self): + # 调用父类的__post_init__方法 + super().__post_init__() + + + # 所有髋关节 + hip_joints = ["jlf0_link", "jrf0_link", "jlf1_link", "jrf1_link"] + # ========== 膝关节 ========== + knee_joints = ["jlf2_link", "jrf2_link"] + + # ========== 踝关节 ========== + ankle_joints = ["jlf3_link", "jrf3_link"] + + + # 设置基础速度的偏航成功阈值 + self.commands.base_velocity.vel_yaw_success_threshold = 0.8 + # 注释：场景中的机器人配置 - 已被注释掉 + #self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # 注释：场景中高度扫描器的路径配置 - 已被注释掉 + #self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/head_link" + + # 初始化事件相关参数 + self.events.add_base_mass = None + #elf.events.base_com = None + # 设置外部力矩事件参数，指定作用在机器人躯干链接上 + self.events.base_external_force_torque.params["asset_cfg"].body_names = "head_link" + # 重置机器人关节的位置范围参数 + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + + + + # 设置各种奖励函数的权重 + self.rewards.lin_vel_z_l2.weight = -0.1 # z方向线速度L2惩罚的权重设为0 + self.rewards.undesired_contacts = None # 禁用不期望接触的奖励 + self.rewards.flat_orientation_l2.weight = -1.0 # 平坦方向L2惩罚的权重设为-1.0 + self.rewards.action_rate_l2.weight = -0.005 # 动作变化率L2惩罚的权重设为-0.005 + self.rewards.dof_acc_l2.weight = -1.25e-7 # 自由度加速度L2惩罚的权重设为-1.25e-7 + # 设置自由度加速度L2惩罚的配置参数 + self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=hip_joints # 只考虑髋关节和膝关节 + ) + + + # 设置自由度扭矩L2惩罚的权重和配置参数 + self.rewards.dof_torques_l2.weight = -1.5e-7 # 自由度扭矩L2惩罚的权重设为-1.5e-7 + self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=hip_joints + knee_joints + ankle_joints # 考虑髋关节、膝关节和踝关节 + ) + + + + # 设置基础速度命令的范围 + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) # x方向线速度范围设为0到1 + self.commands.base_velocity.ranges.lin_vel_y = (-0.3, 0.3) # y方向线速度范围设为0（禁止横向移动） + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # z方向角速度范围设为-1到1 + + + + # 设置终止条件的配置参数 + self.terminations.base_contact.params["sensor_cfg"].body_names = "head_link" + # 终止条件基于躯干接触 + +@configclass +class G1RoughEnvCfg_PLAY(G1RoughEnvCfg): + def __post_init__(self): + # 调用父类的__post_init__方法 + super().__post_init__() + + + + # 设置场景参数 + self.scene.num_envs = 50 # 环境数量 + self.scene.env_spacing = 2.5 # 环境间距 + self.episode_length_s = 40.0 # 每集长度（秒） + self.scene.terrain.max_init_terrain_level = None # 地形最大初始级别设为无限制 + # 如果地形生成器存在，则设置其参数 + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 # 地形行数 + self.scene.terrain.terrain_generator.num_cols = 5 # 地形列数 + self.scene.terrain.terrain_generator.curriculum = False # 禁用地形课程学习 + + + + # 设置基础速度命令范围 + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) # 前向速度范围（固定值1.0） + self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) # 侧向速度范围（固定值0.0） + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # 角速度范围（-1.0到1.0） + self.commands.base_velocity.ranges.heading = (0.0, 0.0) # 朝向范围（固定值0.0） + # 禁用策略观察值的损坏 + self.observations.policy.enable_corruption = False + # 禁用外部力和推力事件 + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_env_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_cnn_env_cfg.py similarity index 88% rename from source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_env_cfg.py rename to source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_cnn_env_cfg.py index e274b87..7c8078b 100644 --- a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_env_cfg.py +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/twofootbot_cnn_env_cfg.py @@ -236,11 +236,36 @@ JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。 BOT_PACKAGE_PATH = Path(__file__).resolve().parents[6] / "bot/2foot" # 获取机器人 ROS 包目录。 URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。 +def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs): + """Spawn the URDF and enable contact reporting on every rigid link.""" + from pxr import Usd, UsdPhysics + from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf + from isaaclab.sim.utils import safe_set_attribute_on_usd_prim + + spawn_cfg = cfg.replace(activate_contact_sensors=False) + prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs) + for child_prim in Usd.PrimRange(prim): + if not child_prim.HasAPI(UsdPhysics.RigidBodyAPI): + continue + applied_schemas = child_prim.GetAppliedSchemas() + if "PhysxRigidBodyAPI" not in applied_schemas: + child_prim.AddAppliedSchema("PhysxRigidBodyAPI") + if "PhysxContactReportAPI" not in applied_schemas: + child_prim.AddAppliedSchema("PhysxContactReportAPI") + safe_set_attribute_on_usd_prim( + child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False + ) + safe_set_attribute_on_usd_prim( + child_prim, "physxContactReport:threshold", 0.0, camel_case=False + ) + return prim + TOFOOTBOT_CFG = ArticulationCfg( spawn=sim_utils.UrdfFileCfg( + func=spawn_twofootbot_from_urdf, asset_path=str(URDF_PATH), fix_base=False, - activate_contact_sensors=True, + activate_contact_sensors=False, ros_package_paths=[{"name": "2foot", "path": str(BOT_PACKAGE_PATH)}], joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg( drive_type="force", @@ -268,7 +293,8 @@ TOFOOTBOT_CFG = ArticulationCfg( actuators={ # 动力配置。 "legs": ImplicitActuatorCfg( joint_names_expr=JOINT_NAMES, # 控制所有腿的关节。 - effort_limit_sim=0.08, # 力矩限制。单位：牛顿米。 + effort_limit_sim=10, # 力矩限制。单位：牛顿米。 + velocity_limit_sim=8.0554, # 速度限制 (rad/s) stiffness=2.0, # 刚度。单位：牛顿/米。 damping=0.08, # 阻尼。单位：牛顿秒/米。 ) @@ -365,15 +391,15 @@ class MySceneCfg(InteractiveSceneCfg): # robots robot: ArticulationCfg = TOFOOTBOT_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") # sensors - #height_scanner = None - height_scanner = RayCasterCfg( + height_scanner = None + '''height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/head_link", offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[0.40, 0.25]), debug_vis=False, mesh_prim_paths=["/World/ground"], - ) + )''' contact_forces = VelocityEnvContactSensorCfg() # lights @@ -404,7 +430,7 @@ class CommandsCfg: heading_control_stiffness=0.5, debug_vis=True, ranges=mdp.UniformVelocityCommandCfg.Ranges( - lin_vel_x=(-0.5, 0.5), lin_vel_y=(-0.3, 0.3), ang_vel_z=(-0.5, 0.5), heading=(-math.pi, math.pi) + lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi) ), ) @@ -559,23 +585,6 @@ class EventsCfg: ) -def feet_air_time_alt(env): - """替代奖励：基于足端高度估计空中时间，鼓励足部腾空。""" - # 获取左右足端位置（世界坐标） - foot_l_pos = env.scene["robot"].data.body_pos_w[:, env.scene["robot"].body_names.index("lf3_link")] - foot_r_pos = env.scene["robot"].data.body_pos_w[:, env.scene["robot"].body_names.index("rf3_link")] - foot_z = torch.stack([foot_l_pos[:, 2], foot_r_pos[:, 2]], dim=1) # (num_envs, 2) - - # 触地判断（高度低于阈值） - contact_threshold = 0.02 # 可根据机器人尺寸调整 - in_air = foot_z > contact_threshold # True表示在空中 - - # 计算每只脚累计空中步数（可在环境状态中存储历史） - # 这里简化：给予即时奖励：空中高度*系数 - #air_time_reward = torch.sum(foot_z * in_air.float(), dim=1) * 1 # 系数可调 - air_time_reward = torch.sum(in_air.float(), dim=1) * 1 # 每只脚在空中给予奖励 - return air_time_reward - @configclass class RewardsCfg: """Reward terms for the MDP.""" @@ -593,37 +602,25 @@ class RewardsCfg: dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5)#关节力矩平方和 dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7)#关节加速度平方和 action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-1.0e-3)#惩罚相邻动作之间的变化量平方 - alive = RewTerm( #固定时间惩罚 - func=mdp.is_alive, # 该函数通常返回1（若机器人存活） - weight=-0.25, - ) - fall_penalty = RewTerm( #摔倒惩罚 - func=mdp.is_terminated_term, - weight=-100.0, - params={"term_keys": "base_contact"}, - ) - feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.1) # 足端空中时间奖励 + #feet_air_time_alt = RewTerm(func=feet_air_time_alt, weight=0.1) # 足端空中时间奖励 - '''feet_air_time = RewTerm( - func=mdp.feet_air_time, - weight=0.125, - params={ - #"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*3_link"), - "sensor_cfg": SceneEntityCfg("robot", body_names=".*3_link"), - "command_name": "base_velocity", - "threshold": 0.5, + feet_air_time = RewTerm( + func=mdp.feet_air_time, + weight=0.125, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), + "command_name": "base_velocity", + "threshold": 0.5, }, ) +# 定义一个不期望接触的惩罚项，用于优化机器人运动 +# 使用RewTerm类创建，其权重为负值，表示对不期望接触的行为进行惩罚 undesired_contacts = RewTerm( - func=mdp.undesired_contacts, - weight=-1.0, - params={ - #"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*0_link"), - "sensor_cfg": SceneEntityCfg("robot", body_names=".*0_link"), - "threshold": 1.0 - }, - )''' + func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts，用于计算不期望接触的惩罚 + weight=-1.0, # 设置权重为-1.0，表示不期望接触的行为会带来负奖励 + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0}, + ) # -- optional penalties flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=-1.5) #惩罚倾斜角度 @@ -635,10 +632,13 @@ class TerminationsCfg: time_out = DoneTerm(func=mdp.time_out, time_out=True) base_contact = DoneTerm( - func=mdp.illegal_contact, - params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="head_link"), "threshold": 1.0}, - #params={"asset_cfg": SceneEntityCfg("robot", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0}, - ) + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=[ + "head_link", + "lf0_link", "lf1_link", "lf2_link", + "rf0_link", "rf1_link", "rf2_link", + ]), "threshold": 1.0}, + ) #机器人Z轴与世界Z轴夹角超90度则终止 '''upside_down = DoneTerm( func=mdp.upside_down, @@ -649,7 +649,8 @@ class TerminationsCfg: class CurriculumCfg: """Curriculum terms for the MDP.""" - terrain_levels = CurrTerm(func=mdp.terrain_levels_vel) + # The scene uses a flat plane, so there is no terrain generator to update. + terrain_levels = None ## @@ -658,7 +659,7 @@ class CurriculumCfg: @configclass -class TwofootbotEnvCfg(ManagerBasedRLEnvCfg): +class TwofootbotcnnEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the locomotion velocity-tracking environment.""" # Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs diff --git a/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/velocity_env_cfg.py b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/velocity_env_cfg.py new file mode 100644 index 0000000..f58dab5 --- /dev/null +++ b/source/TwoFootBot/TwoFootBot/tasks/manager_based/twofootbot/velocity_env_cfg.py @@ -0,0 +1,578 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING +from pathlib import Path +import torch +from isaaclab.sensors import ContactSensorCfg +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg +from isaaclab_newton.sensors import ContactSensorCfg as NewtonContactSensorCfg +from isaaclab_ovphysx.sensors import ContactSensorCfg as OvPhysXContactSensorCfg +from isaaclab_physx.physics import PhysxCfg +from isaaclab_physx.sensors import ContactSensorCfg as PhysXContactSensorCfg + +from isaaclab.actuators import ImplicitActuatorCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import RayCasterCfg, patterns +from isaaclab.sim import SimulationCfg +from isaaclab.terrains import TerrainImporterCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.configclass import configclass +from isaaclab.utils.noise import UniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +#from isaaclab.managers import mdp as base_mdp +from isaaclab_tasks.utils import PresetCfg, preset + +from isaaclab.sensors import CameraCfg +from isaaclab.sensors import TiledCameraCfg +from isaaclab.sim import PinholeCameraCfg + + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg + +from isaaclab.managers import ( + RewardTermCfg, + TerminationTermCfg, + EventTermCfg as EventTerm, + ObservationGroupCfg as ObsGroup, + ObservationTermCfg as ObsTerm, + CurriculumTermCfg as CurriculumTerm, + SceneEntityCfg, +) + + +JOINT_NAMES = [ # 定义机器人需要控制的八个关节名称。 + "jlf0_link", + "jlf1_link", + "jlf2_link", + "jlf3_link", + "jrf0_link", + "jrf1_link", + "jrf2_link", + "jrf3_link", +] +BOT_PACKAGE_PATH = Path(__file__).resolve().parents[6] / "bot/2foot" # 获取机器人 ROS 包目录。 +URDF_PATH = BOT_PACKAGE_PATH / "urdf/2foot.urdf" # 指定正式使用的 2foot URDF 文件。 + + +URDF_PATH = "/home/zhenai/AI/RL/bot/2foot/urdf/2foot.urdf" +BOT_PACKAGE_PATH = "/home/zhenai/AI/RL/bot/2foot" # ⚠️ 确保这个目录下有 meshes/ 文件夹 + + +def spawn_twofootbot_from_urdf(prim_path, cfg, translation=None, orientation=None, **kwargs): + """Spawn the URDF and enable contact reporting on every rigid link.""" + from pxr import Usd, UsdPhysics + from isaaclab.sim.spawners.from_files.from_files import spawn_from_urdf + from isaaclab.sim.utils import safe_set_attribute_on_usd_prim + + spawn_cfg = cfg.replace(activate_contact_sensors=False) + prim = spawn_from_urdf(prim_path, spawn_cfg, translation, orientation, **kwargs) + for child_prim in Usd.PrimRange(prim): + if child_prim.HasAPI(UsdPhysics.RigidBodyAPI): + applied_schemas = child_prim.GetAppliedSchemas() + if "PhysxRigidBodyAPI" not in applied_schemas: + child_prim.AddAppliedSchema("PhysxRigidBodyAPI") + if "PhysxContactReportAPI" not in applied_schemas: + child_prim.AddAppliedSchema("PhysxContactReportAPI") + safe_set_attribute_on_usd_prim( + child_prim, "physxRigidBody:sleepThreshold", 0.0, camel_case=False + ) + safe_set_attribute_on_usd_prim( + child_prim, "physxContactReport:threshold", 0.0, camel_case=False + ) + return prim + + +# 双足机器人URDF配置对象 +TWOFOOTBOT_URDF_CFG = ArticulationCfg( + # 配置双足机器人从URDF文件生成 + spawn=sim_utils.UrdfFileCfg( + # 指定用于生成机器人的函数 + func=spawn_twofootbot_from_urdf, + asset_path=URDF_PATH, +# 设置参数：是否固定基座 + fix_base=False, +# 设置参数：是否激活接触传感器 + activate_contact_sensors=False, + # ROS包路径配置 + ros_package_paths=[{"name": "2foot", "path": BOT_PACKAGE_PATH}], + # 关节驱动配置 +# 创建一个关节驱动配置，使用URDF文件配置中的JointDriveCfg类 + joint_drive=sim_utils.UrdfFileCfg.JointDriveCfg( + # 设置驱动类型为"force"，表示通过力来控制关节 + drive_type="force", + # 设置目标类型为"position"，表示目标是控制关节位置 + target_type="position", + # 设置控制增益参数，使用PD控制器配置 + gains=sim_utils.UrdfFileCfg.JointDriveCfg.PDGainsCfg( + stiffness=17, + damping=0 + ), + ), + # 刚体属性配置 + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + max_linear_velocity=100.0, + max_angular_velocity=100.0, + max_depenetration_velocity=5.0, + ), + # 关节属性配置 + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + ), + ), + # 初始状态配置 + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.22), + joint_pos={ + "jlf0_link": 0.0, "jlf1_link": 0.0, "jlf2_link": 0.0, "jlf3_link": 0.0, + "jrf0_link": 0.0, "jrf1_link": 0.0, "jrf2_link": 0.0, "jrf3_link": 0.0, + }, + joint_vel={".*": 0.0}, + ), + # 执行器配置 + actuators={ + "legs": ImplicitActuatorCfg( + # 关节名称表达式列表 + joint_names_expr=[ + "jlf0_link", "jlf1_link", "jlf2_link", "jlf3_link", + "jrf0_link", "jrf1_link", "jrf2_link", "jrf3_link", + ], + # 仿真中的力限制 + effort_limit_sim=0.5, + # 仿真中的速度限制 + velocity_limit_sim=5.0, # 速度限制 (rad/s) + # 刚度系数 + stiffness=5, + # 阻尼系数 + damping=0.3, + ) + }, +) + + +## +# Physics presets +## + + +@configclass +class RoughPhysicsCfg(PresetCfg): + """Shared physics preset for all rough-terrain locomotion envs.""" + + default = PhysxCfg(gpu_max_rigid_patch_count=10 * 2**15) + newton_mjwarp = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + njmax=200, + nconmax=100, + cone="pyramidal", + impratio=1.0, + integrator="implicitfast", + use_mujoco_contacts=False, + ), + collision_cfg=NewtonCollisionPipelineCfg(max_triangle_pairs=2_500_000), + num_substeps=1, + debug_mode=False, + # 1 cm shape margin is the single most important Newton setting for rough + # terrain — without it, non-Anymal-D robots fail to learn stable contact + # on triangle-mesh terrain. See isaaclab_newton 0.5.22 changelog. + default_shape_cfg=NewtonShapeCfg(margin=0.01), + ) + physx = default + + +## +# Scene definition +## + + +@configclass +class VelocityEnvContactSensorCfg(PresetCfg): + default = PhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) + newton_mjwarp = NewtonContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) + physx = default + ovphysx = OvPhysXContactSensorCfg(prim_path="{ENV_REGEX_NS}/Robot/.*", history_length=3, track_air_time=True) + + +@configclass +class MySceneCfg(InteractiveSceneCfg): + """Configuration for the terrain scene with a legged robot.""" + + # ground terrain + terrain = TerrainImporterCfg( + prim_path="/World/ground", + terrain_type="plane", + collision_group=-1, + physics_material=sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ), + visual_material=sim_utils.MdlFileCfg( + mdl_path=f"{ISAACLAB_NUCLEUS_DIR}/Materials/TilesMarbleSpiderWhiteBrickBondHoned/TilesMarbleSpiderWhiteBrickBondHoned.mdl", + project_uvw=True, + texture_scale=(0.25, 0.25), + ), + debug_vis=False, + ) + # robots + robot: ArticulationCfg = TWOFOOTBOT_URDF_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + # sensors + height_scanner = None + '''height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/head_link", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + ray_alignment="yaw", + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=False, + mesh_prim_paths=["/World/ground"], + )''' + contact_forces = VelocityEnvContactSensorCfg() + # lights + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command specifications for the MDP.""" + + base_velocity = mdp.UniformVelocityCommandCfg( + asset_name="robot", + resampling_time_range=(10.0, 10.0), + rel_standing_envs=0.02, + rel_heading_envs=1.0, + heading_command=True, + heading_control_stiffness=0.5, + debug_vis=True, + ranges=mdp.UniformVelocityCommandCfg.Ranges( + lin_vel_x=(-1.0, 1.0), lin_vel_y=(-1.0, 1.0), ang_vel_z=(-1.0, 1.0), heading=(-math.pi, math.pi) + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + joint_pos = mdp.JointPositionActionCfg(asset_name="robot", joint_names=[".*"], scale=0.5, use_default_offset=True) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2)) + projected_gravity = ObsTerm( + func=mdp.projected_gravity, + noise=Unoise(n_min=-0.05, n_max=0.05), + ) + velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) + joint_pos = ObsTerm( + # 指定计算函数，用于获取关节相对位置 + func=mdp.joint_pos_rel, + # 设置噪声参数，使用均匀分布噪声，最小值为-1e-3，最大值为1e-3 + noise=Unoise(n_min=-1e-3, n_max=1e-3), + # 配置观测参数 + params={ + # 指定观测的实体为机器人，并配置关节名称和顺序 + "asset_cfg": SceneEntityCfg( + "robot", # 实体名称为"robot" + joint_names=JOINT_NAMES, # 指定要观测的关节名称列表 + preserve_order=True, # 保持关节顺序不变 + ) + }, + # 设置历史记录长度为5，即保留最近5个时间步的观测数据 + history_length=5, + ) + joint_vel = ObsTerm( + func=mdp.joint_vel, + noise=Unoise(n_min=-0.04, n_max=0.04), + scale=0.05, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=JOINT_NAMES, + preserve_order=True, + ) + }, + history_length=5, + ) + actions = ObsTerm(func=mdp.last_action) + velocity_commands = ObsTerm( + func=mdp.generated_commands, # 指定调用的函数，即MDP中的generated_commands方法 + params={"command_name": "base_velocity"}, # 传递给函数的参数，指定命令名称为"base_velocity" + scale=5.0, # 缩放因子，用于调整输出值的大小 + ) + '''height_scan = ObsTerm( + func=mdp.height_scan, + params={"sensor_cfg": SceneEntityCfg("height_scanner")}, + noise=Unoise(n_min=-0.1, n_max=0.1), + clip=(-1.0, 1.0), + )''' + + def __post_init__(self): + self.enable_corruption = True + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventsCfg: + """Configuration for events.""" + + # startup + # 创建一个物理材质事件项 + physics_material = EventTerm( + # 指定函数为随机化刚体材质的函数 + func=mdp.randomize_rigid_body_material, + # 设置事件模式为启动时执行 + mode="startup", + # 设置事件参数 + params={ + # 配置场景实体，指定为机器人，并匹配所有身体部位 + "asset_cfg": SceneEntityCfg("robot", body_names=".*"), + # 设置静态摩擦力的范围为固定值0.8 + "static_friction_range": (0.8, 0.8), + # 设置动态摩擦力的范围为固定值0.6 + "dynamic_friction_range": (0.6, 0.6), + # 设置恢复系数的范围为固定值0.0（完全非弹性碰撞） + "restitution_range": (0.0, 0.0), + # 设置材质分桶数量为64 + "num_buckets": 64, + }, + ) + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="head_link"), + # Multiplicative ±25% log-uniform. Scale-invariant across robot sizes + # (no per-robot kg overrides needed) with geometric mean 1.0 and + # symmetric inverse perturbation (acceleration symmetric around nominal). + "mass_distribution_params": (1 / 1.25, 1.25), + "operation": "scale", + "distribution": "log_uniform", + }, + ) + + # 预设基座质心随机化配置 + base_com = preset( # 使用preset函数创建预设配置 + default=EventTerm( # 默认事件项配置 + func=mdp.randomize_rigid_body_com, # 执行的函数：随机化刚体质心 + mode="startup", # 事件模式：启动时执行 + params={ # 函数参数配置 + "asset_cfg": SceneEntityCfg("robot", body_names="head_link"), # 场景实体配置：机器人基座 + "com_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (-0.01, 0.01)}, + }, + ), + newton_mjwarp=None, + ) + + # reset + # 创建一个外部力矩事件项，用于在重置时对机器人基座施加外部力和力矩 + base_external_force_torque = EventTerm( + # 指定事件处理函数为mdp.apply_external_force_torque + func=mdp.apply_external_force_torque, + # 设置事件模式为"reset"，表示在环境重置时触发 + mode="reset", + # 设置事件参数 + params={ + # 指定目标实体为机器人，并指定施加力的部位为基座 + "asset_cfg": SceneEntityCfg("robot", body_names="head_link"), + # 设置力的范围为(0.0, 0.0)，表示不施加任何力 + "force_range": (0.0, 0.0), + # 设置力矩的范围为(-0.0, 0.0)，表示不施加任何力矩 + "torque_range": (-0.0, 0.0), + }, + ) + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (-0.5, 0.5), + "y": (-0.5, 0.5), + "z": (-0.5, 0.5), + "roll": (-0.5, 0.5), + "pitch": (-0.5, 0.5), + "yaw": (-0.5, 0.5), + }, + }, + ) + +# 定义一个重置机器人关节的事件项 + reset_robot_joints = EventTerm( + # 指定事件处理函数为mdp.reset_joints_by_scale + func=mdp.reset_joints_by_scale, + # 设置事件模式为"reset" + mode="reset", + # 设置事件参数 + params={ + # 设置位置范围为(0.5, 1.5) + "position_range": (0.5, 1.5), + # 设置速度范围为(0.0, 0.0) + "velocity_range": (0.0, 0.0), + }, + ) + + # interval +# 创建一个名为push_robot的EventTerm对象，用于执行机器人推动操作 + push_robot = EventTerm( + func=mdp.push_by_setting_velocity, # 指定执行函数为mdp中的push_by_setting_velocity + mode="interval", # 设置执行模式为间隔模式 + interval_range_s=(10.0, 15.0), # 设置执行间隔时间范围为10.0到15.0秒 + params={"velocity_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5)}}, # 设置推动速度范围，x和y方向的速度范围均为-0.5到0.5 + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # -- task + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_exp, weight=1.0, params={"command_name": "base_velocity", "std": math.sqrt(0.25)} + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_exp, weight=0.5, params={"command_name": "base_velocity", "std": math.sqrt(0.25)} + ) + # -- penalties + lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=-2.0) + ang_vel_xy_l2 = RewTerm(func=mdp.ang_vel_xy_l2, weight=-0.05) + dof_torques_l2 = RewTerm(func=mdp.joint_torques_l2, weight=-1.0e-5) + dof_acc_l2 = RewTerm(func=mdp.joint_acc_l2, weight=-2.5e-7) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.01) + feet_air_time = RewTerm( + func=mdp.feet_air_time, + weight=0.125, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=["lf3_link", "rf3_link"]), + "command_name": "base_velocity", + "threshold": 0.5, + }, + ) +# 定义一个不期望接触的惩罚项，用于优化机器人运动 +# 使用RewTerm类创建，其权重为负值，表示对不期望接触的行为进行惩罚 + undesired_contacts = RewTerm( + func=mdp.undesired_contacts, # 指定使用的函数mdp.undesired_contacts，用于计算不期望接触的惩罚 + weight=-1.0, # 设置权重为-1.0，表示不期望接触的行为会带来负奖励 + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=["head_link", "lf0_link", "rf0_link"]), "threshold": 1.0}, + ) + # -- optional penalties + flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=0.0) + dof_pos_limits = RewTerm(func=mdp.joint_pos_limits, weight=0.0) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + base_contact = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", + body_names=[ + "head_link", + "lf0_link", "lf1_link", "lf2_link", + "rf0_link", "rf1_link", "rf2_link", + ]), "threshold": 1.0}, + ) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + # The scene uses a flat plane, so there is no terrain generator to update. + terrain_levels = None + + +## +# Environment configuration base +## + + +@configclass +class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the locomotion velocity-tracking environment.""" + + # Simulation settings — shared physics preset (PhysX + MJWarp) for all rough-terrain envs + sim: SimulationCfg = SimulationCfg(physics=RoughPhysicsCfg()) + # Scene settings + scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=0.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventsCfg = EventsCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 0.005 + self.sim.render_interval = self.decimation + self.sim.physics_material = self.scene.terrain.physics_material + # update sensor update periods + # we tick all the sensors based on the smallest update period (physics update period) + if self.scene.height_scanner is not None: + self.scene.height_scanner.update_period = self.decimation * self.sim.dt + if self.scene.contact_forces is not None: + self.scene.contact_forces.update_period = self.sim.dt + + # check if terrain levels curriculum is enabled - if so, enable curriculum for terrain generator + # this generates terrains with increasing difficulty and is useful for training + if getattr(self.curriculum, "terrain_levels", None) is not None: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = True + else: + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.curriculum = False