Apollo3D/Gym_GPU
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Amend for standing

Browse Source
This commit is contained in:
ChenXi
2026-03-20 03:37:56 -04:00
parent 9cfc127694
commit 2ae7210062
3 changed files with 97 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

127
rl_game/get_up/config/t1_env_cfg.py
View File

@@ -21,53 +21,76 @@ def standing_with_feet_reward(
min_head_height: float,
min_pelvis_height: float,
sensor_cfg: SceneEntityCfg,
force_threshold: float = 30.0
force_threshold: float = 30.0,
max_v_z: float = 0.25
) -> torch.Tensor:
"""
【双高度条件奖励】：只有脚踩地，且头和躯干同时达到高度，才给予高度奖励。
平滑切换的高度奖励：
低高度 -> 纯高度引导
高高度 -> 高度 + 足底力 + 速度约束
"""
# 1. 获取脚部触地力判定
contact_sensor = env.scene.sensors.get(sensor_cfg.name)
foot_forces_z = torch.sum(contact_sensor.data.net_forces_w[:, :, 2], dim=-1)
is_feet_on_ground = foot_forces_z > force_threshold
# 2. 获取头部和躯干索引并提取高度
# 1. 获取基本状态
head_idx, _ = env.scene["robot"].find_bodies("H2")
pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
current_head_h = env.scene["robot"].data.body_state_w[:, head_idx[0], 2]
current_pelvis_h = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
# 3. 计算高度达标度 (0.0 - 1.0)
head_reward = torch.clamp(current_head_h / min_head_height, max=1.2)
pelvis_reward = torch.clamp(current_pelvis_h / min_pelvis_height, max=1.0)
# 2. 计算基础高度得分 (0.0 - 1.0)
head_score = torch.clamp(current_head_h / min_head_height, max=1.0)
pelvis_score = torch.clamp(current_pelvis_h / min_pelvis_height, max=1.0)
combined_height_score = (head_score + pelvis_score) / 2.0
# 综合高度奖励（取平均值）
combined_height_reward = (head_reward + pelvis_reward) / 2.0
# 3. 计算足底力判定
contact_sensor = env.scene.sensors.get(sensor_cfg.name)
foot_forces_z = torch.sum(contact_sensor.data.net_forces_w[:, :, 2], dim=-1)
is_feet_on_ground = (foot_forces_z > force_threshold).float()
# 4. 逻辑门：脚不着地，奖励为 0；脚着地后，根据高度给分
return torch.where(is_feet_on_ground, combined_height_reward, torch.zeros_like(combined_height_reward))
# 4. 计算速度惩罚 (抑制乱跳)
root_vel_z = env.scene["robot"].data.root_lin_vel_w[:, 2]
vel_penalty_factor = torch.exp(-4.0 * torch.clamp(torch.abs(root_vel_z) - max_v_z, min=0.0))
# --- 核心逻辑切换 ---
# 定义一个“过渡高度” (例如盆骨达到 0.4m)
transition_h = 0.4
# 如果高度很低：给纯高度奖，诱导它向上动
low_height_reward = combined_height_score
# 如果高度较高：给 综合奖 (高度 * 速度限制 * 必须踩地)
high_height_reward = combined_height_score * vel_penalty_factor * is_feet_on_ground
return torch.where(current_pelvis_h < transition_h, low_height_reward, high_height_reward)
def arm_push_up_reward(
env: ManagerBasedRLEnv,
sensor_cfg: SceneEntityCfg,
height_threshold: float = 0.6
height_threshold: float = 0.5
) -> torch.Tensor:
"""手臂撑地奖励：辅助机器人从趴/躺状态利用手臂反作用力起身"""
"""
手臂撑地奖励 + 躯干高度线性引导。
引导机器人从完全平躺状态通过手臂受力将躯干撑起。
"""
# 获取传感器受力
contact_sensor = env.scene.sensors.get(sensor_cfg.name)
if contact_sensor is None: return torch.zeros(env.num_envs, device=env.device)
arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
# 当躯干还很低时，鼓励手撑地
# 获取躯干(Pelvis/Trunk)当前高度
pelvis_idx, _ = env.scene["robot"].find_bodies("Trunk")
current_height = env.scene["robot"].data.body_state_w[:, pelvis_idx[0], 2]
pushing_reward = torch.clamp(max_arm_force, max=200.0) / 100.0
return torch.where(current_height < height_threshold, pushing_reward, torch.zeros_like(pushing_reward))
# 1. 手臂受力奖：只有在躯干较低时，鼓励手臂产生向上的反作用力
arm_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
max_arm_force = torch.max(arm_forces_z, dim=-1)[0]
push_reward = torch.clamp(max_arm_force / 50.0, max=1.0) # 归一化受力奖励
# 2. 躯干高度线性引导：只要在 height_threshold 以下，越高分越高
# 这解决了“动不动奖励都一样”的问题
lifting_reward = torch.clamp(current_height / height_threshold, max=1.0)
# 只有在还没站起来（低于阈值）的时候，才给这两个组合奖励
return torch.where(current_height < height_threshold,
push_reward + lifting_reward,
torch.zeros_like(push_reward))
def is_standing_still(
env: ManagerBasedRLEnv,
@@ -134,6 +157,20 @@ def root_vel_z_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> to
vel_z = env.scene[asset_cfg.name].data.root_lin_vel_w[:, 2]
return torch.square(torch.clamp(vel_z, min=0.0))
def joint_pos_rel_l2_local(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor:
# 获取相对默认位置的偏差 (num_envs, num_joints)
rel_pos = mdp.joint_pos_rel(env, asset_cfg)
# 计算平方和 (L2)
return torch.sum(torch.square(rel_pos), dim=-1)
def strict_feet_contact_reward(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor:
"""如果脚不着地，直接给一个很大的负分，强制它必须寻找支点"""
contact_sensor = env.scene.sensors.get(sensor_cfg.name)
# 只要有一只脚没力，就判定为不稳
foot_forces_z = contact_sensor.data.net_forces_w[:, :, 2]
all_feet_cond = torch.min(foot_forces_z, dim=-1)[0] > 5.0 # 左右脚都要有至少5N的力
return (~all_feet_cond).float() # 返回1表示违规
# --- 2. 配置类 ---
@@ -169,8 +206,8 @@ class T1EventCfg:
params={
"asset_cfg": SceneEntityCfg("robot"),
"pose_range": {
"roll": (0, 0),#(-1.57, 1.57),
"pitch": (-1.57, 1.57),#(-1.57, 1.57),
"roll": (-0.2, 0.2),#(-1.57, 1.57),
"pitch": (-1.6, -1.4),#(-1.57, 1.57),
"yaw": (0, 0),#(-3.14, 3.14),
"x": (0.0, 0.0),
"y": (0.0, 0.0),
@@ -231,19 +268,39 @@ class T1GetUpRewardCfg:
# 5. 抑制跳跃：严厉惩罚向上窜的速度
root_vel_z_penalty = RewTerm(
func=root_vel_z_l2_local,
weight=-10.0, # 增大负权重
weight=-50.0, # 增大负权重
params={"asset_cfg": SceneEntityCfg("robot")}
)
# 6. 抑制滞空 (Airtime Penalty)
# 如果脚离开地面，按时间扣分
feet_airtime = RewTerm(
func=feet_airtime_penalty_local,
weight=-10.0,
params={
"sensor_cfg": SceneEntityCfg("feet_contact_sensor"),
"threshold": 0.1, # 超过 0.2s 离地就开始扣分
}
func=strict_feet_contact_reward,
weight=-20.0, # 加大权重，跳一下扣的分比站起来得的分还多
params={"sensor_cfg": SceneEntityCfg("contact_sensor", body_names=[".*_foot_link"])}
)
joint_vel_penalty = RewTerm(
func=mdp.joint_vel_l2,
weight=-0.5, # 惩罚过快的关节运动
params={"asset_cfg": SceneEntityCfg("robot")}
)
action_rate = RewTerm(
func=mdp.action_rate_l2,
weight=-0.5, # 惩罚动作的突变，让动作更丝滑，减少爆发力
)
# 惩罚躯干的翻转和俯仰角速度
base_ang_vel_penalty = RewTerm(
func=lambda env, asset_cfg: torch.norm(mdp.base_ang_vel(env, asset_cfg), dim=-1),
weight=-0.1,
params={"asset_cfg": SceneEntityCfg("robot")}
)
joint_deviation = RewTerm(
func=joint_pos_rel_l2_local,
weight=0.1, # 权重不要太高，只是为了让它动起来
params={"asset_cfg": SceneEntityCfg("robot")}
)
# 7. 成功终极大奖
@@ -259,7 +316,7 @@ class T1GetUpTerminationsCfg:
time_out = DoneTerm(func=mdp.time_out)
# 失败判定：躯干倾斜超过 45 度重置
base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 0.785})
#base_crash = DoneTerm(func=mdp.bad_orientation, params={"limit_angle": 0.785})
# 成功判定：双高度 + 稳定
standing_success = DoneTerm(